diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/Robot.java b/src/main/java/com/frcteam1939/powerup2018/robot/Robot.java index c2d57ce..f023312 100644 --- a/src/main/java/com/frcteam1939/powerup2018/robot/Robot.java +++ b/src/main/java/com/frcteam1939/powerup2018/robot/Robot.java @@ -149,20 +149,55 @@ public void autonomousInit() { Thread.sleep(5); } catch (InterruptedException ie) {} gameData = DriverStation.getInstance().getGameSpecificMessage(); - } + // DEFAULT // if (gameData.length() > 0) { // this.autonomousCommand = this.getAutonomousCommand(gameData); // } - if (gameData.charAt(1) == 'L') { - this.autonomousCommand = new LeftWallToLeftScale(); - } else if (gameData.charAt(0) == 'L') { - this.autonomousCommand = new LeftWallToLeftSwitch(); - } else { - this.autonomousCommand = new CrossAutoLine(); - } + // CENTER + // if (gameData.charAt(0) == 'L') { + // this.autonomousCommand = new CenterWallToLeftSwitch(); + // } else if (gameData.charAt(0) == 'R') { + // this.autonomousCommand = new CenterWallToRightSwitch(); + // } + + // CHOOSE BETWEEN LEFT SIDE - PRIORITIZE SWITCH + // if (gameData.charAt(0) == 'L') { + // this.autonomousCommand = new LeftWallToLeftSwitch(); + // } else if (gameData.charAt(1) == 'L') { + // this.autonomousCommand = new LeftWallToLeftScale(); + // } else { + // this.autonomousCommand = new CrossAutoLine(); + // } + + // CHOOSE BETWEEN RIGHT SIDE - PRIORITIZE SWITCH + // if (gameData.charAt(0) == 'R') { + // this.autonomousCommand = new RightWallToRightSwitch(); + // } else if (gameData.charAt(1) == 'R') { + // this.autonomousCommand = new RightWallToRightScale(); + // } else { + // this.autonomousCommand = new CrossAutoLine(); + // } + + // CHOOSE BETWEEN LEFT SIDE - PRIORITIZE SCALE + // if (gameData.charAt(1) == 'L') { + // this.autonomousCommand = new LeftWallToLeftScale(); + // } else if (gameData.charAt(0) == 'L') { + // this.autonomousCommand = new LeftWallToLeftSwitch(); + // } else { + // this.autonomousCommand = new CrossAutoLine(); + // } + + // CHOOSE BETWEEN RIGHT SIDE - PRIORITIZE SCALE + // if (gameData.charAt(1) == 'R') { + // this.autonomousCommand = new RightWallToRightScale(); + // } else if (gameData.charAt(0) == 'R') { + // this.autonomousCommand = new RightWallToRightSwitch(); + // } else { + // this.autonomousCommand = new CrossAutoLine(); + // } SmartDashboard.putString("Autonomous Command", this.autonomousCommand.getName()); @@ -211,7 +246,6 @@ public void teleopPeriodic() { @Override public void testPeriodic() { LiveWindow.run(); - // this.vision.testPixy1(); } public static double getPressure() { diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/commands/auton/CenterWallToLeftSwitch.java b/src/main/java/com/frcteam1939/powerup2018/robot/commands/auton/CenterWallToLeftSwitch.java index 7b77c04..88c4aa1 100644 --- a/src/main/java/com/frcteam1939/powerup2018/robot/commands/auton/CenterWallToLeftSwitch.java +++ b/src/main/java/com/frcteam1939/powerup2018/robot/commands/auton/CenterWallToLeftSwitch.java @@ -1,5 +1,7 @@ package com.frcteam1939.powerup2018.robot.commands.auton; +import com.frcteam1939.powerup2018.robot.commands.cubemanipulator.CubeManipulatorLower; +import com.frcteam1939.powerup2018.robot.commands.cubemanipulator.IntakeCube; import com.frcteam1939.powerup2018.robot.commands.cubemanipulator.OutputCubeMiddle; import com.frcteam1939.powerup2018.robot.commands.drivetrain.SetDrivetrainMotorsSpeed; import com.frcteam1939.powerup2018.robot.commands.drivetrain.TurnToAngle; @@ -22,5 +24,25 @@ public CenterWallToLeftSwitch() { this.addSequential(new Wait(3.3)); this.addSequential(new SetDrivetrainMotorsSpeed(0)); this.addSequential(new OutputCubeMiddle()); + this.addSequential(new SetDrivetrainMotorsSpeed(0.3)); + this.addSequential(new Wait(0.5)); + this.addSequential(new SetDrivetrainMotorsSpeed(0)); + this.addSequential(new TurnToAngle(80)); + this.addSequential(new CubeManipulatorLower()); + this.addSequential(new SetDrivetrainMotorsSpeed(-0.3)); + this.addSequential(new Wait(1)); + this.addSequential(new SetDrivetrainMotorsSpeed(0)); + this.addSequential(new IntakeCube()); + this.addSequential(new SetDrivetrainMotorsSpeed(0.3)); + this.addSequential(new Wait(1)); + this.addSequential(new SetDrivetrainMotorsSpeed(0)); + this.addSequential(new TurnToAngle(-80)); + this.addSequential(new SetElevatorMotorSpeed(1.0)); + this.addSequential(new Wait(1.25)); + this.addSequential(new SetElevatorMotorSpeed(0)); + this.addSequential(new SetDrivetrainMotorsSpeed(-0.3)); + this.addSequential(new Wait(0.5)); + this.addSequential(new SetDrivetrainMotorsSpeed(0)); + this.addSequential(new OutputCubeMiddle()); } } diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/commands/auton/CenterWallToRightSwitch.java b/src/main/java/com/frcteam1939/powerup2018/robot/commands/auton/CenterWallToRightSwitch.java index c44bc4d..1c37b65 100644 --- a/src/main/java/com/frcteam1939/powerup2018/robot/commands/auton/CenterWallToRightSwitch.java +++ b/src/main/java/com/frcteam1939/powerup2018/robot/commands/auton/CenterWallToRightSwitch.java @@ -1,5 +1,7 @@ package com.frcteam1939.powerup2018.robot.commands.auton; +import com.frcteam1939.powerup2018.robot.commands.cubemanipulator.CubeManipulatorLower; +import com.frcteam1939.powerup2018.robot.commands.cubemanipulator.IntakeCube; import com.frcteam1939.powerup2018.robot.commands.cubemanipulator.OutputCubeMiddle; import com.frcteam1939.powerup2018.robot.commands.drivetrain.SetDrivetrainMotorsSpeed; import com.frcteam1939.powerup2018.robot.commands.drivetrain.TurnToAngle; @@ -22,5 +24,25 @@ public CenterWallToRightSwitch() { this.addSequential(new Wait(3.2)); this.addSequential(new SetDrivetrainMotorsSpeed(0)); this.addSequential(new OutputCubeMiddle()); + this.addSequential(new SetDrivetrainMotorsSpeed(0.3)); + this.addSequential(new Wait(0.5)); + this.addSequential(new SetDrivetrainMotorsSpeed(0)); + this.addSequential(new TurnToAngle(-80)); + this.addSequential(new CubeManipulatorLower()); + this.addSequential(new SetDrivetrainMotorsSpeed(-0.3)); + this.addSequential(new Wait(1)); + this.addSequential(new SetDrivetrainMotorsSpeed(0)); + this.addSequential(new IntakeCube()); + this.addSequential(new SetDrivetrainMotorsSpeed(0.3)); + this.addSequential(new Wait(1)); + this.addSequential(new SetDrivetrainMotorsSpeed(0)); + this.addSequential(new TurnToAngle(80)); + this.addSequential(new SetElevatorMotorSpeed(1.0)); + this.addSequential(new Wait(1.25)); + this.addSequential(new SetElevatorMotorSpeed(0)); + this.addSequential(new SetDrivetrainMotorsSpeed(-0.3)); + this.addSequential(new Wait(0.5)); + this.addSequential(new SetDrivetrainMotorsSpeed(0)); + this.addSequential(new OutputCubeMiddle()); } } diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/commands/auton/DriveToScale.java b/src/main/java/com/frcteam1939/powerup2018/robot/commands/auton/DriveToScale.java deleted file mode 100644 index 020cba2..0000000 --- a/src/main/java/com/frcteam1939/powerup2018/robot/commands/auton/DriveToScale.java +++ /dev/null @@ -1,15 +0,0 @@ -package com.frcteam1939.powerup2018.robot.commands.auton; - -import com.frcteam1939.powerup2018.robot.commands.drivetrain.SetDrivetrainMotorsSpeed; -import com.frcteam1939.powerup2018.util.Wait; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -public class DriveToScale extends CommandGroup { - - public DriveToScale() { - this.addSequential(new SetDrivetrainMotorsSpeed(-0.3)); - this.addSequential(new Wait(8.5)); - this.addSequential(new SetDrivetrainMotorsSpeed(0)); - } -} diff --git a/src/main/java/com/frcteam1939/powerup2018/robot/commands/cubemanipulator/IntakeCube.java b/src/main/java/com/frcteam1939/powerup2018/robot/commands/cubemanipulator/IntakeCube.java index f60ef08..28c4b11 100644 --- a/src/main/java/com/frcteam1939/powerup2018/robot/commands/cubemanipulator/IntakeCube.java +++ b/src/main/java/com/frcteam1939/powerup2018/robot/commands/cubemanipulator/IntakeCube.java @@ -1,6 +1,5 @@ package com.frcteam1939.powerup2018.robot.commands.cubemanipulator; -import com.frcteam1939.powerup2018.robot.Robot; import com.frcteam1939.powerup2018.robot.subsystems.CubeManipulator; import com.frcteam1939.powerup2018.util.Wait; @@ -13,10 +12,8 @@ public IntakeCube() { this.addSequential(new CubeManipulatorWheelsOut()); this.addSequential(new SetCubeManipulatorSpeed(CubeManipulator.IN_SPEED)); this.addSequential(new Wait(1)); - if (Robot.cubeManipulator.haveCube()) { - this.addSequential(new CubeManipulatorWheelsIn()); - this.addSequential(new SetCubeManipulatorSpeed(0)); - this.addSequential(new CubeManipulatorMiddle()); - } + this.addSequential(new CubeManipulatorWheelsIn()); + this.addSequential(new SetCubeManipulatorSpeed(0)); + this.addSequential(new CubeManipulatorMiddle()); } } diff --git a/src/main/java/com/frcteam1939/powerup2018/util/PigeonWrapper.java b/src/main/java/com/frcteam1939/powerup2018/util/PigeonWrapper.java index a6025a3..0262910 100644 --- a/src/main/java/com/frcteam1939/powerup2018/util/PigeonWrapper.java +++ b/src/main/java/com/frcteam1939/powerup2018/util/PigeonWrapper.java @@ -14,8 +14,8 @@ public PigeonWrapper(int deviceNumber) { super(deviceNumber); } - public PigeonWrapper(TalonSRX talonSrx) { - super(talonSrx); + public PigeonWrapper(TalonSRX talonSRX) { + super(talonSRX); } @Override