Elevator testing (#93)

* elevator testing

* changed coral wrist protection

* changed coral wrist protection scheme again

* use units in wrists

* set Algae Wrist to 90 if state unknown

Author: nlaverdure

src/main/java/frc/lib/AutoSelector.java

@@ -95,7 +95,9 @@ private boolean updateAuto() {
   /**
    * @return Object for binding a command to a change in autonomous mode selection
    */
-  public Trigger changedAutoSelection = autoSelectionChanged.castTo(Trigger::new);
+  public Trigger getChangedAutoSelection() {
+    return autoSelectionChanged.castTo(Trigger::new);
+  }
 
   /** Schedules the command corresponding to the selected autonomous mode */
   public void scheduleAuto() {

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

@@ -274,12 +274,14 @@ private void configureOperatorButtonBindings() {
     climbTrigger.onTrue(climber.createDeployCommand()
         .andThen(climber.createClimbByControllerCommand(operator.getHID(), -ClimberConstants.kMaxVelocityInchesPerSecond)));
   }
-  // spotless:on
 
   private void configureEventBindings() {
-    autoSelector.changedAutoSelection.onTrue(leds.createChangeAutoAnimationCommand());
-    lifter.impendingCoralGripperDamage.onTrue(coralWrist.createSetAngleCommand(CoralWristState.L1));
+    autoSelector.getChangedAutoSelection()
+        .onTrue(leds.createChangeAutoAnimationCommand());
+    lifter.tooHighForCoralWrist.and(coralWrist.atRiskOfDamage)
+        .onTrue(coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode));
   }
+  // spotless:on
 
   private void configureAutoOptions() {
     autoSelector.addAuto(new AutoOption(Alliance.Red, 1, new RedL4AlgaeAuto(swerve, elevator)));

src/main/java/frc/robot/auto/BlueL4AlgaeAuto.java

@@ -71,7 +71,7 @@ public AutoRoutine getAutoRoutine() {
             algaeRoller.createOuttakeCommand().withTimeout(0.2),
             Commands.parallel(
                 blueProcessToSource.cmd(),
-                lifter.createSetHeightCommand(LifterState.Intake))));
+                lifter.createSetHeightCommand(LifterState.CoralIntake))));
     // spotless:on
 
     return blueL4AlgAutoRoutine;

src/main/java/frc/robot/auto/RedL4AlgaeAuto.java

@@ -71,7 +71,7 @@ public AutoRoutine getAutoRoutine() {
             algaeRoller.createOuttakeCommand().withTimeout(0.2),
             Commands.parallel(
                 redProcessToSource.cmd(),
-                lifter.createSetHeightCommand(LifterState.Intake))));
+                lifter.createSetHeightCommand(LifterState.CoralIntake))));
     // spotless:on
 
     return redL4AlgAutoRoutine;

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

