From 1afb940a7546b1f9448f3f76b8e3d25e8b9181cf Mon Sep 17 00:00:00 2001 From: aaron345678 <94726964+aaron345678@users.noreply.github.com> Date: Sat, 9 Mar 2024 00:34:59 +0000 Subject: [PATCH 1/9] Changed wpilib pid to Spark PID --- .../lib199/swerve/SwerveModule.java | 42 ++++++++++++------- 1 file changed, 26 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index 412e206a..5cd2e777 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkBase.IdleMode; import edu.wpi.first.math.MathUtil; @@ -20,6 +21,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import com.revrobotics.CANSparkBase; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -39,7 +41,8 @@ public enum ModuleType {FL, FR, BL, BR}; private ModuleType type; private CANSparkMax drive, turn; private CANcoder turnEncoder; - private PIDController drivePIDController; + private SparkPIDController drivePIDController; + private PIDController drivePIDController2; private ProfiledPIDController turnPIDController; private TrapezoidProfile.Constraints turnConstraints; private double driveModifier, turnZeroDeg; @@ -86,15 +89,17 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN this.backwardSimpleMotorFF = new SimpleMotorFeedforward(config.kBackwardVolts[arrIndex], config.kBackwardVels[arrIndex], config.kBackwardAccels[arrIndex]); + drivePIDController2 = new PIDController(config.drivekP[arrIndex], + config.drivekI[arrIndex], + config.drivekD[arrIndex]); - drivePIDController = new PIDController(config.drivekP[arrIndex], - config.drivekI[arrIndex], - config.drivekD[arrIndex]); - + drivePIDController.setP(config.drivekP[arrIndex]); + drivePIDController.setI(config.drivekI[arrIndex]); + drivePIDController.setD(config.drivekD[arrIndex]); /* offset for 1 CANcoder count */ drivetoleranceMPerS = (1.0 / (double)(drive.getEncoder().getCountsPerRevolution()) * positionConstant) / Units.millisecondsToSeconds(drive.getEncoder().getMeasurementPeriod() * drive.getEncoder().getAverageDepth()); - drivePIDController.setTolerance(drivetoleranceMPerS); - + drivePIDController.setIZone(drivetoleranceMPerS); + drivePIDController2.setTolerance(drivetoleranceMPerS); //System.out.println("Velocity Constant: " + (positionConstant / 60)); this.turn = turn; @@ -175,15 +180,18 @@ public void drivePeriodic() { // Use robot characterization as a simple physical model to account for internal resistance, frcition, etc. // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed) - double pidVolts = drivePIDController.calculate(actualSpeed, desiredSpeed); + //Switching to SparkPIDController to fix the large error and slow update time. use WPIlib pid controller to calculate the next voltage and plug it into the set refernece for SparkPIDController to drive using a SParkPID for faster update times + double nextVoltage = drivePIDController2.calculate(actualSpeed,desiredSpeed); + drivePIDController.setReference(nextVoltage, CANSparkBase.ControlType.kVoltage,0,forwardSimpleMotorFF.calculate(desiredSpeed));//drivePIDController.calculate(actualSpeed, desiredSpeed); + SmartDashboard.putNumber(moduleString + "Actual Speed", actualSpeed); SmartDashboard.putNumber(moduleString + "Desired Speed", desiredSpeed); - if (drivePIDController.atSetpoint()) { - pidVolts = 0; - } - targetVoltage += pidVolts; - SmartDashboard.putBoolean(moduleString + " is within drive tolerance", drivePIDController.atSetpoint()); - SmartDashboard.putNumber(moduleString + " pidVolts", pidVolts); + + + + //targetVoltage += pidVolts; + // SmartDashboard.putBoolean(moduleString + " is within drive tolerance", drivePIDController.atSetpoint()); + // SmartDashboard.putNumber(moduleString + " pidVolts", pidVolts); double appliedVoltage = MathUtil.clamp(targetVoltage, -12, 12); SmartDashboard.putNumber(moduleString + " appliedVoltage", appliedVoltage); @@ -342,7 +350,7 @@ public void updateSmartDashboard() { SmartDashboard.putNumber(moduleString + " Encoder Position", drive.getEncoder().getPosition()); // Display the speed that the robot thinks it is travelling at. SmartDashboard.putNumber(moduleString + " Current Speed", getCurrentSpeed()); - SmartDashboard.putBoolean(moduleString + " Drive is at Goal", drivePIDController.atSetpoint()); + //SmartDashboard.putBoolean(moduleString + " Drive is at Goal", drivePIDController.atSetpoint()); SmartDashboard.putNumber(moduleString + " Turn Setpoint Pos (deg)", turnPIDController.getSetpoint().position); SmartDashboard.putNumber(moduleString + " Turn Setpoint Vel (dps)", turnPIDController.getSetpoint().velocity); SmartDashboard.putNumber(moduleString + " Turn Goal Pos (deg)", turnPIDController.getGoal().position); @@ -365,10 +373,12 @@ public void updateSmartDashboard() { if (drivePIDController.getD() != drivekD) { drivePIDController.setD(drivekD); } + /* double driveTolerance = SmartDashboard.getNumber(moduleString + " Drive Tolerance", drivePIDController.getPositionTolerance()); if (drivePIDController.getPositionTolerance() != driveTolerance) { drivePIDController.setTolerance(driveTolerance); } + */ double drivekS = SmartDashboard.getNumber(moduleString + " Drive kS", forwardSimpleMotorFF.ks); double drivekV = SmartDashboard.getNumber(moduleString + " Drive kV", forwardSimpleMotorFF.kv); double drivekA = SmartDashboard.getNumber(moduleString + " Drive kA", forwardSimpleMotorFF.ka); @@ -442,7 +452,7 @@ public void initSendable(SendableBuilder builder) { builder.addDoubleProperty("Antigravitational Acceleration", () -> calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()), null); builder.addBooleanProperty("Turn is at Goal", turnPIDController::atGoal, null); builder.addDoubleProperty("Error (deg)", turnPIDController::getPositionError, null); - builder.addDoubleProperty("Desired Speed (mps)", drivePIDController::getSetpoint, null); +// builder.addDoubleProperty("Desired Speed (mps)", drivePIDController::getSetpoint, null); builder.addDoubleProperty("Angle Diff (deg)", () -> angleDiffRot, null); builder.addDoubleProperty("Turn PID Output", () -> turnSpeedCorrectionVolts, null); From a51219e12abc20abde35d004bfae7f52d4b11b76 Mon Sep 17 00:00:00 2001 From: aaron345678 <94726964+aaron345678@users.noreply.github.com> Date: Sat, 9 Mar 2024 01:32:45 +0000 Subject: [PATCH 2/9] changed setReference according to deans advice --- .../org/carlmontrobotics/lib199/swerve/SwerveModule.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index 5cd2e777..bcf820de 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -11,6 +11,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.SparkPIDController.ArbFFUnits; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; @@ -66,7 +67,6 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN this.config = config; this.type = type; this.drive = drive; - double positionConstant = config.wheelDiameterMeters * Math.PI / config.driveGearing; drive.setInverted(config.driveInversion[arrIndex]); drive.getEncoder().setPositionConversionFactor(positionConstant); @@ -96,6 +96,8 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN drivePIDController.setP(config.drivekP[arrIndex]); drivePIDController.setI(config.drivekI[arrIndex]); drivePIDController.setD(config.drivekD[arrIndex]); + + drivePIDController.setFF(forwardSimpleMotorFF.kv); /* offset for 1 CANcoder count */ drivetoleranceMPerS = (1.0 / (double)(drive.getEncoder().getCountsPerRevolution()) * positionConstant) / Units.millisecondsToSeconds(drive.getEncoder().getMeasurementPeriod() * drive.getEncoder().getAverageDepth()); drivePIDController.setIZone(drivetoleranceMPerS); @@ -158,7 +160,7 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN SmartDashboard.putData(this); SendableRegistry.addLW(this, "SwerveModule", type.toString()); - + } public ModuleType getType() { @@ -182,7 +184,7 @@ public void drivePeriodic() { // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed) //Switching to SparkPIDController to fix the large error and slow update time. use WPIlib pid controller to calculate the next voltage and plug it into the set refernece for SparkPIDController to drive using a SParkPID for faster update times double nextVoltage = drivePIDController2.calculate(actualSpeed,desiredSpeed); - drivePIDController.setReference(nextVoltage, CANSparkBase.ControlType.kVoltage,0,forwardSimpleMotorFF.calculate(desiredSpeed));//drivePIDController.calculate(actualSpeed, desiredSpeed); + drivePIDController.setReference(nextVoltage, CANSparkBase.ControlType.kVelocity,0,Math.copySign(forwardSimpleMotorFF.ks,nextVoltage));//drivePIDController.calculate(actualSpeed, desiredSpeed); SmartDashboard.putNumber(moduleString + "Actual Speed", actualSpeed); SmartDashboard.putNumber(moduleString + "Desired Speed", desiredSpeed); From 6a0c49b92b9f997b6780842e0394b4ffbcf61a5b Mon Sep 17 00:00:00 2001 From: aaron345678 <94726964+aaron345678@users.noreply.github.com> Date: Sat, 9 Mar 2024 02:00:02 +0000 Subject: [PATCH 3/9] hopefully fixed measurements --- .../org/carlmontrobotics/lib199/swerve/SwerveModule.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index bcf820de..1b6200e1 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -53,6 +53,7 @@ public enum ModuleType {FL, FR, BL, BR}; private SimpleMotorFeedforward forwardSimpleMotorFF, backwardSimpleMotorFF, turnSimpleMotorFeedforward; private double maxControllableAccerlationRps2; private double desiredSpeed, lastAngle, maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2, drivetoleranceMPerS, turnToleranceRot, angleDiffRot; + private double positionConstant; private double turnSpeedCorrectionVolts, turnFFVolts, turnVolts; private double maxTurnVelocityWithoutTippingRps; @@ -67,7 +68,7 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN this.config = config; this.type = type; this.drive = drive; - double positionConstant = config.wheelDiameterMeters * Math.PI / config.driveGearing; + this.positionConstant = config.wheelDiameterMeters * Math.PI / config.driveGearing; drive.setInverted(config.driveInversion[arrIndex]); drive.getEncoder().setPositionConversionFactor(positionConstant); drive.getEncoder().setVelocityConversionFactor(positionConstant / 60); @@ -94,10 +95,8 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN config.drivekD[arrIndex]); drivePIDController.setP(config.drivekP[arrIndex]); - drivePIDController.setI(config.drivekI[arrIndex]); - drivePIDController.setD(config.drivekD[arrIndex]); - drivePIDController.setFF(forwardSimpleMotorFF.kv); + drivePIDController.setFF(forwardSimpleMotorFF.kv/positionConstant*(2*Math.PI/60)); /* offset for 1 CANcoder count */ drivetoleranceMPerS = (1.0 / (double)(drive.getEncoder().getCountsPerRevolution()) * positionConstant) / Units.millisecondsToSeconds(drive.getEncoder().getMeasurementPeriod() * drive.getEncoder().getAverageDepth()); drivePIDController.setIZone(drivetoleranceMPerS); @@ -184,7 +183,7 @@ public void drivePeriodic() { // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed) //Switching to SparkPIDController to fix the large error and slow update time. use WPIlib pid controller to calculate the next voltage and plug it into the set refernece for SparkPIDController to drive using a SParkPID for faster update times double nextVoltage = drivePIDController2.calculate(actualSpeed,desiredSpeed); - drivePIDController.setReference(nextVoltage, CANSparkBase.ControlType.kVelocity,0,Math.copySign(forwardSimpleMotorFF.ks,nextVoltage));//drivePIDController.calculate(actualSpeed, desiredSpeed); + drivePIDController.setReference(nextVoltage, CANSparkBase.ControlType.kVelocity,0,Math.copySign(forwardSimpleMotorFF.ks/(positionConstant*(2*Math.PI/60)),nextVoltage));//drivePIDController.calculate(actualSpeed, desiredSpeed); SmartDashboard.putNumber(moduleString + "Actual Speed", actualSpeed); SmartDashboard.putNumber(moduleString + "Desired Speed", desiredSpeed); From 0dcc27ba5e3f0a7996d820148ccfda8f842d4104 Mon Sep 17 00:00:00 2001 From: aaron345678 <94726964+aaron345678@users.noreply.github.com> Date: Sat, 9 Mar 2024 02:37:42 +0000 Subject: [PATCH 4/9] Dean reviewed and fixed --- .../lib199/swerve/SwerveModule.java | 35 ++++++------------- 1 file changed, 11 insertions(+), 24 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index 1b6200e1..83dab995 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -43,16 +43,16 @@ public enum ModuleType {FL, FR, BL, BR}; private CANSparkMax drive, turn; private CANcoder turnEncoder; private SparkPIDController drivePIDController; - private PIDController drivePIDController2; + private ProfiledPIDController turnPIDController; private TrapezoidProfile.Constraints turnConstraints; private double driveModifier, turnZeroDeg; private Supplier pitchDegSupplier, rollDegSupplier; private boolean reversed; private Timer timer; - private SimpleMotorFeedforward forwardSimpleMotorFF, backwardSimpleMotorFF, turnSimpleMotorFeedforward; + private SimpleMotorFeedforward forwardSimpleMotorFF, turnSimpleMotorFeedforward; private double maxControllableAccerlationRps2; - private double desiredSpeed, lastAngle, maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2, drivetoleranceMPerS, turnToleranceRot, angleDiffRot; + private double desiredSpeed, lastAngle, maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2, turnToleranceRot, angleDiffRot; private double positionConstant; private double turnSpeedCorrectionVolts, turnFFVolts, turnVolts; @@ -87,20 +87,11 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex], config.kForwardVels[arrIndex], config.kForwardAccels[arrIndex]); - this.backwardSimpleMotorFF = new SimpleMotorFeedforward(config.kBackwardVolts[arrIndex], - config.kBackwardVels[arrIndex], - config.kBackwardAccels[arrIndex]); - drivePIDController2 = new PIDController(config.drivekP[arrIndex], - config.drivekI[arrIndex], - config.drivekD[arrIndex]); - - drivePIDController.setP(config.drivekP[arrIndex]); - drivePIDController.setFF(forwardSimpleMotorFF.kv/positionConstant*(2*Math.PI/60)); + drivePIDController = drive.getPIDController(); + drivePIDController.setP((config.drivekP[arrIndex]/12)/drive.getEncoder().getVelocityConversionFactor()); + drivePIDController.setFF((forwardSimpleMotorFF.kv/12)/drive.getEncoder().getVelocityConversionFactor()); /* offset for 1 CANcoder count */ - drivetoleranceMPerS = (1.0 / (double)(drive.getEncoder().getCountsPerRevolution()) * positionConstant) / Units.millisecondsToSeconds(drive.getEncoder().getMeasurementPeriod() * drive.getEncoder().getAverageDepth()); - drivePIDController.setIZone(drivetoleranceMPerS); - drivePIDController2.setTolerance(drivetoleranceMPerS); //System.out.println("Velocity Constant: " + (positionConstant / 60)); this.turn = turn; @@ -176,14 +167,11 @@ public void periodic() { public void drivePeriodic() { String moduleString = type.toString(); double actualSpeed = getCurrentSpeed(); - double targetVoltage = (actualSpeed >= 0 ? forwardSimpleMotorFF : - backwardSimpleMotorFF).calculate(desiredSpeed, calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()));//clippedAcceleration); // Use robot characterization as a simple physical model to account for internal resistance, frcition, etc. // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed) //Switching to SparkPIDController to fix the large error and slow update time. use WPIlib pid controller to calculate the next voltage and plug it into the set refernece for SparkPIDController to drive using a SParkPID for faster update times - double nextVoltage = drivePIDController2.calculate(actualSpeed,desiredSpeed); - drivePIDController.setReference(nextVoltage, CANSparkBase.ControlType.kVelocity,0,Math.copySign(forwardSimpleMotorFF.ks/(positionConstant*(2*Math.PI/60)),nextVoltage));//drivePIDController.calculate(actualSpeed, desiredSpeed); + drivePIDController.setReference(desiredSpeed, CANSparkBase.ControlType.kVelocity,0,Math.copySign(forwardSimpleMotorFF.ks,desiredSpeed),ArbFFUnits.kVoltage);//drivePIDController.calculate(actualSpeed, desiredSpeed); SmartDashboard.putNumber(moduleString + "Actual Speed", actualSpeed); SmartDashboard.putNumber(moduleString + "Desired Speed", desiredSpeed); @@ -193,10 +181,10 @@ public void drivePeriodic() { //targetVoltage += pidVolts; // SmartDashboard.putBoolean(moduleString + " is within drive tolerance", drivePIDController.atSetpoint()); // SmartDashboard.putNumber(moduleString + " pidVolts", pidVolts); - double appliedVoltage = MathUtil.clamp(targetVoltage, -12, 12); - SmartDashboard.putNumber(moduleString + " appliedVoltage", appliedVoltage); + + SmartDashboard.putNumber(moduleString + " applied outubt", drive.getAppliedOutput()); - drive.setVoltage(appliedVoltage); + //drive.setVoltage(appliedVoltage); } public void turnPeriodic() { @@ -385,7 +373,6 @@ public void updateSmartDashboard() { double drivekA = SmartDashboard.getNumber(moduleString + " Drive kA", forwardSimpleMotorFF.ka); if (forwardSimpleMotorFF.ks != drivekS || forwardSimpleMotorFF.kv != drivekV || forwardSimpleMotorFF.ka != drivekA) { forwardSimpleMotorFF = new SimpleMotorFeedforward(drivekS, drivekV, drivekA); - backwardSimpleMotorFF = new SimpleMotorFeedforward(drivekS, drivekV, drivekA); } double kP = SmartDashboard.getNumber(moduleString + " Swerve kP", turnPIDController.getP()); if (turnPIDController.getP() != kP) { @@ -453,7 +440,7 @@ public void initSendable(SendableBuilder builder) { builder.addDoubleProperty("Antigravitational Acceleration", () -> calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()), null); builder.addBooleanProperty("Turn is at Goal", turnPIDController::atGoal, null); builder.addDoubleProperty("Error (deg)", turnPIDController::getPositionError, null); -// builder.addDoubleProperty("Desired Speed (mps)", drivePIDController::getSetpoint, null); + builder.addDoubleProperty("Desired Speed (mps)", () -> desiredSpeed, null); builder.addDoubleProperty("Angle Diff (deg)", () -> angleDiffRot, null); builder.addDoubleProperty("Turn PID Output", () -> turnSpeedCorrectionVolts, null); From cc19020b73a46804c3c512e744021b02e1eb5001 Mon Sep 17 00:00:00 2001 From: ProfessorAtomicManiac <59379639+ProfessorAtomicManiac@users.noreply.github.com> Date: Fri, 8 Mar 2024 20:16:49 -0800 Subject: [PATCH 5/9] fixed bug --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index 83dab995..c651e2f0 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -89,8 +89,8 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN config.kForwardAccels[arrIndex]); drivePIDController = drive.getPIDController(); - drivePIDController.setP((config.drivekP[arrIndex]/12)/drive.getEncoder().getVelocityConversionFactor()); - drivePIDController.setFF((forwardSimpleMotorFF.kv/12)/drive.getEncoder().getVelocityConversionFactor()); + drivePIDController.setP((config.drivekP[arrIndex]/12) * drive.getEncoder().getVelocityConversionFactor()); + drivePIDController.setFF((forwardSimpleMotorFF.kv/12) * drive.getEncoder().getVelocityConversionFactor()); /* offset for 1 CANcoder count */ //System.out.println("Velocity Constant: " + (positionConstant / 60)); From 9775a6873dba95a92dea65faa8fd840e79213200 Mon Sep 17 00:00:00 2001 From: ProfessorAtomicManiac <59379639+ProfessorAtomicManiac@users.noreply.github.com> Date: Fri, 8 Mar 2024 20:26:53 -0800 Subject: [PATCH 6/9] fix bug --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index c651e2f0..d865698c 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -88,7 +88,7 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN config.kForwardVels[arrIndex], config.kForwardAccels[arrIndex]); - drivePIDController = drive.getPIDController(); + drivePIDController = drive.getPIDController(); drivePIDController.setP((config.drivekP[arrIndex]/12) * drive.getEncoder().getVelocityConversionFactor()); drivePIDController.setFF((forwardSimpleMotorFF.kv/12) * drive.getEncoder().getVelocityConversionFactor()); /* offset for 1 CANcoder count */ @@ -171,7 +171,7 @@ public void drivePeriodic() { // Use robot characterization as a simple physical model to account for internal resistance, frcition, etc. // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed) //Switching to SparkPIDController to fix the large error and slow update time. use WPIlib pid controller to calculate the next voltage and plug it into the set refernece for SparkPIDController to drive using a SParkPID for faster update times - drivePIDController.setReference(desiredSpeed, CANSparkBase.ControlType.kVelocity,0,Math.copySign(forwardSimpleMotorFF.ks,desiredSpeed),ArbFFUnits.kVoltage);//drivePIDController.calculate(actualSpeed, desiredSpeed); + drivePIDController.setReference(desiredSpeed / drive.getEncoder().getVelocityConversionFactor(), CANSparkBase.ControlType.kVelocity,0,Math.copySign(forwardSimpleMotorFF.ks,desiredSpeed),ArbFFUnits.kVoltage);//drivePIDController.calculate(actualSpeed, desiredSpeed); SmartDashboard.putNumber(moduleString + "Actual Speed", actualSpeed); SmartDashboard.putNumber(moduleString + "Desired Speed", desiredSpeed); From 00fd4624c8e1fce58ae012dc92fd5c1432b37fff Mon Sep 17 00:00:00 2001 From: ProfessorAtomicManiac <59379639+ProfessorAtomicManiac@users.noreply.github.com> Date: Fri, 8 Mar 2024 20:48:53 -0800 Subject: [PATCH 7/9] reomove ks --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index d865698c..396a63ce 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -171,7 +171,7 @@ public void drivePeriodic() { // Use robot characterization as a simple physical model to account for internal resistance, frcition, etc. // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed) //Switching to SparkPIDController to fix the large error and slow update time. use WPIlib pid controller to calculate the next voltage and plug it into the set refernece for SparkPIDController to drive using a SParkPID for faster update times - drivePIDController.setReference(desiredSpeed / drive.getEncoder().getVelocityConversionFactor(), CANSparkBase.ControlType.kVelocity,0,Math.copySign(forwardSimpleMotorFF.ks,desiredSpeed),ArbFFUnits.kVoltage);//drivePIDController.calculate(actualSpeed, desiredSpeed); + drivePIDController.setReference(desiredSpeed / drive.getEncoder().getVelocityConversionFactor(), CANSparkBase.ControlType.kVelocity,0);//Math.copySign(forwardSimpleMotorFF.ks,desiredSpeed),ArbFFUnits.kVoltage);//drivePIDController.calculate(actualSpeed, desiredSpeed); SmartDashboard.putNumber(moduleString + "Actual Speed", actualSpeed); SmartDashboard.putNumber(moduleString + "Desired Speed", desiredSpeed); From 21bcaae721cd7ff71adc174fd1d37ce9430222c8 Mon Sep 17 00:00:00 2001 From: ProfessorAtomicManiac <59379639+ProfessorAtomicManiac@users.noreply.github.com> Date: Sat, 9 Mar 2024 11:10:58 -0800 Subject: [PATCH 8/9] added tolerance to reduce movement --- .../carlmontrobotics/lib199/swerve/SwerveModule.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index 396a63ce..e77ab91a 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -52,7 +52,7 @@ public enum ModuleType {FL, FR, BL, BR}; private Timer timer; private SimpleMotorFeedforward forwardSimpleMotorFF, turnSimpleMotorFeedforward; private double maxControllableAccerlationRps2; - private double desiredSpeed, lastAngle, maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2, turnToleranceRot, angleDiffRot; + private double desiredSpeed, lastAngle, maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2, drivetoleranceMPerS, turnToleranceRot, angleDiffRot; private double positionConstant; private double turnSpeedCorrectionVolts, turnFFVolts, turnVolts; @@ -93,6 +93,7 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN drivePIDController.setFF((forwardSimpleMotorFF.kv/12) * drive.getEncoder().getVelocityConversionFactor()); /* offset for 1 CANcoder count */ //System.out.println("Velocity Constant: " + (positionConstant / 60)); + drivetoleranceMPerS = (1.0 / (double)(drive.getEncoder().getCountsPerRevolution()) * positionConstant) / Units.millisecondsToSeconds(drive.getEncoder().getMeasurementPeriod() * drive.getEncoder().getAverageDepth()); this.turn = turn; @@ -171,8 +172,11 @@ public void drivePeriodic() { // Use robot characterization as a simple physical model to account for internal resistance, frcition, etc. // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed) //Switching to SparkPIDController to fix the large error and slow update time. use WPIlib pid controller to calculate the next voltage and plug it into the set refernece for SparkPIDController to drive using a SParkPID for faster update times - drivePIDController.setReference(desiredSpeed / drive.getEncoder().getVelocityConversionFactor(), CANSparkBase.ControlType.kVelocity,0);//Math.copySign(forwardSimpleMotorFF.ks,desiredSpeed),ArbFFUnits.kVoltage);//drivePIDController.calculate(actualSpeed, desiredSpeed); - + if (desiredSpeed < drivetoleranceMPerS) { + drivePIDController.setReference(desiredSpeed / drive.getEncoder().getVelocityConversionFactor(), CANSparkBase.ControlType.kVelocity,0); + } else { + drivePIDController.setReference(desiredSpeed / drive.getEncoder().getVelocityConversionFactor(), CANSparkBase.ControlType.kVelocity,0, Math.copySign(forwardSimpleMotorFF.ks,desiredSpeed), ArbFFUnits.kVoltage); + } SmartDashboard.putNumber(moduleString + "Actual Speed", actualSpeed); SmartDashboard.putNumber(moduleString + "Desired Speed", desiredSpeed); From 63d860b5f95a2a3ef44d8b15b8e67d366b2fe14a Mon Sep 17 00:00:00 2001 From: ProfessorAtomicManiac <59379639+ProfessorAtomicManiac@users.noreply.github.com> Date: Sat, 9 Mar 2024 19:18:03 -0800 Subject: [PATCH 9/9] fixed bug --- .../java/org/carlmontrobotics/lib199/swerve/SwerveModule.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index e77ab91a..3c843fd4 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -172,7 +172,7 @@ public void drivePeriodic() { // Use robot characterization as a simple physical model to account for internal resistance, frcition, etc. // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed) //Switching to SparkPIDController to fix the large error and slow update time. use WPIlib pid controller to calculate the next voltage and plug it into the set refernece for SparkPIDController to drive using a SParkPID for faster update times - if (desiredSpeed < drivetoleranceMPerS) { + if (Math.abs(desiredSpeed) < drivetoleranceMPerS) { drivePIDController.setReference(desiredSpeed / drive.getEncoder().getVelocityConversionFactor(), CANSparkBase.ControlType.kVelocity,0); } else { drivePIDController.setReference(desiredSpeed / drive.getEncoder().getVelocityConversionFactor(), CANSparkBase.ControlType.kVelocity,0, Math.copySign(forwardSimpleMotorFF.ks,desiredSpeed), ArbFFUnits.kVoltage);