package org.usfirst.frc.team3501.robot.commandgroups;
+import org.usfirst.frc.team3501.robot.Constants.Direction;
import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
*
*/
public class BaselineWallAlign extends CommandGroup {
+ private static final String TEAM = "RED"; // set to either RED or BLUE team
public BaselineWallAlign() {
- addSequential(new DriveDistance(0, 0));
- addSequential(new TurnForAngle(0, LEFT, 0));
- addSequential(new DriveDistance(0, 0));
- addSequential(new TurnForAngle(0, LEFT, 0));
+ addSequential(new DriveDistance(10, -0));
+
+ if (TEAM.equals("RED")) {
+ addSequential(new TurnForAngle(115.91, Direction.RIGHT, 0));
+ addSequential(new DriveDistance(126.35, 0));
+ addSequential(new TurnForAngle(90, Direction.LEFT, 0));
+ } else {
+ addSequential(new TurnForAngle(115.91, Direction.LEFT, 0));
+ addSequential(new DriveDistance(117.15, 0));
+ addSequential(new TurnForAngle(90, Direction.RIGHT, 0));
+ }
addSequential(new PrepareToShoot());
}