Code as of half-way through first day of Granite State District Event. Minor tweaks to autonomous programs and ShooterFiringSequence. Minor naming convention tweaks.

Author: coach-streeter

src/main/java/org/mayheminc/robot2020/RobotContainer.java

@@ -199,11 +199,10 @@ private void configureDriverPadButtons() {
         DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterWheelAdjust(50.0));
         DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterWheelAdjust(-50.0));
         DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0));
-        DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(3000));
+        DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON
+                .whenPressed(new ShooterWheelSet(RobotContainer.shooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED));
 
         DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whileHeld(new DriveStraight(0.1));
-        // TODO: above hard-coded constant (3000) should be a named constant from
-        // Shooter.java
 
         DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenHeld(new ShooterFiringSequence(60.0));
         DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire());

src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java

@@ -27,7 +27,7 @@ public StartBWOppTrench() {
                 // lower the intake and turn it on before driving forwards
                 addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
                 addCommands(new IntakeSetRollers(-1.0));
-                addCommands(new Wait(5.0));
+                addCommands(new Wait(2.5));
 
                 // make sure the hood is down
                 addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION));
@@ -53,6 +53,9 @@ public StartBWOppTrench() {
                 // turn on the intake gently while shooting to help balls settle
                 addCommands(new IntakeSetRollers(-0.2));
 
+                // wait 2/10 of a second to get some camera data after turning turret
+                addCommands(new Wait(0.2));
+
                 // use the "one button" firing sequence
                 addCommands(new ShooterFiringSequence(5.0));
 

src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java

@@ -28,14 +28,15 @@ public ShooterFiringSequence(double waitDuration) {
     // one last aim at the target before starting shooting.
     addCommands(new ShooterAimToTarget());
 
-    addCommands(new ParallelRaceGroup(new ShooterWheelSetToTarget(true), new TurretAimToTarget()));
+    addCommands(new ParallelRaceGroup(new ShooterWheelSetToTarget(true), new TurretAimToTargetContinuously()));
 
     // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the
     // revolver turntable, shoot for specified duration
     // TODO: should really shoot until no balls detected any more
-    addCommands(
-        new ParallelRaceGroup(new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)),
-            new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.3)), new Wait(waitDuration)));
+    addCommands(new ParallelRaceGroup( //
+        new TurretAimToTargetContinuously(), // continue aiming while shooting
+        new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)),
+        new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.3)), new Wait(waitDuration)));
 
     // turn off the feeder, chimney, and revolver, ending after 0.1 seconds
     addCommands(new ShooterCeaseFire());

src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java

@@ -32,7 +32,7 @@ public ShooterReadyAimFire(double waitDuration) {
     // to speed
     addCommands(new ParallelRaceGroup(
         new ParallelCommandGroup(/* new TargetingIsOnTarget(), */ new ShooterWheelSet(3000.0, true)),
-        new TurretAimToTarget()));
+        new TurretAimToTargetContinuously()));
 
     // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the
     // revolver turntable, shoot for specified duration

src/main/java/org/mayheminc/robot2020/commands/TurretAimToTarget.java renamed to src/main/java/org/mayheminc/robot2020/commands/TurretAimToTargetContinuously.java

