Supercycling implementation

.vscode/settings.json

@@ -57,5 +57,6 @@
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
   ],
-  "editor.tabSize": 2
+  "editor.tabSize": 2,
+  "wpilib.autoStartRioLog": false
 }

src/main/java/frc/robot/Controls.java

@@ -272,6 +272,8 @@ private void configureSuperStructureBindings() {
         .onTrue(
             selectScoringHeight(BranchHeight.CORAL_LEVEL_ONE, AlgaeIntakeHeight.ALGAE_LEVEL_GROUND)
                 .withName("coral level 1, algae ground level"));
+
+    // Processor scoring and left coral pre-score
     driverController
         .leftTrigger()
         .onTrue(
@@ -283,11 +285,24 @@ private void configureSuperStructureBindings() {
                                   superStructure.algaeProcessorScore(
                                       driverController.rightBumper()),
                                   Commands.waitSeconds(0.7),
-                                  getAlgaeIntakeCommand())
+                                  Commands.deferredProxy(
+                                      () ->
+                                          switch (sensors.intakeSensor.inIntakeSwitchable()) {
+                                            case 1 -> Commands.sequence(
+                                                Commands.runOnce(
+                                                        () -> scoringMode = ScoringMode.CORAL)
+                                                    .alongWith(scoringModeSelectRumble())
+                                                    .withName("Coral Scoring Mode"),
+                                                superStructure
+                                                    .superCycleCoralHandoff()
+                                                    .withName("supercycle handoff"));
+                                            default -> getAlgaeIntakeCommand();
+                                          }))
                               .withName("Processor score");
                         })
                 .withName("Schedule processor score"));
 
+    // Algae mode
     operatorController
         .leftBumper()
         .onTrue(
@@ -297,14 +312,22 @@ private void configureSuperStructureBindings() {
         .onTrue(
             Commands.runOnce(() -> CommandScheduler.getInstance().schedule(getAlgaeIntakeCommand()))
                 .withName("run algae intake"));
+
+    // Coral Mode
     operatorController // should work???
         .leftTrigger()
         .onTrue(
             Commands.runOnce(() -> scoringMode = ScoringMode.CORAL)
                 .alongWith(scoringModeSelectRumble())
                 .withName("Coral Scoring Mode"))
         .onTrue(superStructure.coralPreIntake())
-        .onTrue(s.climbPivotSubsystem.toStow());
+        .onTrue(s.climbPivotSubsystem.toStow())
+        .onTrue(
+            s.groundArm
+                .moveToPosition(GroundArm.STOWED_POSITION)
+                .withName("Ground stowed position"));
+
+    // Stow Mode
     operatorController
         .povLeft()
         .onTrue(
@@ -371,6 +394,8 @@ private void configureSuperStructureBindings() {
                           : Commands.none())
                   .withName("Automatic Intake"));
     }
+
+    // Net scoring and right coral pre-score
     driverController
         .rightTrigger()
         .onTrue(
@@ -387,7 +412,21 @@ private void configureSuperStructureBindings() {
                                         () -> getDriveY(),
                                         () -> getDriveRotate(),
                                         driverController.rightBumper()),
-                                    getAlgaeIntakeCommand())
+                                    // If we are supercycling (have a coral), hand it off and switch
+                                    // to coral mode
+                                    Commands.deferredProxy(
+                                        () ->
+                                            switch (sensors.intakeSensor.inIntakeSwitchable()) {
+                                              case 1 -> Commands.sequence(
+                                                  Commands.runOnce(
+                                                          () -> scoringMode = ScoringMode.CORAL)
+                                                      .alongWith(scoringModeSelectRumble())
+                                                      .withName("Coral Scoring Mode"),
+                                                  superStructure
+                                                      .superCycleCoralHandoff()
+                                                      .withName("supercycle handoff"));
+                                              default -> getAlgaeIntakeCommand();
+                                            }))
                                 .withName("Algae score then intake");
                           };
                       CommandScheduler.getInstance().schedule(scoreCommand);

src/main/java/frc/robot/sensors/IntakeSensor.java

@@ -59,4 +59,9 @@ public Trigger inIntake() {
             })
         .debounce(0.1);
   }
+
+  public int inIntakeSwitchable() {
+    double distance = getSensorDistance().in(Meters);
+    return (distance > INTAKE_LOWER_LIMIT && distance < INTAKE_UPPER_LIMIT) ? 1 : 0;
+  }
 }

