diff --git a/src/org/usfirst/frc4388/robot/OI.java b/src/org/usfirst/frc4388/robot/OI.java index 7c8cb19..c518792 100644 --- a/src/org/usfirst/frc4388/robot/OI.java +++ b/src/org/usfirst/frc4388/robot/OI.java @@ -9,6 +9,9 @@ import buttons.XBoxTriggerButton; import org.usfirst.frc4388.controller.IHandController; import org.usfirst.frc4388.controller.XboxController; import org.usfirst.frc4388.robot.commands.*; +import org.usfirst.frc4388.robot.commands.auton.CrossTheBaseLine; +import org.usfirst.frc4388.robot.commands.auton.RightSwitchAuton; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; @@ -190,6 +193,7 @@ public class OI SmartDashboard.putData("move elevator", new ElevatorBasic(20)); + SmartDashboard.putData("test", new RightSwitchAuton()); ///SmartDashboard.putData("Move Down", new ElevatorSetSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED)); ///SmartDashboard.putData("Move UP", new ElevatorSetSpeed(ElevatorAuton.STOP_ELEVATOR_SPEED)); diff --git a/src/org/usfirst/frc4388/robot/Robot.java b/src/org/usfirst/frc4388/robot/Robot.java index a7b1e62..0204b23 100644 --- a/src/org/usfirst/frc4388/robot/Robot.java +++ b/src/org/usfirst/frc4388/robot/Robot.java @@ -123,16 +123,16 @@ public class Robot extends IterativeRobot RRautonTaskChooser.addDefault("Choose RR Program", new CrossTheBaseLine()); - RRautonTaskChooser.addObject("1", new CrossTheBaseLine()); + RRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); - 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()); RRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); + RRautonTaskChooser.addObject("Scale3", new ScaleFrom3()); SmartDashboard.putData("RRAuton Task", RRautonTaskChooser); @@ -145,14 +145,14 @@ public class Robot extends IterativeRobot RLautonTaskChooser.addDefault("Choose RL Program", new CrossTheBaseLine()); - RLautonTaskChooser.addObject("1", new CrossTheBaseLine()); + RLautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); - 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()); RLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); @@ -168,14 +168,14 @@ public class Robot extends IterativeRobot LLautonTaskChooser.addDefault("Choose LL Program", new CrossTheBaseLine()); - LLautonTaskChooser.addObject("1", new CrossTheBaseLine()); + LLautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); - 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()); LLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); @@ -189,15 +189,13 @@ public class Robot extends IterativeRobot LRautonTaskChooser = new SendableChooser(); LRautonTaskChooser.addDefault("Choose LR Program", new CrossTheBaseLine()); - - LRautonTaskChooser.addObject("1", new CrossTheBaseLine()); + LRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); - LRautonTaskChooser.addObject("2", new CrossTheBaseLine()); + 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()); LRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); @@ -251,28 +249,31 @@ public class Robot extends IterativeRobot String gameData; gameData = DriverStation.getInstance().getGameSpecificMessage(); - if(gameData.length() > 0) - { - if(gameData.charAt(0) == 'L') - { - if(gameData.charAt(1) == 'L') - { - if (LLautonomousCommand != null) LLautonomousCommand.start(); - } else { - if (LRautonomousCommand != null) LRautonomousCommand.start(); - } - } - } else { - if(gameData.charAt(1) == 'L') - { - if (RLautonomousCommand != null) RLautonomousCommand.start(); - } else { - if (RRautonomousCommand != null) RRautonomousCommand.start(); - } - } - - } - + if (gameData.length() > 0) { + if (gameData.charAt(0) == 'L') { + if (gameData.charAt(1) == 'L') { + if (LLautonomousCommand != null) { + LLautonomousCommand.start(); + } + } else { + if (LRautonomousCommand != null) { + LRautonomousCommand.start(); + } + } + } else { + if (gameData.charAt(1) == 'L') { + if (RLautonomousCommand != null) { + RLautonomousCommand.start(); + } + } else { + if (RRautonomousCommand != null) { + RRautonomousCommand.start(); + } + } + } + } + } + public void autonomousPeriodic() { Scheduler.getInstance().run(); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java new file mode 100644 index 0000000..f1042f9 --- /dev/null +++ b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java @@ -0,0 +1,43 @@ +package org.usfirst.frc4388.robot.commands.auton; + +import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP; +import org.usfirst.frc4388.robot.commands.DriveGyroReset; +import org.usfirst.frc4388.robot.commands.DriveSpeedShift; +import org.usfirst.frc4388.robot.commands.DriveStraightBasic; +import org.usfirst.frc4388.robot.commands.DriveStraightMP; +import org.usfirst.frc4388.robot.commands.DriveTurnBasic; +import org.usfirst.frc4388.robot.commands.ElevatorBasic; +import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed; +import org.usfirst.frc4388.robot.commands.IntakePosition; +import org.usfirst.frc4388.robot.commands.IntakeSetSpeed; +import org.usfirst.frc4388.robot.subsystems.Carriage; +import org.usfirst.frc4388.robot.subsystems.Drive; +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +/** + * + */ +public class ScaleFrom3 extends CommandGroup { + + public ScaleFrom3() { + addSequential(new DriveGyroReset()); + addSequential(new DriveSpeedShift(true)); + addSequential(new IntakePosition(true)); + addSequential(new DriveStraightBasic(-200, 60, true, true, 0)); + addSequential(new ElevatorBasic(10)); + addSequential(new DriveTurnBasic(true, 150, 300, MPSoftwareTurnType.TANK)); + addSequential(new ElevatorBasic(70)); + addSequential(new DriveStraightBasic(10, 20, true, true, 0)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); + //addSequential(new WaitCommand(.1)); + addSequential(new IntakePosition(false)); + addSequential(new WaitCommand(.5)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + + //addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville + + } +} \ No newline at end of file