Skip to content

Spark pid controller drive motors #84

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 9 commits into
base: master
Choose a base branch
from
Open

Spark pid controller drive motors #84

Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
1afb940
Changed wpilib pid to Spark PID
aaron345678 Mar 9, 2024
a51219e
changed setReference according to deans advice
aaron345678 Mar 9, 2024
6a0c49b
hopefully fixed measurements
aaron345678 Mar 9, 2024
0dcc27b
Dean reviewed and fixed
aaron345678 Mar 9, 2024
cc19020
fixed bug
ProfessorAtomicManiac Mar 9, 2024
9775a68
fix bug
ProfessorAtomicManiac Mar 9, 2024
00fd462
reomove ks
ProfessorAtomicManiac Mar 9, 2024
21bcaae
added tolerance to reduce movement
ProfessorAtomicManiac Mar 9, 2024
63d860b
fixed bug
ProfessorAtomicManiac Mar 10, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
62 changes: 32 additions & 30 deletions src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,9 @@
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 com.revrobotics.SparkPIDController.ArbFFUnits;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
Expand All @@ -20,6 +22,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;
Expand All @@ -39,16 +42,18 @@ public enum ModuleType {FL, FR, BL, BR};
private ModuleType type;
private CANSparkMax drive, turn;
private CANcoder turnEncoder;
private PIDController drivePIDController;
private SparkPIDController drivePIDController;

private ProfiledPIDController turnPIDController;
private TrapezoidProfile.Constraints turnConstraints;
private double driveModifier, turnZeroDeg;
private Supplier<Float> 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 positionConstant;

private double turnSpeedCorrectionVolts, turnFFVolts, turnVolts;
private double maxTurnVelocityWithoutTippingRps;
Expand All @@ -63,8 +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);
Expand All @@ -83,19 +87,13 @@ 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]);

drivePIDController = new PIDController(config.drivekP[arrIndex],
config.drivekI[arrIndex],
config.drivekD[arrIndex]);

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.setTolerance(drivetoleranceMPerS);

//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;

Expand Down Expand Up @@ -153,7 +151,7 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN
SmartDashboard.putData(this);

SendableRegistry.addLW(this, "SwerveModule", type.toString());

}

public ModuleType getType() {
Expand All @@ -170,24 +168,27 @@ 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)
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
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);
}
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);
double appliedVoltage = MathUtil.clamp(targetVoltage, -12, 12);
SmartDashboard.putNumber(moduleString + " appliedVoltage", appliedVoltage);



//targetVoltage += pidVolts;
// SmartDashboard.putBoolean(moduleString + " is within drive tolerance", drivePIDController.atSetpoint());
// SmartDashboard.putNumber(moduleString + " pidVolts", pidVolts);

SmartDashboard.putNumber(moduleString + " applied outubt", drive.getAppliedOutput());

drive.setVoltage(appliedVoltage);
//drive.setVoltage(appliedVoltage);
}

public void turnPeriodic() {
Expand Down Expand Up @@ -342,7 +343,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);
Expand All @@ -365,16 +366,17 @@ 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);
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) {
Expand Down Expand Up @@ -442,7 +444,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);
Expand Down