src/main/java/frc/robot/subsystems/ClimbPivot.java

@@ -56,6 +56,9 @@ public enum TargetPositions {
   private final double CLIMB_HOLD_CLIMBED = -0.0705;
   private final double CLIMB_IN_SPEED = -0.75;
 
+  private final double climbInKp = 30;
+  private final double climbOutKp = 50;
+
   // positions for relation between motor encoder and WCP encoder
   // relative to eachother, likely not accurately zero'ed when obtained.
   private static final double MIN_ROTOR_POSITION = -50.45;
@@ -83,6 +86,8 @@ public enum TargetPositions {
 
   // the target speed
   private double setSpeed = 0;
+  private double speedToSet = 0;
+  private double pError = 0;
 
   // alerts for checking if either motor is or isnt connected
   private final Alert NotConnectedErrorOne =
@@ -390,12 +395,15 @@ public Command advanceClimbCheck() {
             setSpeed = 0;
           } else {
             if (!moveComplete) {
-              if (minTargetPos > getClimbPosition()) {
-                motorLeft.set(CLIMB_OUT_SPEED);
-                setSpeed = CLIMB_OUT_SPEED;
+              pError = minTargetPos - getClimbPosition();
+              if (pError > 0) {
+                speedToSet = CLIMB_OUT_SPEED * pError * climbOutKp;
+                motorLeft.set(speedToSet);
+                setSpeed = speedToSet;
               } else {
-                motorLeft.set(CLIMB_IN_SPEED);
-                setSpeed = CLIMB_IN_SPEED;
+                speedToSet = CLIMB_IN_SPEED * -pError * climbInKp;
+                motorLeft.set(speedToSet);
+                setSpeed = speedToSet;
               }
             }
           }

src/main/java/frc/robot/subsystems/GroundArm.java

@@ -26,8 +26,9 @@ public class GroundArm extends SubsystemBase {
   private final double ARMPIVOT_KG = 0.048;
   private final double ARMPIVOT_KA = 0;
   public static final double STOWED_POSITION = 0.45;
-  public static final double UP_POSITION =
-      0.27; // untested - should be somewhere in between stowed and ground
+  public static final double UP_POSITION = 0.25; // Tested and working for supercycling
+  public static final double MIN_OUT_QUICK_HANDOFF =
+      0.15; // Needs to be tested, used by supercycling
   public static final double GROUND_POSITION = -0.050;
   public static final double QUICK_INTAKE_POSITION = 0.31;
   public static final double POS_TOLERANCE = Units.degreesToRotations(5);

src/main/java/frc/robot/subsystems/GroundSpinny.java

@@ -17,6 +17,7 @@ public class GroundSpinny extends SubsystemBase {
   public static final double FUNNEL_INTAKE_SPEED = -2;
   public static final double QUICK_HANDOFF_EXTAKE_SPEED = 1;
   private static final double STATOR_CURRENT_STALL_THRESHOLD = 20;
+  public static final double GROUND_INTAKE_HOLD_SPEED = -1;
 
   // TalonFX
   private final TalonFX motor;
@@ -80,6 +81,10 @@ public Command holdFunnelIntakePower() {
     return holdPower(FUNNEL_INTAKE_SPEED).withName("hold funnel intake power");
   }
 
+  public Command holdCoralPower() {
+    return holdPower(GROUND_INTAKE_HOLD_SPEED).withName("hold coral holding power");
+  }
+
   public void imperativeSetGroundIntakePower() {
     motor.setVoltage(GROUND_INTAKE_SPEED);
     lastSetPower = GROUND_INTAKE_SPEED;

src/main/java/frc/robot/subsystems/SuperStructure.java

@@ -203,6 +203,60 @@ public Command groundIntake(BooleanSupplier retract) {
     }
   }
 
+  public Command supercycleGroundIntake(BooleanSupplier retract) {
+    return // Core intake sequence
+    Commands.sequence(
+            // Deploy the ground arm (and wait until it reaches the position).
+            groundArm.moveToPosition(GroundArm.GROUND_POSITION),
+            // Once it's out, set the ground spinny speed
+            groundSpinny.setGroundIntakePower(),
+            // After it's deployed, apply a constant voltage to press it into the bumper
+            // and continue.
+            groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE))
+
+        // Move on from the intake being down when stuff is triggered
+        .withDeadline(Commands.waitUntil(intakeSensor.inIntake().or(retract)))
+        // And bring it back inside the robot
+        .andThen(
+            Commands.sequence(
+                // Move ground intake to in positiong and hold it there
+                groundArm.moveToPosition(GroundArm.UP_POSITION).andThen(Commands.idle()),
+                // Set ground intake power to just hold the coral tightly
+                groundSpinny.holdCoralPower()));
+  }
+
+  // This is called when a coral is in the ground intake and the robot has just scored an algae
+  public Command superCycleCoralHandoff() {
+    return Commands.sequence(
+        // Initial setup- Move elevator high enough for ground arm to be clear, start moving
+        // arm pivot, stop the spinny claw, and start spinning the ground intake
+        Commands.parallel(
+                elevator.setLevel(ElevatorSubsystem.MIN_FULL_GROUND_INTAKE),
+                armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE),
+                spinnyClaw.stop(), // just as a backup in case things are silly
+                groundSpinny.setGroundIntakePower())
+            // Move on even if arm isn't in position yet as long as elevator is high enough
+            .until(elevator.above(ElevatorSubsystem.MIN_FULL_GROUND_INTAKE)),
+        // Move the ground intake out
+        groundArm.moveToPosition(GroundArm.MIN_OUT_QUICK_HANDOFF),
+        // Prep is done, now do the handoff
+        Commands.parallel(
+            // These three are the initial setup: Move elevator down to the handoff
+            // height, make sure armPivot finishes moving to the right height, and
+            // spin claw
+            elevator.setLevel(ElevatorSubsystem.CORAL_QUICK_INTAKE),
+            armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE),
+            spinnyClaw.coralIntakePower()),
+        // Start the sequence of handing off the coral
+        groundArm
+            .moveToPosition(GroundArm.STOWED_POSITION)
+            .until(groundArm.atPosition(GroundArm.QUICK_INTAKE_POSITION)),
+        // Spin groundSpinny out, but skip if we lost the coral.
+        groundSpinny.setQuickHandoffExtakeSpeed().onlyIf(armSensor.inClaw()),
+        // Go back to stow, but skip if we lost the coral.
+        coralStow().onlyIf(armSensor.inClaw()));
+  }
+
   // This is the actual version in use. It moves the coral directly into the claw.
   public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph
     if (groundSpinny == null || groundArm == null || intakeSensor == null) {
@@ -370,7 +424,8 @@ public Command algaeLevelThreeFourIntake() { // removes algae from upper reef
             Commands.parallel(
                 spinnyClaw.algaeIntakePower(),
                 armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE),
-                elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR)))
+                groundArm.moveToPosition(GroundArm.UP_POSITION).andThen(Commands.idle())),
+            elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR))
         .withName("Algae L3-L4 Intake");
   }
 
