Switch drivebase from openloop voltage to velocity control.

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

@@ -37,6 +37,7 @@
 import frc.robot.util.BranchHeight;
 import frc.robot.util.RobotType;
 import frc.robot.util.ScoringMode;
+import frc.robot.util.ScoringType;
 import frc.robot.util.SoloScoringMode;
 import java.util.function.BooleanSupplier;
 
@@ -83,8 +84,7 @@ public class Controls {
       new SwerveRequest.FieldCentric()
           .withDeadband(0.0001)
           .withRotationalDeadband(0.0001)
-          .withDriveRequestType(
-              DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
+          .withDriveRequestType(DriveRequestType.Velocity);
 
   private final Telemetry logger = new Telemetry(MaxSpeed);
 
@@ -112,6 +112,14 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) {
     configureGroundSpinnyBindings();
     configureGroundArmBindings();
     configureSoloControllerBindings();
+    Shuffleboard.getTab("Elevator")
+        .addBoolean("Intaking mode Algae", () -> intakeMode == ScoringMode.ALGAE);
+    Shuffleboard.getTab("Elevator")
+        .addString("Scoring Mode", () -> getSoloScoringMode().toString());
+  }
+
+  public SoloScoringMode getSoloScoringMode() {
+    return soloScoringMode;
   }
 
   private Trigger connected(CommandXboxController controller) {
@@ -287,7 +295,7 @@ private void configureSuperStructureBindings() {
             Commands.deferredProxy(
                     () ->
                         switch (scoringMode) {
-                          case CORAL -> getCoralBranchHeightCommand();
+                          case CORAL -> getCoralBranchHeightCommand(ScoringType.DRIVER);
                           case ALGAE -> Commands.sequence(
                                   superStructure.algaeProcessorScore(
                                       driverController.rightBumper()),
@@ -394,12 +402,7 @@ private void configureSuperStructureBindings() {
                           case ALGAE -> soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW;
                         }
                       })
-                  .withName("Set solo scoring mode"));
-
-      sensors
-          .armSensor
-          .inClaw()
-          .and(RobotModeTriggers.teleop())
+                  .withName("Set solo scoring mode"))
           .onFalse(
               Commands.runOnce(
                       () -> {
@@ -423,7 +426,7 @@ private void configureSuperStructureBindings() {
                     () -> {
                       Command scoreCommand =
                           switch (scoringMode) {
-                            case CORAL -> getCoralBranchHeightCommand();
+                            case CORAL -> getCoralBranchHeightCommand(ScoringType.DRIVER);
                             case ALGAE -> Commands.sequence(
                                     BargeAlign.bargeScore(
                                         s.drivebaseSubsystem,
@@ -448,30 +451,55 @@ private Command getAlgaeIntakeCommand() {
     };
   }
 
-  private Command getCoralBranchHeightCommand() {
-    return switch (branchHeight) {
-      case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(driverController.rightBumper());
-      case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(driverController.rightBumper());
-      case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(driverController.rightBumper());
-      case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(driverController.rightBumper());
-    };
-  }
-
-  private Command getSoloCoralBranchHeightCommand() {
-    return switch (branchHeight) {
-      case CORAL_LEVEL_FOUR -> superStructure
-          .coralLevelFour(soloController.rightBumper())
-          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
-      case CORAL_LEVEL_THREE -> superStructure
-          .coralLevelThree(soloController.rightBumper())
-          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
-      case CORAL_LEVEL_TWO -> superStructure
-          .coralLevelTwo(soloController.rightBumper())
-          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
-      case CORAL_LEVEL_ONE -> superStructure
-          .coralLevelOne(soloController.rightBumper())
-          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
-    };
+  private Command getCoralBranchHeightCommand(ScoringType version) {
+    if (version == ScoringType.SOLOC_LEFT) {
+      return switch (branchHeight) {
+        case CORAL_LEVEL_FOUR -> superStructure
+            .coralLevelFour(soloController.rightBumper())
+            .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+        case CORAL_LEVEL_THREE -> superStructure
+            .coralLevelThree(
+                soloController.rightBumper(),
+                () -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB))
+            .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+        case CORAL_LEVEL_TWO -> superStructure
+            .coralLevelTwo(
+                soloController.rightBumper(),
+                () -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB))
+            .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+        case CORAL_LEVEL_ONE -> superStructure
+            .coralLevelOne(
+                soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1LB))
+            .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+      };
+    } else if (version == ScoringType.SOLOC_RIGHT) {
+      return switch (branchHeight) {
+        case CORAL_LEVEL_FOUR -> superStructure
+            .coralLevelFour(soloController.rightBumper())
+            .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+        case CORAL_LEVEL_THREE -> superStructure
+            .coralLevelThree(
+                soloController.rightBumper(),
+                () -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB))
+            .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+        case CORAL_LEVEL_TWO -> superStructure
+            .coralLevelTwo(
+                soloController.rightBumper(),
+                () -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB))
+            .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+        case CORAL_LEVEL_ONE -> superStructure
+            .coralLevelOne(
+                soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1RB))
+            .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+      };
+    } else {
+      return switch (branchHeight) {
+        case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(driverController.rightBumper());
+        case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(driverController.rightBumper());
+        case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(driverController.rightBumper());
+        case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(driverController.rightBumper());
+      };
+    }
   }
 
   private void configureElevatorBindings() {
@@ -806,12 +834,12 @@ private void configureAutoAlignBindings() {
         .rightTrigger()
         .and(() -> scoringMode == ScoringMode.CORAL)
         .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
-        .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this));
+        .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB));
     driverController
         .leftTrigger()
         .and(() -> scoringMode == ScoringMode.CORAL)
         .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
-        .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this));
+        .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB));
   }
 
   private void configureGroundSpinnyBindings() {
@@ -926,7 +954,7 @@ private void configureSoloControllerBindings() {
                   switch (soloScoringMode) {
                     case CORAL_IN_CLAW -> {
                       scoreCommand =
-                          getSoloCoralBranchHeightCommand()
+                          getCoralBranchHeightCommand(ScoringType.SOLOC_LEFT)
                               .until(
                                   () ->
                                       soloController.a().getAsBoolean()
@@ -968,7 +996,12 @@ private void configureSoloControllerBindings() {
         .leftTrigger()
         .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
         .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
-        .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this));
+        .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB));
+    soloController
+        .leftTrigger()
+        .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
+        .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE)
+        .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1LB));
     // Processor + Auto align right + Select scoring mode Coral
     soloController
         .rightTrigger()
