diff --git a/src/org/usfirst/frc4388/robot/Robot.java b/src/org/usfirst/frc4388/robot/Robot.java index 12980c0..2d363a5 100644 --- a/src/org/usfirst/frc4388/robot/Robot.java +++ b/src/org/usfirst/frc4388/robot/Robot.java @@ -128,6 +128,7 @@ public class Robot extends IterativeRobot RRautonTaskChooser.addObject("2", new CrossTheBaseLine()); RRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); + RRautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); RRautonTaskChooser.addObject("3", new CrossTheBaseLine()); RRautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); @@ -149,6 +150,7 @@ public class Robot extends IterativeRobot RLautonTaskChooser.addObject("2", new CrossTheBaseLine()); RLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); + RLautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); RLautonTaskChooser.addObject("3", new CrossTheBaseLine()); RLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); @@ -171,6 +173,7 @@ public class Robot extends IterativeRobot LLautonTaskChooser.addObject("2", new CrossTheBaseLine()); LLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); + LLautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); LLautonTaskChooser.addObject("3", new CrossTheBaseLine()); LLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); @@ -191,7 +194,8 @@ public class Robot extends IterativeRobot LRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); LRautonTaskChooser.addObject("2", new CrossTheBaseLine()); - LRautonTaskChooser.addObject("Center Left Switch", new CenterLeft()); + LRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); + LRautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); LRautonTaskChooser.addObject("3", new CrossTheBaseLine()); LRautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java index 957bc7e..5d8bc4b 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java @@ -29,10 +29,10 @@ public class CenterLeft extends CommandGroup { addSequential(new IntakePosition(true)); addSequential(new DriveStraightBasic(-15, 60, true, true, 0)); addSequential(new DriveTurnBasic(true, 130, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(65, 60, true, true, 0)); - addSequential(new DriveTurnBasic(true, 28, 300, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(53, 60, true, true, 0)); addSequential(new ElevatorBasic(20)); - addSequential(new DriveStraightBasic(30, 60, true, true, 0)); + addSequential(new DriveTurnBasic(true, 34.5, 300, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(19, 60, true, true, 0)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java index 28166ad..a31e2c9 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java @@ -20,8 +20,8 @@ import edu.wpi.first.wpilibj.command.WaitCommand; /** * */ -public class CenterRight extends CommandGroup -{ +public class CenterRight extends CommandGroup { + public CenterRight() { addSequential(new DriveGyroReset());