@@ -379,7 +434,8 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef
             Commands.parallel(
                 spinnyClaw.algaeIntakePower(),
                 armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE),
-                elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE)))
+                elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE)),
+            groundArm.moveToPosition(GroundArm.STOWED_POSITION).andThen(Commands.idle()))
         .withName("Algae L2-L3 Intake");
   }
 
@@ -388,7 +444,9 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef
     return Commands.parallel(
             spinnyClaw.algaeIntakePower(),
             Commands.sequence(
-                armPivot.moveToPosition(ArmPivot.ALGAE_GROUND_INTAKE),
+                Commands.parallel(
+                    armPivot.moveToPosition(ArmPivot.ALGAE_GROUND_INTAKE),
+                    groundArm.moveToPosition(GroundArm.STOWED_POSITION).andThen(Commands.idle())),
                 elevator.setLevel(ElevatorSubsystem.ALGAE_GROUND_INTAKE)))
         .withName("Algae Ground Intake");
   }
@@ -417,7 +475,7 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p
   public Command algaeNetScore(BooleanSupplier score) {
     return Commands.sequence(
             Commands.parallel(
-                elevator.setLevel(ElevatorSubsystem.ALGAE_NET_SCORE),
+                groundArm.moveToPosition(GroundArm.UP_POSITION).andThen(Commands.idle()),
                 armPivot.moveToPosition(ArmPivot.ALGAE_NET_SCORE),
                 spinnyClaw.algaeIntakePower()),
             Commands.waitUntil(score),