Reset ProfiledPIDControllers at the same time that control begins (#119)

* reset each PID controller when (and only when) control begins

* reset PID controllers upon enabling, use RobotModeTriggers consistently

* move algae wrist to coralmode upon first enable

Author: nlaverdure

src/main/java/frc/robot/Robot.java

@@ -19,6 +19,7 @@
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.lib.AllianceSelector;
 import frc.lib.AutoOption;
@@ -44,8 +45,6 @@
 import frc.robot.elevator.AlgaeRoller;
 import frc.robot.elevator.CoralRoller;
 import frc.robot.elevator.Elevator;
-import frc.robot.elevator.ElevatorConstants.AlgaeWristConstants.AlgaeWristState;
-import frc.robot.elevator.ElevatorConstants.CoralWristConstants.CoralWristState;
 import frc.robot.elevator.Lifter;
 import frc.robot.vision.Vision;
 import frc.util.Gamepiece;
@@ -170,9 +169,6 @@ public void disabledPeriodic() {
   @Override
   public void autonomousInit() {
     autoSelector.scheduleAuto();
-    climber.lockRatchet();
-    elevator.getCoralWrist().createSetAngleCommand(CoralWristState.Initial).schedule();
-    elevator.getAlgaeWrist().createSetAngleCommand(AlgaeWristState.Initial).schedule();
     lifter.setDefaultCommand(lifter.createRemainAtCurrentHeightCommand());
     leds.replaceDefaultCommandImmediately(
         leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier));
@@ -184,18 +180,10 @@ public void autonomousPeriodic() {}
   @Override
   public void teleopInit() {
     autoSelector.cancelAuto();
-
-    swerve.resetHeadingOffset();
-    climber.resetEncoder();
-    climber.lockRatchet();
-    elevator.resetPositionControllers();
     lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID()));
-    // lifter.setDefaultCommand(lifter.createJoystickVoltageCommand(operator.getHID()));
     leds.replaceDefaultCommandImmediately(
         leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier));
 
-    elevator.getCoralWrist().createSetAngleCommand(CoralWristState.Initial).schedule();
-    elevator.getAlgaeWrist().createSetAngleCommand(AlgaeWristState.Initial).schedule();
     // Test wrist feedforwards
     // algaeWrist.setDefaultCommand(algaeWrist.createJoystickControlCommand(operator.getHID()));
     // coralWrist.setDefaultCommand(coralWrist.createJoystickControlCommand(operator.getHID()));
@@ -334,14 +322,24 @@ public boolean getAsBoolean() {
   }
 
   private void configureEventBindings() {
+    RobotModeTriggers.autonomous()
+        .onTrue(elevator.resetPositionControllers()
+        .andThen(climber.lockRatchet()));
+    RobotModeTriggers.teleop()
+        .onTrue(swerve.resetHeadingOffset()
+        .andThen(elevator.resetPositionControllers())
+        .andThen(climber.lockRatchet())
+        .andThen(climber.resetEncoder()));
+
     algaeRoller.hasAlgae.whileTrue(algaeRoller.createHoldAlgaeCommand());
     coralRoller.isRolling.whileTrue(createRollerAnimationCommand());
   }
