Merge branch 'ch-wait-to-shoot' into ch-new-limelights

elastic-layout.json

@@ -9,7 +9,7 @@
         "containers": [
           {
             "title": "Delay",
-            "x": 0.0,
+            "x": 512.0,
             "y": 384.0,
             "width": 128.0,
             "height": 128.0,
@@ -52,7 +52,7 @@
           },
           {
             "title": "Auto",
-            "x": 128.0,
+            "x": 640.0,
             "y": 384.0,
             "width": 256.0,
             "height": 128.0,
@@ -115,7 +115,7 @@
               "topic": "/SmartDashboard/Mode(Std)",
               "period": 0.06,
               "data_type": "boolean",
-              "true_color": 4279537940,
+              "true_color": 4283215696,
               "false_color": 4294901760,
               "true_icon": "None",
               "false_icon": "None"
@@ -137,7 +137,7 @@
           },
           {
             "title": "Index",
-            "x": 640.0,
+            "x": 768.0,
             "y": 256.0,
             "width": 128.0,
             "height": 128.0,
@@ -147,14 +147,14 @@
               "period": 0.06,
               "data_type": "boolean",
               "true_color": 4283215696,
-              "false_color": 4289280827,
+              "false_color": 4294198070,
               "true_icon": "None",
               "false_icon": "None"
             }
           },
           {
             "title": "Shooter",
-            "x": 640.0,
+            "x": 768.0,
             "y": 128.0,
             "width": 128.0,
             "height": 128.0,
@@ -214,20 +214,6 @@
               "true_icon": "None",
               "false_icon": "None"
             }
-          },
-          {
-            "title": "FL Rot Output",
-            "x": 768.0,
-            "y": 256.0,
-            "width": 128.0,
-            "height": 128.0,
-            "type": "Text Display",
-            "properties": {
-              "topic": "/SmartDashboard/Swerve/FL Rot Output",
-              "period": 0.06,
-              "data_type": "double",
-              "show_submit_button": false
-            }
           }
         ]
       }
@@ -583,66 +569,6 @@
               "period": 0.06,
               "counter_clockwise_positive": false
             }
-          },
-          {
-            "title": "FL Rot Output",
-            "x": 1024.0,
-            "y": 0.0,
-            "width": 256.0,
-            "height": 256.0,
-            "type": "Graph",
-            "properties": {
-              "topic": "/SmartDashboard/Swerve/FL Rot Output",
-              "period": 0.033,
-              "data_type": "double",
-              "time_displayed": 5.0,
-              "color": 4278238420,
-              "line_width": 2.0
-            }
-          },
-          {
-            "title": "FR Rot Output",
-            "x": 1280.0,
-            "y": 0.0,
-            "width": 128.0,
-            "height": 128.0,
-            "type": "Text Display",
-            "properties": {
-              "topic": "/SmartDashboard/Swerve/FR Rot Output",
-              "period": 0.06,
-              "data_type": "double",
-              "show_submit_button": false
-            }
-          },
-          {
-            "title": "RL Rot Output",
-            "x": 1024.0,
-            "y": 256.0,
-            "width": 384.0,
-            "height": 256.0,
-            "type": "Graph",
-            "properties": {
-              "topic": "/SmartDashboard/Swerve/RL Rot Output",
-              "period": 0.033,
-              "data_type": "double",
-              "time_displayed": 5.0,
-              "color": 4278238420,
-              "line_width": 2.0
-            }
-          },
-          {
-            "title": "RR Rot Output",
-            "x": 1408.0,
-            "y": 0.0,
-            "width": 128.0,
-            "height": 128.0,
-            "type": "Text Display",
-            "properties": {
-              "topic": "/SmartDashboard/Swerve/RR Rot Output",
-              "period": 0.06,
-              "data_type": "double",
-              "show_submit_button": false
-            }
           }
         ]
       }

src/main/java/team/gif/lib/drivePace.java

