Merge pull request #148 from FRC3620/frc3620

Frc3620

docs/json/swervemodule.md

@@ -17,6 +17,7 @@ This configuration file interacts directly with swerve kinematics.
 | absoluteEncoderOffset   | Degrees                     | Y        | Absolute encoder offset from 0 in degrees.                                |
 | absoluteEncoderInverted | Bool                        | N        | Inversion state of the Absolute Encoder.                                  |
 | location                | [MotorConfig](#MotorConfig) | Y        | The location of the swerve module from the center of the robot in inches. |
+| useCosineCompensator    | Bool                        | N        | Whether or not to modulate drive motors when pointed in wrong direction (defaults to True) |
 
 ### MotorConfig
 

src/main/java/swervelib/SwerveModule.java

@@ -204,18 +204,21 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
       driveMotor.set(percentOutput);
     } else
     {
-      // 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 */
-      double 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 || desiredState.speedMetersPerSecond == 0)
-      {
-        cosineScalar = 0.0;
+      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);

src/main/java/swervelib/parser/SwerveModuleConfiguration.java

@@ -62,6 +62,10 @@ public class SwerveModuleConfiguration
    * Name for the swerve module for telemetry.
    */
   public String                name;
+  /**
+   * Should do cosine compensation when not pointing correct direction;.
+   */
+  public boolean               useCosineCompensator;
 
   /**
    * Construct a configuration object for swerve modules.
@@ -80,6 +84,7 @@ public class SwerveModuleConfiguration
    * @param physicalCharacteristics Physical characteristics of the swerve module.
    * @param name                    The name for the swerve module.
    * @param conversionFactors       Conversion factors to be applied to the drive and angle motors.
+   * @param useCosineCompensator    Should use cosineCompensation.
    */
   public SwerveModuleConfiguration(
       SwerveMotor driveMotor,
@@ -95,7 +100,8 @@ public SwerveModuleConfiguration(
       boolean absoluteEncoderInverted,
       boolean driveMotorInverted,
       boolean angleMotorInverted,
-      String name)
+      String name,
+      boolean useCosineCompensator)
   {
     this.driveMotor = driveMotor;
     this.angleMotor = angleMotor;
@@ -110,6 +116,7 @@ public SwerveModuleConfiguration(
     this.velocityPIDF = velocityPIDF;
     this.physicalCharacteristics = physicalCharacteristics;
     this.name = name;
+    this.useCosineCompensator = useCosineCompensator;
   }
 
   /**
@@ -127,6 +134,7 @@ public SwerveModuleConfiguration(
    * @param velocityPIDF            Velocity PIDF configuration.
    * @param physicalCharacteristics Physical characteristics of the swerve module.
    * @param name                    Name for the module.
+   * @param useCosineCompensator    Should use cosineCompensation.
    */
   public SwerveModuleConfiguration(
       SwerveMotor driveMotor,
@@ -139,7 +147,8 @@ public SwerveModuleConfiguration(
       PIDFConfig anglePIDF,
       PIDFConfig velocityPIDF,
       SwerveModulePhysicalCharacteristics physicalCharacteristics,
-      String name)
+      String name,
+      boolean useCosineCompensator)
   {
     this(
         driveMotor,
@@ -155,7 +164,8 @@ public SwerveModuleConfiguration(
         false,
         false,
         false,
-        name);
+        name, 
+        useCosineCompensator);
   }
 
 

src/main/java/swervelib/parser/json/ModuleJson.java

@@ -53,6 +53,10 @@ public class ModuleJson
    * The location of the swerve module from the center of the robot in inches.
    */
   public LocationJson      location;
+  /**
+   * Should do cosine compensation when not pointing correct direction;.
+   */
+  public boolean           useCosineCompensator = true;
 
   /**
    * Create the swerve module configuration based off of parsed data.
@@ -131,6 +135,7 @@ public SwerveModuleConfiguration createModuleConfiguration(
         absoluteEncoderInverted,
         inverted.drive,
         inverted.angle,
-        name.replaceAll("\\.json", ""));
+        name.replaceAll("\\.json", ""),
+        useCosineCompensator);
   }
 }