@@ -977,7 +1010,8 @@ private void configureSoloControllerBindings() {
                     () -> {
                       Command scoreCommand =
                           switch (soloScoringMode) {
-                            case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand()
+                            case CORAL_IN_CLAW -> getCoralBranchHeightCommand(
+                                    ScoringType.SOLOC_RIGHT)
                                 .until(
                                     () ->
                                         soloController.a().getAsBoolean()
@@ -1005,7 +1039,12 @@ private void configureSoloControllerBindings() {
         .rightTrigger()
         .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
         .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
-        .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this));
+        .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB));
+    soloController
+        .rightTrigger()
+        .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
+        .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE)
+        .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1RB));
     // Ground Intake
     soloController
         .leftBumper()
@@ -1095,11 +1134,5 @@ private void configureSoloControllerBindings() {
                 .startMovingVoltage(
                     () -> Volts.of(ElevatorSubsystem.UP_VOLTAGE * -soloController.getLeftY()))
                 .withName("Elevator Manual Control"));
-    // Ready to score rumble
-    Trigger readyToScore = new Trigger(() -> AutoAlign.poseInPlace());
-    readyToScore.onTrue(
-        Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 1.0)));
-    readyToScore.onFalse(
-        Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0)));
   }
 }

src/main/java/frc/robot/generated/CompTunerConstants.java

@@ -54,7 +54,7 @@ public class CompTunerConstants {
   // When using closed-loop control, the drive motor uses the control
   // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
   private static final Slot0Configs driveGains =
-      new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124);
+      new Slot0Configs().withKP(0.2).withKI(0).withKD(0).withKS(0.1).withKV(0.124).withKA(0);
 
   // The closed-loop output type to use for the steer motors;
   // This affects the PID/FF gains for the steer motors
@@ -80,7 +80,14 @@ public class CompTunerConstants {
 
   // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
   // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
-  private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
+  private static final TalonFXConfiguration driveInitialConfigs =
+      new TalonFXConfiguration()
+          .withCurrentLimits(
+              new CurrentLimitsConfigs()
+                  .withStatorCurrentLimit(Amps.of(160))
+                  .withStatorCurrentLimitEnable(true)
+                  .withSupplyCurrentLimit(Amps.of(100))
+                  .withSupplyCurrentLimitEnable(true));
   private static final TalonFXConfiguration steerInitialConfigs =
       new TalonFXConfiguration()
           .withCurrentLimits(
@@ -89,7 +96,9 @@ public class CompTunerConstants {
                   // low
                   // stator current limit to help avoid brownouts without impacting performance.
                   .withStatorCurrentLimit(Amps.of(60))
-                  .withStatorCurrentLimitEnable(true));
+                  .withStatorCurrentLimitEnable(true)
+                  .withSupplyCurrentLimit(Amps.of(40))
+                  .withSupplyCurrentLimitEnable(true));
   private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
   // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
   private static final Pigeon2Configuration pigeonConfigs = null;

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

@@ -67,6 +67,19 @@ private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score)
     }
   }
 