@@ -5,6 +5,7 @@
 public enum drivePace {
     COAST_FR(Constants.Drivetrain.COAST_SPEED_METERS_PER_SECOND, true),
     COAST_RR(Constants.Drivetrain.COAST_SPEED_METERS_PER_SECOND, false),
+    COAST_ROT(Constants.Drivetrain.COAST_SPEED_METERS_PER_SECOND, false),
     SLOW_FR(Constants.Drivetrain.SLOW_SPEED_METERS_PER_SECOND, true),
     SLOW_RR(Constants.Drivetrain.SLOW_SPEED_METERS_PER_SECOND, false),
     BOOST_FR(Constants.Drivetrain.BOOST_SPEED_METERS_PER_SECOND, true),

src/main/java/team/gif/robot/Constants.java

@@ -248,8 +248,10 @@ public static final class Shooter {
 
         public static final int SHOOT_CYCLES = 12;
         public static final double REEF_SENSOR_TARGET_DISTANCE_MM = 500;
-        public static final double ALIGN_STRAFE_SPEED_MPS = 0.3; //Meters per Second
+        public static final double ALIGN_STRAFE_SPEED_MPS = 0.30; //Meters per Second
         public static final double AUTON_STRAFE_SPEED_MPS = 0.15; //Meters per Second
+
+        public static final double REEF_SENSOR_DEBOUNCE_SECS = 0.040;
     }
 
     public static final class Climber {
@@ -281,7 +283,8 @@ public static final class Elevator{
         public static final double MAX_ACCELERATION = 130;
         public static final double REV_MAX_ACCELERATION = 80;
 
-        public static final double PID_TOLERANCE = 0.1;
+        public static final double SHOOT_TOLERANCE = 0.3;
+        public static final double MOTION_MAGIC_TOLERANCE = 0.1;
         public static final double MIN_PERCENT_MANUAL = -0.15;
         public static final double MAX_PERCENT_MANUAL = 0.15;
 

src/main/java/team/gif/robot/OI.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -10,7 +11,7 @@
 import team.gif.robot.commands.climber.ClimberDeploy;
 import team.gif.robot.commands.climber.ClimberClimb;
 import team.gif.robot.commands.climber.PistonToggleState;
-import team.gif.robot.commands.driveModes.EnableRobotOrientedMode;
+import team.gif.robot.commands.driveModes.EnableRotatedMode;
 import team.gif.robot.commands.drivetrain.Reset180;
 import team.gif.robot.commands.elevator.SetElevatorPosition;
 import team.gif.robot.commands.shooter.Shoot;
@@ -19,7 +20,6 @@
 import team.gif.robot.commands.drivetrain.Reset0;
 import team.gif.robot.commands.toggleManualControl.ToggleManualControl;
 
-
 public class OI {
     /*
      * Instantiate all joysticks/controllers and their buttons here
@@ -123,7 +123,7 @@ public OI() {
         dLBump.whileTrue(new EnableBoost());
         dX.whileTrue(new AutoDriveAndShoot(false));
         dB.whileTrue(new AutoDriveAndShoot(true));
-        dRBump.whileTrue(new EnableRobotOrientedMode());
+        dRBump.whileTrue(new EnableRotatedMode());
 
         // aux controls
         aStart.and(aDPadUp).onTrue(new Reset0());
@@ -134,11 +134,13 @@ public OI() {
         aA.whileTrue(new ClimberDeploy());
         aStart.and(aBack).toggleOnTrue(new ToggleManualControl());
         aRBump.onTrue(new PistonToggleState());
-        aLBump.onTrue(new SetElevatorPosition(Constants.Elevator.LEVEL_4_POSITION));
-        aDPadUp.and(aStart.negate()).onTrue(new SetElevatorPosition(Constants.Elevator.LEVEL_3_POSITION));
-        aDPadLeft.and(aStart.negate()).onTrue(new SetElevatorPosition(Constants.Elevator.LEVEL_2_POSITION));
-        aDPadDown.and(aStart.negate()).onTrue(new SetElevatorPosition(Constants.Elevator.COLLECTOR_POSITION));
-
+        // Only run the SetElevatorPosition if robot is in StandardOp mode, false condition (i.e. manual mode) still returns
+        // to elevator defalt command which is PID mode, so need to restart ElevatorManualMode
+        aLBump.onTrue(new ConditionalCommand(new SetElevatorPosition(Constants.Elevator.LEVEL_4_POSITION), new InstantCommand(Robot::enableRobotModeManual), Robot::isRobotInStandardOpMode));
+        aDPadUp.and(aStart.negate()).onTrue(new ConditionalCommand(new SetElevatorPosition(Constants.Elevator.LEVEL_3_POSITION), new InstantCommand(Robot::enableRobotModeManual), Robot::isRobotInStandardOpMode));
+        aDPadLeft.and(aStart.negate()).onTrue(new ConditionalCommand(new SetElevatorPosition(Constants.Elevator.LEVEL_2_POSITION), new InstantCommand(Robot::enableRobotModeManual), Robot::isRobotInStandardOpMode));
+        aDPadDown.and(aStart.negate()).onTrue(new ConditionalCommand(new SetElevatorPosition(Constants.Elevator.COLLECTOR_POSITION), new InstantCommand(Robot::enableRobotModeManual), Robot::isRobotInStandardOpMode));
+
         shooterSensor.debounce(Constants.DEBOUNCE_DEFAULT).onTrue(new Rumble().andThen(new WaitCommand(0.1).andThen(new Rumble())));
 
         //test sys id for elevator, delete later

src/main/java/team/gif/robot/Robot.java

@@ -225,6 +225,10 @@ public static RobotMode getRobotMode() {
         return robotMode;
     }
 
+    public static boolean isRobotInStandardOpMode() {
+        return robotMode == RobotMode.STANDARD_OP;
+    }
+
     static public void enableRobotModeManual() {
         robotMode = RobotMode.MANUAL;
 
@@ -234,5 +238,7 @@ static public void enableRobotModeManual() {
 
     static public void enableRobotModeStandardOp() {
         robotMode = RobotMode.STANDARD_OP;
+
+        elevator.enableElevator();
     }
 }

src/main/java/team/gif/robot/commands/climber/ClimberClimb.java

@@ -9,13 +9,18 @@ public class ClimberClimb extends Command {
 
     public ClimberClimb() {
         super();
+
         addRequirements(Robot.climber, Robot.swerveDrive);
     }
 
     // Called when the command is initially scheduled.
     @Override
     public void initialize() {
-        Robot.swerveDrive.setDrivePace(drivePace.COAST_RR);
+        // Disable the elevator when climbing preventing the aux from accidentally raising the elevator
+        // Do not re-enable at the end of the command. Want to keep elevator disabled until
+        // aux toggles manaul mode. Although Deploy called this, it is possible aux toggled manual
+        // control after deploying so make sure the elevator continues to be disabled
+        Robot.elevator.disableElevator();
     }
 
     // Called every time the scheduler runs (~20ms) while the command is scheduled
@@ -24,10 +29,12 @@ public void execute() {
         Robot.climber.move(-Constants.Climber.CLIMB_PERCENT);
 
         // deploy the piston when the climber reaches a predetermined set point
+        // and only call it once
 //        if (Robot.climber.getPosition() < Constants.Climber.PISTON_DEPLOY_POS && !Robot.climber.getPistonStateOut()) {
-//        if (!Robot.climber.getPistonStateOut()) {
-            Robot.climber.setPistonOut();
-//        }
+        Robot.climber.setPistonOut();
+
+        // Drive forward at a slow speed to assist in the climb
+        Robot.swerveDrive.setDrivePace(drivePace.COAST_ROT);
         Robot.swerveDrive.drive(0, Constants.Climber.DRIVE_SPEED_MPS, 0);
     }
 

src/main/java/team/gif/robot/commands/climber/ClimberDeploy.java

@@ -14,7 +14,12 @@ public ClimberDeploy() {
 
     // Called when the command is initially scheduled.
     @Override
-    public void initialize() {}
+    public void initialize() {
+        // Disable the elevator when climbing preventing the aux from accidentally raising the elevator
+        // Do not re-enable at the end of the command. Want to keep elevator disabled until
+        // aux toggles manaul mode.
+        Robot.elevator.disableElevator();
+    }
 
     // Called every time the scheduler runs (~20ms) while the command is scheduled
     @Override

src/main/java/team/gif/robot/commands/driveModes/EnableRobotOrientedMode.java → src/main/java/team/gif/robot/commands/driveModes/EnableRotatedMode.java

@@ -5,18 +5,17 @@
 import team.gif.lib.drivePace;
 import team.gif.robot.Robot;
 
-public class EnableRobotOrientedMode extends Command {
+public class EnableRotatedMode extends Command {
 
-    public EnableRobotOrientedMode() {
+    public EnableRotatedMode() {
         super();
-        //addRequirements(Robot.climber); // uncomment
     }
 
     // Called when the command is initially scheduled.
     @Override
     public void initialize() {
-        System.out.println("Robot Oriented Enabled w/ " + DriverStation.getMatchTime() + " remaining");
-        Robot.swerveDrive.setDrivePace(drivePace.COAST_RR);
+        System.out.println("Robot Rotated Mode Enabled w/ " + DriverStation.getMatchTime() + " remaining");
+        Robot.swerveDrive.setDrivePace(drivePace.COAST_ROT);
     }
 
     // Called every time the scheduler runs (~20ms) while the command is scheduled
@@ -32,7 +31,7 @@ public boolean isFinished() {
     // Called when the command ends or is interrupted.
     @Override
     public void end(boolean interrupted) {
-        System.out.println("Robot Oriented Disabled w/" +  DriverStation.getMatchTime() + " remaining");
+        System.out.println("Robot Rotated Mode Disabled w/" +  DriverStation.getMatchTime() + " remaining");
         Robot.swerveDrive.setDrivePace(drivePace.COAST_FR);
     }
 }

src/main/java/team/gif/robot/commands/drivetrain/DriveSwerve.java

@@ -34,7 +34,7 @@ public void execute() {
 
             // During robot oriented mode (_RR), make climber the front of the bot (rotate forward and strafe 90 degrees)
             // This only occurs when using joysticks since DriveSwerve is for joystick response
-            if (Robot.swerveDrive.getDrivePace() == drivePace.COAST_RR) {
+            if (Robot.swerveDrive.getDrivePace() == drivePace.COAST_ROT) {
                 double forwardStore = forward;
                 double strafeStore = strafe;
                 forward = -strafeStore;

src/main/java/team/gif/robot/commands/shooter/AutoDriveAndShoot.java

@@ -30,28 +30,31 @@ public AutoDriveAndShoot(boolean moveRight) {
     // Called when the command is initially scheduled.
     @Override
     public void initialize() {
-        hasTarget = false;
         Robot.swerveDrive.setDrivePace(drivePace.COAST_RR);
+        hasTarget = Robot.shooter.isShooterAligned();
     }
 
     // Called every time the scheduler runs (~20ms) while the command is scheduled
     @Override
     public void execute() {
-        if (Robot.shooter.isShooterAligned() && !hasTarget) {
+        if (hasTarget) {
+            Robot.swerveDrive.stopDrive();
             hasTarget = true;
         } else {
             boolean inverted = moveRight;
             //If we are facing the alliance wall, invert
             inverted = Robot.pigeon.get360Heading() > 90 && Robot.pigeon.get360Heading() < 270 ? !inverted : inverted;
             double speed = Constants.Shooter.ALIGN_STRAFE_SPEED_MPS * (inverted ? -1 : 1);
             Robot.swerveDrive.drive(0, speed, 0.0);
+
+            hasTarget = Robot.shooter.isShooterAligned();
         }
     }
 
     // Return true when the command should end, false if it should continue. Runs every ~20ms.
     @Override
     public boolean isFinished() {
-        return hasTarget;
+        return Robot.elevator.isReadyToShoot() && hasTarget;
     }
 
     // Called when the command ends or is interrupted.
@@ -61,17 +64,15 @@ public void end(boolean interrupted) {
         Robot.swerveDrive.setDrivePace(drivePace.COAST_FR);
 
         // only shoot if the robot found the target during the command
-        if (hasTarget) {
+        if (Robot.elevator.isReadyToShoot() && hasTarget) {
             // Run the shooter using the standard shoot command and return the elevator
             // The "drive away" and "move the elevator" need to be in separate schedulers
             // or the driver won't be able to move the robot until the move elevator command
             // has completed. Need to delay the elevator until the shooter times out.
             // Effectively, this executes the drive away and moving of the elevator at the same time
             new SequentialCommandGroup(
                     new Shoot(),
-//                    new ParallelDeadlineGroup( // running these in parallel provides plenty of time to clear
-                            new ShortDriveAway()
-//                            new StopModules())
+                    new ShortDriveAway()
             ).schedule();
             new SequentialCommandGroup(
                     new WaitCommand(Constants.Shooter.SHOOT_CYCLES * 0.020), // scheduler runs every 20 ms

src/main/java/team/gif/robot/commands/shooter/AutonStrafeToTarget.java

@@ -28,7 +28,7 @@ public void execute() {
         // This makes sure the sensors don't trip if the robot lined up
         // correctly and is still raising the elevator
         // Since this only runs in auto, safe to just chcek height of level 4
-        if (Robot.elevator.getPosition() < Constants.Elevator.LEVEL_4_POSITION - 0.3 ) {
+        if (Robot.elevator.getPosition() < Constants.Elevator.LEVEL_4_POSITION - Constants.Elevator.SHOOT_TOLERANCE) {
             return;
         }