-  // spotless:on
 
   private void configureAutoOptions() {
-    autoSelector.addAuto(new AutoOption(Alliance.Red, 1, new RedL4Auto(swerve, elevator)));
-    autoSelector.addAuto(new AutoOption(Alliance.Blue, 1, new BlueL4Auto(swerve, elevator)));
+    autoSelector.addAuto(
+        new AutoOption(Alliance.Red, 1, new RedL4Auto(swerve, elevator)));
+    autoSelector.addAuto(
+        new AutoOption(Alliance.Blue, 1, new BlueL4Auto(swerve, elevator)));
     autoSelector.addAuto(
         new AutoOption(Alliance.Red, 2, new RedNoProcess3PieceAuto(swerve, elevator)));
     autoSelector.addAuto(
@@ -350,9 +348,12 @@ private void configureAutoOptions() {
         new AutoOption(Alliance.Red, 3, new RedProcess3PieceAuto(swerve, elevator)));
     autoSelector.addAuto(
         new AutoOption(Alliance.Blue, 3, new BlueProcess3PieceAuto(swerve, elevator)));
-    autoSelector.addAuto(new AutoOption(Alliance.Blue, 4, new BlueMoveAuto(swerve)));
-    autoSelector.addAuto(new AutoOption(Alliance.Red, 4, new RedMoveAuto(swerve)));
+    autoSelector.addAuto(
+        new AutoOption(Alliance.Blue, 4, new BlueMoveAuto(swerve)));
+    autoSelector.addAuto(
+        new AutoOption(Alliance.Red, 4, new RedMoveAuto(swerve)));
   }
+  // spotless:on
 
   /**
    * Gets the current drawn from the Power Distribution Hub by a CAN motor controller, assuming that

src/main/java/frc/robot/climber/Climber.java

@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ClimberConstants;
 import frc.robot.Constants.MotorConstants.NEOVortexConstants;
@@ -83,16 +84,16 @@ public void periodic() {
     // SmartDashboard.putNumber("Climber/Applied Duty Cycle", motor.getAppliedOutput());
   }
 
-  public void resetEncoder() {
-    encoder.setPosition(0);
+  public Command resetEncoder() {
+    return new InstantCommand(() -> encoder.setPosition(0));
   }
 
-  public void unlockRatchet() {
-    servo.set(ClimberConstants.kDisengedPosition);
+  public Command unlockRatchet() {
+    return new InstantCommand(() -> servo.set(ClimberConstants.kDisengedPosition));
   }
 
-  public void lockRatchet() {
-    servo.set(ClimberConstants.kEngagedPosition);
+  public Command lockRatchet() {
+    return new InstantCommand(() -> servo.set(ClimberConstants.kEngagedPosition));
   }
 
   private void setPosition(double targetPosition) {
@@ -135,51 +136,43 @@ public Command createDefaultClimberCommand() {
         });
   }
 
-  public Command createUnlockCommand() {
-    return this.run(() -> unlockRatchet());
-  }
-
-  public Command createLockCommand() {
-    return this.run(() -> lockRatchet());
-  }
-
   /**
    * @return Command that unlocks and deploys the climber arm
    */
   public Command createDeployCommand() {
     return new FunctionalCommand(
-        // initialize
-        () -> {
-          unlockRatchet();
-          setPosition(ClimberConstants.kDeployPosition);
-        },
-        // execute
-        () -> {},
-        // end
-        interrupted -> {
-          lockRatchet();
-          motor.set(0.0);
-        },
-        // isFinished
-        () -> isDeployed(),
-        // requirements
-        this);
+            // initialize
+            () -> {
+              setPosition(ClimberConstants.kDeployPosition);
+            },
+            // execute
+            () -> {},
+            // end
+            interrupted -> {
+              motor.set(0.0);
+            },
+            // isFinished
+            () -> isDeployed(),
+            // requirements
+            this)
+        .beforeStarting(unlockRatchet())
+        .andThen(lockRatchet());
   }
 
   public Command createRetractCommand() {
     return new FunctionalCommand(
-        // initialze
-        () -> {
-          lockRatchet();
-          setPosition(ClimberConstants.kRetractPosition);
-        },
-        // execute
-        () -> {},
-        // end
-        interrupted -> {},
-        // isFinished
-        () -> isRetracted(),
-        // requirements
-        this);
+            // initialze
+            () -> {
+              setPosition(ClimberConstants.kRetractPosition);
+            },
+            // execute
+            () -> {},
+            // end
+            interrupted -> {},
+            // isFinished
+            () -> isRetracted(),
+            // requirements
+            this)
+        .beforeStarting(lockRatchet());
   }
 }

src/main/java/frc/robot/drivetrain/Drivetrain.java

@@ -21,6 +21,8 @@
 import edu.wpi.first.units.measure.Dimensionless;
 import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.AutoConstants.RotationControllerGains;
 import frc.robot.Constants.AutoConstants.TranslationControllerGains;
@@ -181,8 +183,8 @@ public Rotation2d getHeadingOffset() {
     return headingOffset;
   }
 