@@ -16,6 +16,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -79,11 +80,11 @@ public AlgaeWrist() {
 
   @Override
   public void periodic() {
-    SmartDashboard.putNumber("Algae Wrist/Current Angle Degrees", getCurrentAngleDegrees());
+    SmartDashboard.putNumber("Algae Wrist/Current Angle Degrees", getCurrentAngle().in(Degrees));
     SmartDashboard.putNumber("Algae Wrist/Current Angle Radians", encoder.getPosition());
     SmartDashboard.putNumber("Algae Wrist/Current Angular Velocity RPS", encoder.getVelocity());
     SmartDashboard.putString("Algae Wrist/Target State", getTargetState().name());
-    SmartDashboard.putNumber("Algae Wrist/Setpoint Angle Degrees", getSetpointAngleDegrees());
+    SmartDashboard.putNumber("Algae Wrist/Setpoint Angle Degrees", getSetpointAngle().in(Degrees));
     SmartDashboard.putNumber("Algae Wrist/Setpoint Angle Radians", feedback.getSetpoint().position);
     SmartDashboard.putNumber(
         "Algae Wrist/Setpoint Angular Velocity RPS", feedback.getSetpoint().velocity);
@@ -92,12 +93,12 @@ public void periodic() {
     SmartDashboard.putBoolean("Algae Wrist/At Goal", feedback.atGoal());
   }
 
-  private double getCurrentAngleDegrees() {
-    return Radians.of(encoder.getPosition()).in(Degrees);
+  private Angle getCurrentAngle() {
+    return Radians.of(encoder.getPosition());
   }
 
-  private double getSetpointAngleDegrees() {
-    return Radians.of(feedback.getSetpoint().position).in(Degrees);
+  private Angle getSetpointAngle() {
+    return Radians.of(feedback.getSetpoint().position);
   }
 
   public void resetController() {
@@ -139,7 +140,10 @@ public Command createRemainAtCurrentAngleCommand() {
     return new FunctionalCommand(
         // initialize
         () -> {
-          if (targetState == AlgaeWristState.Unknown) feedback.setGoal(encoder.getPosition());
+          if (targetState == AlgaeWristState.Unknown) {
+            targetState = AlgaeWristState.CoralMode;
+            feedback.setGoal(targetState.angle.in(Radians));
+          }
         },
         // execute
         () -> control(),

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

@@ -16,11 +16,13 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.XboxController;
 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.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.RobotConstants;
 import frc.robot.elevator.ElevatorConstants.CoralWristConstants;
 import frc.robot.elevator.ElevatorConstants.CoralWristConstants.CoralWristState;
@@ -81,11 +83,11 @@ public CoralWrist() {
 
   @Override
   public void periodic() {
-    SmartDashboard.putNumber("Coral Wrist/Current Angle Degrees", getCurrentAngleDegrees());
+    SmartDashboard.putNumber("Coral Wrist/Current Angle Degrees", getCurrentAngle().in(Degrees));
     SmartDashboard.putNumber("Coral Wrist/Current Angle Radians", encoder.getPosition());
     SmartDashboard.putNumber("Coral Wrist/Current Angular Velocity RPS", encoder.getVelocity());
     SmartDashboard.putString("Coral Wrist/Target State", getTargetState().name());
-    SmartDashboard.putNumber("Coral Wrist/Setpoint Angle Degrees", getSetpointAngleDegrees());
+    SmartDashboard.putNumber("Coral Wrist/Setpoint Angle Degrees", getSetpointAngle().in(Degrees));
     SmartDashboard.putNumber("Coral Wrist/Setpoint Angle Radians", feedback.getSetpoint().position);
     SmartDashboard.putNumber(
         "Coral Wrist/Setpoint Angular Velocity RPS", feedback.getSetpoint().velocity);
@@ -94,12 +96,12 @@ public void periodic() {
     SmartDashboard.putBoolean("Coral Wrist/At Goal", feedback.atGoal());
   }
 
-  private double getCurrentAngleDegrees() {
-    return Radians.of(encoder.getPosition()).in(Degrees);
+  private Angle getCurrentAngle() {
+    return Radians.of(encoder.getPosition());
   }
 
-  private double getSetpointAngleDegrees() {
-    return Radians.of(feedback.getSetpoint().position).in(Degrees);
+  private Angle getSetpointAngle() {
+    return Radians.of(feedback.getSetpoint().position);
   }
 
   public void resetController() {
@@ -120,6 +122,14 @@ public CoralWristState getTargetState() {
     return this.targetState;
   }
 
+  public Boolean inState(CoralWristState state) {
+    return this.targetState.equals(state);
+  }
+
+  public Trigger atRiskOfDamage =
+      new Trigger(
+          () -> getCurrentAngle().gt(CoralWristState.AlgaeMode.angle.plus(Degrees.of(3.0))));
+
   public Command createSetAngleCommand(CoralWristState state) {
     return new FunctionalCommand(
         // initialize

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

@@ -44,35 +44,35 @@ public void resetPositionControllers() {
 
   public Command coralL4PositionCG() {
     return Commands.parallel(
-        lifter.createSetHeightCommand(LifterState.L4),
+        lifter.createSetHeightCommand(LifterState.CoralL4),
         coralWrist.createSetAngleCommand(CoralWristState.L4),
         algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode));
   }
 
   public Command coralL3PositionCG() {
     return Commands.parallel(
-        lifter.createSetHeightCommand(LifterState.L3),
+        lifter.createSetHeightCommand(LifterState.CoralL3),
         coralWrist.createSetAngleCommand(CoralWristState.L3),
         algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode));
   }
 
   public Command coralL2PositionCG() {
     return Commands.parallel(
-        lifter.createSetHeightCommand(LifterState.L2),
+        lifter.createSetHeightCommand(LifterState.CoralL2),
         coralWrist.createSetAngleCommand(CoralWristState.L2),
         algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode));
   }
 
   public Command coralL1PositionCG() {
     return Commands.parallel(
-        lifter.createSetHeightCommand(LifterState.L1),
+        lifter.createSetHeightCommand(LifterState.CoralL1),
         coralWrist.createSetAngleCommand(CoralWristState.L1),
         algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode));
   }
 
   public Command coralIntakeCG() {
     return Commands.parallel(
-        lifter.createSetHeightCommand(LifterState.Intake),
+        lifter.createSetHeightCommand(LifterState.CoralIntake),
         coralWrist.createSetAngleCommand(CoralWristState.Intake),
         algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode),
         coralRoller.createIntakeCommand().until(coralRoller.hasCoral));
@@ -103,14 +103,14 @@ public Command algaeL2IntakeCG() {
 
   public Command algaeProcessorPositionCG() {
     return Commands.parallel(
-        lifter.createSetHeightCommand(LifterState.Processor),
+        lifter.createSetHeightCommand(LifterState.AlgaeProcessor),
         coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
         algaeWrist.createSetAngleCommand(AlgaeWristState.Processor));
   }
 
   public Command algaeFloorIntakeCG() {
     return Commands.parallel(
-        lifter.createSetHeightCommand(LifterState.Floor),
+        lifter.createSetHeightCommand(LifterState.AlgaeIntakeFloor),
         coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
         algaeWrist.createSetAngleCommand(AlgaeWristState.Floor),
         algaeRoller.createIntakeCommand().until(algaeRoller.hasAlage));

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

@@ -31,7 +31,7 @@ public static final class LifterConstants {
     public static final double kVelocityConversionFactor = kPositionConversionFactor / 60.0;
 
     public static final LinearVelocity kFineVelocity = InchesPerSecond.of(15.0);
-    public static final LinearVelocity kRapidVelocity = InchesPerSecond.of(50.0);
+    public static final LinearVelocity kRapidVelocity = InchesPerSecond.of(15.0);
     public static final LinearAccelerationUnit inchesPerSecondPerSecond =
         InchesPerSecond.per(Second);
     public static final LinearAcceleration kRapidAcceleration = inchesPerSecondPerSecond.of(1000);
@@ -48,17 +48,17 @@ public final class LifterController {
     public static enum LifterState {
       Unknown(0.0),
       Min(0.0),
-      Floor(0.0),
-      Reset(0.36),
-      L1(18.0),
-      L2(21.0),
-      L3(24.0),
-      L4(27.0),
-      Intake(12.0),
-      Processor(12.0),
+      AlgaeIntakeFloor(0.0),
+      EncoderReset(0.36),
+      CoralL1(18.0),
+      CoralL2(21.0),
+      CoralL3(24.0),
+      CoralL4(27.0),
+      CoralIntake(12.0),
+      AlgaeProcessor(12.0),
       AlgaeL2(30),
       AlgaeL3(46.5),
-      Max(52.0);
+      Max(67.8);
 
       public Distance height;
 
@@ -130,15 +130,15 @@ public static final class CoralWristConstants {
     public static final Angle kIZone = Degrees.of(30.0);
 
     public static enum CoralWristState {
-      Unknown(125),
+      Unknown(90),
       Min(15),
       Max(135),
       L1(90),
       L2(55),
       L3(55),
       L4(25),
       Intake(125),
-      AlgaeMode(125);
+      AlgaeMode(90);
 
       public Angle angle;
 

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

@@ -68,7 +68,7 @@ public Lifter() {
         .velocityConversionFactor(LifterConstants.kVelocityConversionFactor);
 
     leaderConfig.softLimit
-        .reverseSoftLimit(LifterState.Floor.height.in(Inches))
+        .reverseSoftLimit(LifterState.Min.height.in(Inches))
         .reverseSoftLimitEnabled(true)
         .forwardSoftLimit(LifterState.Max.height.in(Inches))
         .forwardSoftLimitEnabled(true);
@@ -138,12 +138,12 @@ public Boolean inState(LifterState state) {
     return this.targetState.equals(state);
   }
 
-  public Trigger atIntakingHeight = new Trigger(() -> inState(LifterState.Intake));
-  public Trigger impendingCoralGripperDamage =
-      new Trigger(() -> getHeight().gt(LifterState.L3.height.plus(Inches.of(2.0))));
+  public Trigger atIntakingHeight = new Trigger(() -> inState(LifterState.CoralIntake));
+  public Trigger tooHighForCoralWrist =
+      new Trigger(() -> getHeight().gt(LifterState.CoralL3.height.plus(Inches.of(2.0))));
 
   private void resetEncoder() {
-    encoder.setPosition(LifterState.Reset.height.in(Inches));
+    encoder.setPosition(LifterState.EncoderReset.height.in(Inches));
   }
 
   public Command createSetHeightCommand(LifterState state) {