Moved swerve module desired state telemetry into SwerveModule. Adde…

…d telemetry reporting of cosine compensation. Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
BroncBotz3481 · Jan 26, 2024 · c5aac6f · c5aac6f
1 parent 1714e62
commit c5aac6f
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 29 deletions.
diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
@@ -543,14 +543,6 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpe
     for (SwerveModule module : swerveModules)
     {
       module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false);
-
-      if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
-      {
-        SwerveDriveTelemetry.desiredStates[module.moduleNumber *
-                                           2] = module.lastState.angle.getDegrees();
-        SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) +
-                                           1] = module.lastState.speedMetersPerSecond;
-      }
     }
   }
 

diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java
@@ -56,7 +56,7 @@ public class SwerveModule
   /**
    * Last swerve module state applied.
    */
-  public        SwerveModuleState      lastState;
+  private       SwerveModuleState      lastState;
   /**
    * Angle offset from the absolute encoder.
    */
@@ -197,39 +197,23 @@ public void queueSynchronizeEncoders()
   public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
   {
     desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition()));
+    // Cosine compensation.
+    double velocity = configuration.useCosineCompensator ? getCosineCompensatedVelocity(desiredState)
+                                                         : desiredState.speedMetersPerSecond;
 
     if (isOpenLoop)
     {
       double percentOutput = desiredState.speedMetersPerSecond / maxSpeed;
       driveMotor.set(percentOutput);
     } else
     {
-      double cosineScalar = 1.0;
-      if (configuration.useCosineCompensator)
-      {
-        // Taken from the CTRE SwerveModule class.
-        // https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46
-        /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */
-        /* To reduce the "skew" that occurs when changing direction */
-        double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition();
-        /* If error is close to 0 rotations, we're already there, so apply full power */
-        /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */
-        cosineScalar = Math.cos(Units.degreesToRadians(steerMotorError));
-        /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */
-        if (cosineScalar < 0.0)
-        {
-          cosineScalar = 0.0;
-        }
-      }
-
-      double velocity = desiredState.speedMetersPerSecond * (cosineScalar);
       driveMotor.setReference(velocity, feedforward.calculate(velocity));
     }
 
     // If we are forcing the angle
     if (!force)
     {
-    // Prevents module rotation if speed is less than 1%
+      // Prevents module rotation if speed is less than 1%
       SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4));
     }
 
@@ -253,13 +237,45 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
       simModule.updateStateAndPosition(desiredState);
     }
 
+    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
+    {
+      SwerveDriveTelemetry.desiredStates[moduleNumber * 2] = desiredState.angle.getDegrees();
+      SwerveDriveTelemetry.desiredStates[(moduleNumber * 2) + 1] = velocity;
+    }
+
     if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
     {
       SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint", desiredState.speedMetersPerSecond);
       SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint", desiredState.angle.getDegrees());
     }
   }
 
+  /**
+   * Get the cosine compensated velocity to set the swerve module to.
+   *
+   * @param desiredState Desired {@link SwerveModuleState} to use.
+   * @return Cosine compensated velocity in meters/second.
+   */
+  private double getCosineCompensatedVelocity(SwerveModuleState desiredState)
+  {
+    double cosineScalar = 1.0;
+    // Taken from the CTRE SwerveModule class.
+    // https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46
+    /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */
+    /* To reduce the "skew" that occurs when changing direction */
+    double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition();
+    /* If error is close to 0 rotations, we're already there, so apply full power */
+    /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */
+    cosineScalar = Math.cos(Units.degreesToRadians(steerMotorError));
+    /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */
+    if (cosineScalar < 0.0)
+    {
+      cosineScalar = 0.0;
+    }
+
+    return desiredState.speedMetersPerSecond * (cosineScalar);
+  }
+
   /**
    * Set the angle for the module.
    *