-  public void resetHeadingOffset() {
-    headingOffset = new Rotation2d();
+  public Command resetHeadingOffset() {
+    return new InstantCommand(() -> this.headingOffset = new Rotation2d());
   }
 
   public void setHeadingOffset() {

src/main/java/frc/robot/elevator/AlgaeWrist.java

@@ -146,9 +146,12 @@ public Command createRemainAtCurrentAngleCommand() {
     return new FunctionalCommand(
         // initialize
         () -> {
-          if (targetState == AlgaeWristState.Unknown) {
+          if (targetState == AlgaeWristState.Initial) {
             targetState = AlgaeWristState.CoralMode;
             feedback.setGoal(targetState.angle.in(Radians));
+            // Users should call reset() when they first start running the controller to avoid
+            // unwanted behavior.
+            resetController();
           }
         },
         // execute

src/main/java/frc/robot/elevator/CoralWrist.java

@@ -157,7 +157,12 @@ public Command createRemainAtCurrentAngleCommand() {
     return new FunctionalCommand(
         // initialize
         () -> {
-          if (targetState == CoralWristState.Unknown) feedback.setGoal(encoder.getPosition());
+          if (targetState == CoralWristState.Initial) {
+            feedback.setGoal(encoder.getPosition());
+            // Users should call reset() when they first start running the controller to avoid
+            // unwanted behavior.
+            resetController();
+          }
         },
         // execute
         () -> control(),

src/main/java/frc/robot/elevator/Elevator.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.elevator.ElevatorConstants.AlgaeWristConstants.AlgaeWristState;
 import frc.robot.elevator.ElevatorConstants.CoralWristConstants.CoralWristState;
 import frc.robot.elevator.ElevatorConstants.LifterConstants.LifterState;
@@ -36,10 +37,13 @@ public AlgaeWrist getAlgaeWrist() {
     return algaeWrist;
   }
 
-  public void resetPositionControllers() {
-    lifter.resetController();
-    coralWrist.resetController();
-    algaeWrist.resetController();
+  public Command resetPositionControllers() {
+    return new InstantCommand(
+        () -> {
+          lifter.resetController();
+          coralWrist.resetController();
+          algaeWrist.resetController();
+        });
   }
 
   public Command coralL4PositionCG() {

src/main/java/frc/robot/elevator/ElevatorConstants.java

@@ -62,7 +62,7 @@ public final class LifterController {
     }
 
     public static enum LifterState {
-      Unknown(0.0),
+      Initial(0.0),
       Min(0.0),
       AlgaeIntakeFloor(1.0),
       EncoderReset(0.36),
@@ -149,7 +149,6 @@ public static final class CoralWristConstants {
 
     public static enum CoralWristState {
       Initial(90),
-      Unknown(90),
       Min(15),
       Max(135),
       L1(125),
@@ -234,7 +233,6 @@ public static final class AlgaeWristConstants {
 
     public static enum AlgaeWristState {
       Initial(80),
-      Unknown(90),
       Floor(0),
       Min(-10),
       Max(95),

src/main/java/frc/robot/elevator/Lifter.java

@@ -57,7 +57,7 @@ public class Lifter extends SubsystemBase {
       new ElevatorFeedforward(LifterController.kS, LifterController.kG, LifterController.kV);
 
   private final EventLoop loop = new EventLoop();
-  private LifterState targetState = LifterState.Unknown;
+  private LifterState targetState = LifterState.Initial;
 
   public Lifter() {
     // spotless:off
@@ -110,7 +110,8 @@ public void periodic() {
     // SmartDashboard.putNumber("Lifter/Velocity", encoder.getVelocity());
 
     SmartDashboard.putString("Lifter/Target State", getTargetState().name());
-    SmartDashboard.putNumber("Lifter/Target Height", feedback.getGoal().position);
+    SmartDashboard.putNumber("Lifter/Height Setpoint", feedback.getSetpoint().position);
+    SmartDashboard.putNumber("Lifter/Height Goal", feedback.getGoal().position);
 
     // SmartDashboard.putNumber("Lifter/Leader Applied Duty Cycle", leaderMotor.getAppliedOutput());
     // SmartDashboard.putNumber(
@@ -126,10 +127,6 @@ public Distance getCurrentHeight() {
     return Meters.of(encoder.getPosition());
   }
 
-  private Distance getSetPointHeight() {
-    return Meters.of(feedback.getSetpoint().position);
-  }
-
   public Dimensionless getProportionOfMaxHeight() {
     return getCurrentHeight().div(LifterState.Max.height);
   }
@@ -185,7 +182,12 @@ public Command createRemainAtCurrentHeightCommand() {
     return new FunctionalCommand(
         // initialize
         () -> {
-          if (targetState == LifterState.Unknown) feedback.setGoal(encoder.getPosition());
+          if (targetState == LifterState.Initial) {
+            feedback.setGoal(encoder.getPosition());
+            // Users should call reset() when they first start running the controller to avoid
+            // unwanted behavior.
+            resetController();
+          }
         },
         // execute
         () -> control(),