From d1c78c3fe2c1375c2997647a09c12057248e429f Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Mon, 22 Jan 2024 21:34:38 -0500 Subject: [PATCH 1/5] Add our changes. --- src/main/java/swervelib/SwerveDrive.java | 19 +++++++++++++++++++ src/main/java/swervelib/SwerveModule.java | 9 +++++++++ .../java/swervelib/motors/SparkMaxSwerve.java | 10 ++++++++++ 3 files changed, 38 insertions(+) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 0244c577..d068f207 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -535,6 +535,14 @@ public double getMaximumAngularVelocity() */ private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) { + { + List velocities = new ArrayList<>(); + for (var swerveModuleState : desiredStates) { + velocities.add(swerveModuleState.speedMetersPerSecond); + } + Double[] speeds = velocities.toArray(new Double[velocities.size()]); + SmartDashboard.putNumberArray ("rawspeeds", speeds); + } // Desaturates wheel speeds if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) { @@ -546,12 +554,23 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpe { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxSpeedMPS); } + { + List velocities = new ArrayList<>(); + for (var swerveModuleState : desiredStates) { + velocities.add(swerveModuleState.speedMetersPerSecond); + } + Double[] speeds = velocities.toArray(new Double[velocities.size()]); + SmartDashboard.putNumberArray ("desaturatedspeeds", speeds); + } + // Sets states for (SwerveModule module : swerveModules) { module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false); + SmartDashboard.putString ("module." + module.configuration.name + ".status", "open:" + isOpenLoop + ", drivespeed=" + desiredStates[module.moduleNumber].speedMetersPerSecond); + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { SwerveDriveTelemetry.desiredStates[module.moduleNumber * diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 4800c30a..ce77526c 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -1,5 +1,7 @@ package swervelib; +import com.revrobotics.CANSparkMax; + import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -212,6 +214,11 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, { cosineScalar = 0.0; } + // TODO remove thjis + //cosineScalar = 1.0; + CANSparkMax cdm = (CANSparkMax) driveMotor.getMotor(); + SmartDashboard.putNumber("motor." + cdm.getDeviceId() + ".cosineScalar", cosineScalar); + SmartDashboard.putNumber("motor." + cdm.getDeviceId() + ".steermotorerror", steerMotorError); double velocity = desiredState.speedMetersPerSecond * (cosineScalar); driveMotor.setReference(velocity, feedforward.calculate(velocity)); @@ -250,6 +257,8 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, { SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint", desiredState.speedMetersPerSecond); SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint", desiredState.angle.getDegrees()); + SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Absolute Position", getAbsolutePosition()); + SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Relative Position", getRelativePosition()); } } diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index e0aa13be..9f098cad 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -12,6 +12,8 @@ import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkPIDController; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; @@ -339,6 +341,7 @@ public void burnFlash() @Override public void set(double percentOutput) { + SmartDashboard.putNumber(dashboardName("percent"), percentOutput); motor.set(percentOutput); } @@ -356,8 +359,10 @@ public void setReference(double setpoint, double feedforward) // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); int pidSlot = 0; + SmartDashboard.putNumber(dashboardName("feedforward"), feedforward); if (isDriveMotor) { + SmartDashboard.putNumber(dashboardName("velocity.setpoint"), setpoint); configureSparkMax(() -> pid.setReference( setpoint, @@ -366,6 +371,7 @@ public void setReference(double setpoint, double feedforward) feedforward)); } else { + SmartDashboard.putNumber(dashboardName("position.setpoint"), setpoint); configureSparkMax(() -> pid.setReference( setpoint, @@ -428,6 +434,10 @@ public void setPosition(double position) } } + String dashboardName(String s) { + return "motor." + motor.getDeviceId() + "." + s; + } + /** * REV Slots for PID configuration. */ From 263ebefe540b9e3585aa4b91afcf61948b2e60c4 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Mon, 22 Jan 2024 23:04:49 -0500 Subject: [PATCH 2/5] Make cosineCompensation switchable. --- src/main/java/swervelib/SwerveModule.java | 36 ++++++++++--------- .../parser/SwerveModuleConfiguration.java | 16 +++++++-- .../swervelib/parser/json/ModuleJson.java | 7 +++- 3 files changed, 38 insertions(+), 21 deletions(-) diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index ce77526c..3d23653f 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -201,24 +201,26 @@ 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) - { - cosineScalar = 0.0; + double cosineScalar = 1.0; + if (configuration.shouldCosineCompensate) { + // 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; + } + + CANSparkMax cdm = (CANSparkMax) driveMotor.getMotor(); + SmartDashboard.putNumber("motor." + cdm.getDeviceId() + ".cosineScalar", cosineScalar); + SmartDashboard.putNumber("motor." + cdm.getDeviceId() + ".steermotorerror", steerMotorError); } - // TODO remove thjis - //cosineScalar = 1.0; - CANSparkMax cdm = (CANSparkMax) driveMotor.getMotor(); - SmartDashboard.putNumber("motor." + cdm.getDeviceId() + ".cosineScalar", cosineScalar); - SmartDashboard.putNumber("motor." + cdm.getDeviceId() + ".steermotorerror", steerMotorError); double velocity = desiredState.speedMetersPerSecond * (cosineScalar); driveMotor.setReference(velocity, feedforward.calculate(velocity)); diff --git a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java index 2e8d9f89..e8faf2c6 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 shouldCosineCompensate; /** * 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. + * @oaram shouldCosineCompensate Should use consineCompensation. */ public SwerveModuleConfiguration( SwerveMotor driveMotor, @@ -95,7 +100,8 @@ public SwerveModuleConfiguration( boolean absoluteEncoderInverted, boolean driveMotorInverted, boolean angleMotorInverted, - String name) + String name, + boolean shouldCosineCompensate) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; @@ -110,6 +116,7 @@ public SwerveModuleConfiguration( this.velocityPIDF = velocityPIDF; this.physicalCharacteristics = physicalCharacteristics; this.name = name; + this.shouldCosineCompensate = shouldCosineCompensate; } /** @@ -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. + * @oaram shouldCosineCompensate Should use consineCompensation. */ public SwerveModuleConfiguration( SwerveMotor driveMotor, @@ -139,7 +147,8 @@ public SwerveModuleConfiguration( PIDFConfig anglePIDF, PIDFConfig velocityPIDF, SwerveModulePhysicalCharacteristics physicalCharacteristics, - String name) + String name, + boolean shouldCosineCompensate) { this( driveMotor, @@ -155,7 +164,8 @@ public SwerveModuleConfiguration( false, false, false, - name); + name, + shouldCosineCompensate); } diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index d3454151..f1d2c95e 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 shouldCosineCompensate = false; /** * 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", ""), + shouldCosineCompensate); } } From 73d6d3ddc59b93638e986a7d8a86a9df03e00c9b Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Tue, 23 Jan 2024 22:07:33 -0500 Subject: [PATCH 3/5] Fix names. --- docs/json/swervemodule.md | 1 + src/main/java/swervelib/SwerveModule.java | 6 +----- .../parser/SwerveModuleConfiguration.java | 14 +++++++------- .../java/swervelib/parser/json/ModuleJson.java | 4 ++-- 4 files changed, 11 insertions(+), 14 deletions(-) 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 3d23653f..0114bb92 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -202,7 +202,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, } else { double cosineScalar = 1.0; - if (configuration.shouldCosineCompensate) { + 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 */ @@ -216,10 +216,6 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, { cosineScalar = 0.0; } - - CANSparkMax cdm = (CANSparkMax) driveMotor.getMotor(); - SmartDashboard.putNumber("motor." + cdm.getDeviceId() + ".cosineScalar", cosineScalar); - SmartDashboard.putNumber("motor." + cdm.getDeviceId() + ".steermotorerror", steerMotorError); } double velocity = desiredState.speedMetersPerSecond * (cosineScalar); diff --git a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java index e8faf2c6..0fbe0553 100644 --- a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java +++ b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java @@ -65,7 +65,7 @@ public class SwerveModuleConfiguration /** * Should do cosine compensation when not pointing correct direction;. */ - public boolean shouldCosineCompensate; + public boolean useCosineCompensator; /** * Construct a configuration object for swerve modules. @@ -84,7 +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. - * @oaram shouldCosineCompensate Should use consineCompensation. + * @param useCosineCompensator Should use cosineCompensation. */ public SwerveModuleConfiguration( SwerveMotor driveMotor, @@ -101,7 +101,7 @@ public SwerveModuleConfiguration( boolean driveMotorInverted, boolean angleMotorInverted, String name, - boolean shouldCosineCompensate) + boolean useCosineCompensator) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; @@ -116,7 +116,7 @@ public SwerveModuleConfiguration( this.velocityPIDF = velocityPIDF; this.physicalCharacteristics = physicalCharacteristics; this.name = name; - this.shouldCosineCompensate = shouldCosineCompensate; + this.useCosineCompensator = useCosineCompensator; } /** @@ -134,7 +134,7 @@ public SwerveModuleConfiguration( * @param velocityPIDF Velocity PIDF configuration. * @param physicalCharacteristics Physical characteristics of the swerve module. * @param name Name for the module. - * @oaram shouldCosineCompensate Should use consineCompensation. + * @param useCosineCompensator Should use cosineCompensation. */ public SwerveModuleConfiguration( SwerveMotor driveMotor, @@ -148,7 +148,7 @@ public SwerveModuleConfiguration( PIDFConfig velocityPIDF, SwerveModulePhysicalCharacteristics physicalCharacteristics, String name, - boolean shouldCosineCompensate) + boolean useCosineCompensator) { this( driveMotor, @@ -165,7 +165,7 @@ public SwerveModuleConfiguration( false, false, name, - shouldCosineCompensate); + useCosineCompensator); } diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index f1d2c95e..9b9e6aae 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -56,7 +56,7 @@ public class ModuleJson /** * Should do cosine compensation when not pointing correct direction;. */ - public boolean shouldCosineCompensate = false; + public boolean useCosineCompensator = true; /** * Create the swerve module configuration based off of parsed data. @@ -136,6 +136,6 @@ public SwerveModuleConfiguration createModuleConfiguration( inverted.drive, inverted.angle, name.replaceAll("\\.json", ""), - shouldCosineCompensate); + useCosineCompensator); } } From e24ce93a2f0bfc8e34dadc129078fc55da33456e Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Tue, 23 Jan 2024 22:20:45 -0500 Subject: [PATCH 4/5] whoops. don't send too much upstream. --- src/main/java/swervelib/SwerveDrive.java | 19 ------------------- src/main/java/swervelib/SwerveModule.java | 4 ---- .../java/swervelib/motors/SparkMaxSwerve.java | 9 --------- 3 files changed, 32 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index d068f207..0244c577 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -535,14 +535,6 @@ public double getMaximumAngularVelocity() */ private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) { - { - List velocities = new ArrayList<>(); - for (var swerveModuleState : desiredStates) { - velocities.add(swerveModuleState.speedMetersPerSecond); - } - Double[] speeds = velocities.toArray(new Double[velocities.size()]); - SmartDashboard.putNumberArray ("rawspeeds", speeds); - } // Desaturates wheel speeds if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) { @@ -554,23 +546,12 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpe { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxSpeedMPS); } - { - List velocities = new ArrayList<>(); - for (var swerveModuleState : desiredStates) { - velocities.add(swerveModuleState.speedMetersPerSecond); - } - Double[] speeds = velocities.toArray(new Double[velocities.size()]); - SmartDashboard.putNumberArray ("desaturatedspeeds", speeds); - } - // Sets states for (SwerveModule module : swerveModules) { module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false); - SmartDashboard.putString ("module." + module.configuration.name + ".status", "open:" + isOpenLoop + ", drivespeed=" + desiredStates[module.moduleNumber].speedMetersPerSecond); - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { SwerveDriveTelemetry.desiredStates[module.moduleNumber * diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 0114bb92..6c6fc67a 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -1,7 +1,5 @@ package swervelib; -import com.revrobotics.CANSparkMax; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -255,8 +253,6 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, { SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint", desiredState.speedMetersPerSecond); SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint", desiredState.angle.getDegrees()); - SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Absolute Position", getAbsolutePosition()); - SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Relative Position", getRelativePosition()); } } diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 9f098cad..c7bf84ad 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -12,8 +12,6 @@ import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkPIDController; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; @@ -359,10 +357,8 @@ public void setReference(double setpoint, double feedforward) // isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); int pidSlot = 0; - SmartDashboard.putNumber(dashboardName("feedforward"), feedforward); if (isDriveMotor) { - SmartDashboard.putNumber(dashboardName("velocity.setpoint"), setpoint); configureSparkMax(() -> pid.setReference( setpoint, @@ -371,7 +367,6 @@ public void setReference(double setpoint, double feedforward) feedforward)); } else { - SmartDashboard.putNumber(dashboardName("position.setpoint"), setpoint); configureSparkMax(() -> pid.setReference( setpoint, @@ -434,10 +429,6 @@ public void setPosition(double position) } } - String dashboardName(String s) { - return "motor." + motor.getDeviceId() + "." + s; - } - /** * REV Slots for PID configuration. */ From 159d858cb7608b0fc55ddde4502abee950a9b2a0 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Tue, 23 Jan 2024 22:22:19 -0500 Subject: [PATCH 5/5] Another upstream oops. --- src/main/java/swervelib/motors/SparkMaxSwerve.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index c7bf84ad..e0aa13be 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -339,7 +339,6 @@ public void burnFlash() @Override public void set(double percentOutput) { - SmartDashboard.putNumber(dashboardName("percent"), percentOutput); motor.set(percentOutput); }