+  private Command repeatPrescoreScoreSwing(
+      Command command, BooleanSupplier score, BooleanSupplier ipScore) {
+    // repeats scoring sequence if the coral is still in the claw
+    if (armSensor == null) {
+      return Commands.sequence(
+          command,
+          Commands.waitUntil(() -> !score.getAsBoolean()),
+          Commands.waitUntil(ipScore).until(score));
+    } else {
+      return command.repeatedly().onlyWhile(armSensor.inClaw());
+    }
+  }
+
   public Command coralLevelFour(BooleanSupplier score) {
     if (branchSensors != null) { // checks if sensor enabled then use for faster scoring
       score = branchSensors.withinScoreRange().or(score);
@@ -168,6 +181,85 @@ public Command coralLevelOne(BooleanSupplier score) {
         .withName("Coral Level 1");
   }
 
+  // New versions with ipScore
+  // if robot is in position for intermediate scoring, it can score early but if vision is dead
+  // manual control still works
+  // only used on solo controls
+
+  public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { // same as L4
+    return Commands.sequence(
+            Commands.parallel(
+                    elevator
+                        .setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS)
+                        .deadlineFor(
+                            armPivot
+                                .moveToPosition(ArmPivot.CORAL_PRESET_UP)
+                                .until(ipScore)
+                                .until(score)),
+                    spinnyClaw.stop())
+                .withTimeout(0.5),
+            repeatPrescoreScoreSwing(
+                Commands.repeatingSequence(
+                    armPivot
+                        .moveToPosition(ArmPivot.CORAL_PRESET_L3)
+                        .withDeadline(Commands.waitUntil(ipScore).until(score)),
+                    armPivot
+                        .moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
+                        .withTimeout(1.5)
+                        .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
+                score,
+                ipScore),
+            coralPreIntake())
+        .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy())
+        .withName("Coral Level 3");
+  }
+
+  public Command coralLevelTwo(
+      BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 and L3
+    return Commands.sequence(
+            Commands.parallel(
+                    elevator
+                        .setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS)
+                        .deadlineFor(
+                            armPivot
+                                .moveToPosition(ArmPivot.CORAL_PRESET_UP)
+                                .until(ipScore)
+                                .until(score)),
+                    spinnyClaw.stop())
+                .withTimeout(0.5),
+            repeatPrescoreScoreSwing(
+                Commands.sequence(
+                    armPivot
+                        .moveToPosition(ArmPivot.CORAL_PRESET_L2)
+                        .withDeadline(Commands.waitUntil(ipScore).until(score)),
+                    armPivot
+                        .moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
+                        .withTimeout(1.5)
+                        .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
+                score,
+                ipScore),
+            coralPreIntake())
+        .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy())
+        .withName("Coral Level 2");
+  }
+
+  public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) {
+    return Commands.sequence(
+            Commands.parallel(
+                    elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS),
+                    armPivot.moveToPosition(ArmPivot.CORAL_PRESET_L1),
+                    spinnyClaw.stop()) // holds coral without wearing flywheels
+                .withTimeout(0.5)
+                .withDeadline(Commands.waitUntil(ipScore).until(score)),
+            spinnyClaw
+                .coralLevelOneHoldExtakePower()
+                .withTimeout(0.5 /* this time could be shorter */), // spits out coral
+            Commands.waitSeconds(1), // Wait to clear the reef
+            coralPreIntake())
+        .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy())
+        .withName("Coral Level 1");
+  }
+
   // quickGroundIntake() is used instead since it's faster and still reliable.
   // (This one moves the coral intake the normal intake position, then does the normal intake.
   // quickGroundIntake() instead hands the coral directly to the claw.)
@@ -216,11 +308,11 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph
       // Make the big sequence.
       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
+              // arm pivot, and start spinning the ground intake
               Commands.parallel(
                       elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE),
                       armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE),
-                      spinnyClaw.stop(), // just as a backup in case things are silly
+                      spinnyClaw.coralIntakePower(),
                       groundSpinny.setGroundIntakePower())
                   // Move on even if arm isn't in position yet as long as elevator is high enough
                   .until(elevator.above(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE)),