Vapor - Day Two Tweaks (#114)

* Reduce threshold for roller motion trigger, fix pose-seek logic

* Fix typo and logic error

* Tweaks

* Day 1 changes

* Spotless

* Day two tweaks

Author: caparomula

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

@@ -194,6 +194,8 @@ public void teleopInit() {
     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()));
@@ -330,15 +332,7 @@ public boolean getAsBoolean() {
   }
 
   private void configureEventBindings() {
-    // autoSelector.getChangedAutoSelection()
-    //     .onTrue(leds.createScrollingRainbowCommand()
-    //     .ignoringDisable(true)
-    //     .withTimeout(3))
-    //     ;
-
     coralRoller.isRolling.whileTrue(createRollerAnimationCommand());
-    // lifter.tooHighForCoralWrist.and(coralWrist.atRiskOfDamage)
-    // .onTrue(coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode));
   }
   // spotless:on
 

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

@@ -86,18 +86,20 @@ public Command algaeBargePositionCG() {
 
   public Command algaeL3IntakeCG() {
     return Commands.parallel(
-        lifter.createSetHeightCommand(LifterState.AlgaeL3),
-        coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
-        algaeWrist.createSetAngleCommand(AlgaeWristState.L3),
-        algaeRoller.createIntakeCommand().until(algaeRoller.hasAlgae));
+            lifter.createSetHeightCommand(LifterState.AlgaeL3),
+            coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
+            algaeWrist.createSetAngleCommand(AlgaeWristState.L3),
+            algaeRoller.createIntakeCommand().until(algaeRoller.hasAlgae))
+        .andThen(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
   }
 
   public Command algaeL2IntakeCG() {
     return Commands.parallel(
-        lifter.createSetHeightCommand(LifterState.AlgaeL2),
-        coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
-        algaeWrist.createSetAngleCommand(AlgaeWristState.L2),
-        algaeRoller.createIntakeCommand().until(algaeRoller.hasAlgae));
+            lifter.createSetHeightCommand(LifterState.AlgaeL2),
+            coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
+            algaeWrist.createSetAngleCommand(AlgaeWristState.L2),
+            algaeRoller.createIntakeCommand().until(algaeRoller.hasAlgae))
+        .andThen(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
   }
 
   public Command algaeProcessorPositionCG() {
@@ -109,9 +111,10 @@ public Command algaeProcessorPositionCG() {
 
   public Command algaeFloorIntakeCG() {
     return Commands.parallel(
-        lifter.createSetHeightCommand(LifterState.AlgaeIntakeFloor),
-        coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
-        algaeWrist.createSetAngleCommand(AlgaeWristState.Floor),
-        algaeRoller.createIntakeCommand().until(algaeRoller.hasAlgae));
+            lifter.createSetHeightCommand(LifterState.AlgaeIntakeFloor),
+            coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
+            algaeWrist.createSetAngleCommand(AlgaeWristState.Floor),
+            algaeRoller.createIntakeCommand().until(algaeRoller.hasAlgae))
+        .andThen(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
   }
 }

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

@@ -35,7 +35,7 @@ public static final class LifterConstants {
         kPositionConversionFactor.per(Minutes);
 
     public static final LinearVelocity kFineVelocity = InchesPerSecond.of(15.0);
-    public static final LinearVelocity kRapidVelocity = InchesPerSecond.of(40.0);
+    public static final LinearVelocity kRapidVelocity = InchesPerSecond.of(47.0);
     public static final LinearAccelerationUnit inchesPerSecondPerSecond =
         InchesPerSecond.per(Second);
     public static final LinearAcceleration kRapidAcceleration = inchesPerSecondPerSecond.of(1000);