From 7372995e8603f7fa33b3496acbc9493d43dda0f2 Mon Sep 17 00:00:00 2001
From: Surya Thoppae <63197592+suryatho@users.noreply.github.com>
Date: Wed, 27 Mar 2024 18:37:12 -0400
Subject: [PATCH] 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>
---
 .../commands/WheelRadiusCharacterization.java   |  1 +
 .../frc2024/subsystems/drive/Drive.java         | 17 ++++++++++-------
 2 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/WheelRadiusCharacterization.java b/src/main/java/org/littletonrobotics/frc2024/commands/WheelRadiusCharacterization.java
index 6f4bda6a..0d859d81 100644
--- 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
index 9ae50db8..d1eaff4c 100644
--- 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(
             () ->