diff --git a/docs/json/swervemodule.md b/docs/json/swervemodule.md index a8bb1ca1..2afbeba0 100644 --- a/docs/json/swervemodule.md +++ b/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 diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index f7145221..bcc3eb6a 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/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); diff --git a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java index 2e8d9f89..0fbe0553 100644 --- a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java +++ b/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); } diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index d3454151..9b9e6aae 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/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); } }