Fix orient modules command (#206)

* Fix orient modules command * End drive characterization at end of wheel radius characterization --------- Co-authored-by: Jonah <47046556+jwbonner@users.noreply.github.com>
Mechanical-Advantage · Mar 27, 2024 · 7372995 · 7372995
1 parent 46d1af7
commit 7372995
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 7 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/WheelRadiusCharacterization.java b/src/main/java/org/littletonrobotics/frc2024/commands/WheelRadiusCharacterization.java
@@ -88,6 +88,7 @@ public void execute() {
 
   @Override
   public void end(boolean interrupted) {
+    drive.endCharacterization();
     if (accumGyroYawRads <= Math.PI * 2.0) {
       System.out.println("Not enough data for characterization");
     } else {

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java
@@ -254,16 +254,16 @@ public void periodic() {
       default -> {}
     }
 
-    // Run robot at desiredSpeeds
-    // Generate feasible next setpoint
-    currentSetpoint =
-        setpointGenerator.generateSetpoint(
-            currentModuleLimits, currentSetpoint, desiredSpeeds, Constants.loopPeriodSecs);
-
-    // run modules
+    // Run modules
     if (currentDriveMode != DriveMode.CHARACTERIZATION && !modulesOrienting) {
+      // Run robot at desiredSpeeds
+      // Generate feasible next setpoint
+      currentSetpoint =
+          setpointGenerator.generateSetpoint(
+              currentModuleLimits, currentSetpoint, desiredSpeeds, Constants.loopPeriodSecs);
       SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4];
       SwerveModuleState[] optimizedSetpointTorques = new SwerveModuleState[4];
+
       for (int i = 0; i < modules.length; i++) {
         // Optimize setpoints
         optimizedSetpointStates[i] =
@@ -415,11 +415,14 @@ public Command orientModules(Rotation2d orientation) {
    */
   public Command orientModules(Rotation2d[] orientations) {
     return run(() -> {
+          SwerveModuleState[] states = new SwerveModuleState[4];
           for (int i = 0; i < orientations.length; i++) {
             modules[i].runSetpoint(
                 new SwerveModuleState(0.0, orientations[i]),
                 new SwerveModuleState(0.0, new Rotation2d()));
+            states[i] = new SwerveModuleState(0.0, modules[i].getAngle());
           }
+          currentSetpoint = new SwerveSetpoint(new ChassisSpeeds(), states);
         })
         .until(
             () ->