@@ -11,11 +11,11 @@
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
-public class TurretAimToTarget extends CommandBase {
+public class TurretAimToTargetContinuously extends CommandBase {
   /**
    * Creates a new TurretAimToTarget.
    */
-  public TurretAimToTarget() {
+  public TurretAimToTargetContinuously() {
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(RobotContainer.turret);
   }
@@ -28,7 +28,11 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    // TODO: may want to only aim at the target if recent valid target data from the
+    // camera
     double pos = RobotContainer.targeting.getDesiredAzimuth();
+    // TODO: if the desired azimuth is "beyond the soft stop" should we turn the
+    // robot?
     RobotContainer.turret.setPositionAbs(pos);
   }
 

src/main/java/org/mayheminc/robot2020/subsystems/Climber.java

@@ -23,8 +23,10 @@ public class Climber extends SubsystemBase {
 
   private final MayhemTalonSRX winchLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT);
   private final MayhemTalonSRX winchRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT);
-  // private final MayhemTalonSRX walkerLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT);
-  // private final MayhemTalonSRX walkerRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT);
+  // private final MayhemTalonSRX walkerLeft = new
+  // MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT);
+  // private final MayhemTalonSRX walkerRight = new
+  // MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT);
 
   private final Solenoid pistons = new Solenoid(Constants.Solenoid.CLIMBER_PISTONS);
 
@@ -33,8 +35,6 @@ public class Climber extends SubsystemBase {
    */
   public Climber() {
 
-    // TODO:  Add position control to go all the way to the top, and nearly all the way in for climbing
-    
     winchLeft.setNeutralMode(NeutralMode.Brake);
     winchLeft.configNominalOutputVoltage(+0.0f, -0.0f);
     winchLeft.configPeakOutputVoltage(+12.0, -12.0);
@@ -52,7 +52,7 @@ public Climber() {
     winchRight.configForwardSoftLimitEnable(true);
     winchRight.configReverseSoftLimitThreshold(MIN_HEIGHT_SOFT_LIMIT);
     winchRight.configReverseSoftLimitEnable(true);
-    
+
     // walkerRight.setNeutralMode(NeutralMode.Brake);
     // walkerRight.configNominalOutputVoltage(+0.0f, -0.0f);
     // walkerRight.configPeakOutputVoltage(+12.0, -12.0);
@@ -86,11 +86,11 @@ public void setWinchRightSpeed(double power) {
   }
 
   // public void setWalkerLeftSpeed(double power) {
-  //   walkerLeft.set(ControlMode.Velocity, power);
+  // walkerLeft.set(ControlMode.Velocity, power);
   // }
 
   // public void setWalkerRightSpeed(double power) {
-  //   walkerRight.set(ControlMode.Velocity, power);
+  // walkerRight.set(ControlMode.Velocity, power);
   // }
 
   public void setPistons(boolean b) {

src/main/java/org/mayheminc/robot2020/subsystems/Intake.java

@@ -37,8 +37,8 @@ enum PivotMode {
     MANUAL_MODE, PID_MODE,
   };
 
-  PivotMode mode = PivotMode.MANUAL_MODE;
-  boolean isMoving;
+  PivotMode m_mode = PivotMode.MANUAL_MODE;
+  boolean m_isMoving;
   double m_targetPosition;
   double m_feedForward;
   Timer m_pidTimer = new Timer();
@@ -104,14 +104,14 @@ public void setRollers(double power) {
 
   public void setPivot(Double b) {
     m_targetPosition = b;
-    mode = PivotMode.PID_MODE;
-    isMoving = true;
+    m_mode = PivotMode.PID_MODE;
+    m_isMoving = true;
 
     m_pidTimer.start();
   }
 
   public boolean isPivotAtPosition() {
-    return !isMoving;
+    return !m_isMoving;
   }
 
   @Override
@@ -125,12 +125,12 @@ public void periodic() {
 
   private void updatePivotPower() {
 
-    if (mode == PivotMode.PID_MODE) {
+    if (m_mode == PivotMode.PID_MODE) {
       // if the pivot is close enough or it has been on too long, turn the motor on
       // gently downards
       if ((Math.abs(pivotTalon.getPosition() - m_targetPosition) < PIVOT_CLOSE_ENOUGH)
           || m_pidTimer.hasPeriodPassed(Intake.MAX_PID_MOVEMENT_TIME_SEC)) {
-        isMoving = false;
+        m_isMoving = false;
 
         // if the current position is closer to PIVOT UP than PIVOT DOWN, apply a little
         // positive power.
@@ -171,14 +171,14 @@ public void updateSmartDashBoard() {
     SmartDashboard.putNumber("Intake Target", m_targetPosition);
     SmartDashboard.putNumber("Intake FeedForward", m_feedForward);
 
-    SmartDashboard.putBoolean("Intake Is Moving", isMoving);
-    SmartDashboard.putBoolean("Intake PID Mode", (mode == PivotMode.PID_MODE));
+    SmartDashboard.putBoolean("Intake Is Moving", m_isMoving);
+    SmartDashboard.putBoolean("Intake PID Mode", (m_mode == PivotMode.PID_MODE));
     SmartDashboard.putNumber("Intake Rollers", rollerTalon.getMotorOutputPercent());
   }
 
   public void setPivotVBus(double VBus) {
     pivotTalon.set(ControlMode.PercentOutput, VBus);
-    mode = PivotMode.MANUAL_MODE;
+    m_mode = PivotMode.MANUAL_MODE;
   }
 
   @Override