Skip to content

Commit ce91324

Browse files
CoolSpy3CoolSpy3
authored
Merge pull request #29 from DeepBlueRobotics/sendables
Add LiveWindow compatibility
2 parents 87c7970 + d3ea1f4 commit ce91324

2 files changed

Lines changed: 78 additions & 64 deletions

File tree

‎src/main/java/org/carlmontrobotics/lib199/SparkVelocityPIDController.java‎

Lines changed: 32 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,17 @@
11
package org.carlmontrobotics.lib199;
22

33
import com.revrobotics.CANSparkMax;
4+
import com.revrobotics.CANSparkMax.ControlType;
45
import com.revrobotics.RelativeEncoder;
56
import com.revrobotics.SparkMaxPIDController;
6-
import com.revrobotics.CANSparkMax.ControlType;
77

8-
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
8+
import edu.wpi.first.util.sendable.Sendable;
9+
import edu.wpi.first.util.sendable.SendableBuilder;
10+
import edu.wpi.first.util.sendable.SendableRegistry;
911

10-
public class SparkVelocityPIDController {
12+
public class SparkVelocityPIDController implements Sendable {
1113

14+
@SuppressWarnings("unused")
1215
private final CANSparkMax spark;
1316
private final SparkMaxPIDController pidController;
1417
private final RelativeEncoder encoder;
@@ -29,36 +32,9 @@ public SparkVelocityPIDController(String name, CANSparkMax spark, double default
2932
this.kS = kS;
3033
this.kV = kV;
3134

32-
SmartDashboard.putNumber(name + ": P", currentP);
33-
SmartDashboard.putNumber(name + ": I", currentI);
34-
SmartDashboard.putNumber(name + ": D", currentD);
35-
SmartDashboard.putNumber(name + ": Target Speed", targetSpeed);
36-
SmartDashboard.putNumber(name + ": Tolerance", tolerance);
37-
3835
pidController.setReference(targetSpeed, ControlType.kVelocity, 0, calculateFF(targetSpeed));
39-
}
40-
41-
public void periodic() {
42-
double p = SmartDashboard.getNumber(name + ": P", currentP);
43-
double i = SmartDashboard.getNumber(name + ": I", currentI);
44-
double d = SmartDashboard.getNumber(name + ": D", currentD);
45-
46-
if(p != currentP) {
47-
pidController.setP(p);
48-
currentP = p;
49-
}
50-
if(i != currentI) {
51-
pidController.setI(i);
52-
currentI = i;
53-
}
54-
if(d != currentD) {
55-
pidController.setD(d);
56-
currentD = d;
57-
}
5836

59-
setTargetSpeed(SmartDashboard.getNumber(name + ": Target Speed", targetSpeed));
60-
setTolerance(SmartDashboard.getNumber(name + ": Tolerance", tolerance));
61-
SmartDashboard.putNumber(name + ": Current Speed", encoder.getVelocity());
37+
SendableRegistry.addLW(this, "SparkVelocityPIDController", spark.getDeviceId());
6238
}
6339

6440
public RelativeEncoder getEncoder() {
@@ -79,7 +55,6 @@ public double getTargetSpeed() {
7955

8056
public void setTargetSpeed(double targetSpeed) {
8157
if(targetSpeed == this.targetSpeed) return;
82-
SmartDashboard.putNumber(name + ": Target Speed", targetSpeed);
8358
this.targetSpeed = targetSpeed;
8459
pidController.setReference(targetSpeed, ControlType.kVelocity, 0, calculateFF(targetSpeed));
8560
}
@@ -89,12 +64,36 @@ public double getTolerance() {
8964
}
9065

9166
public void setTolerance(double tolerance) {
92-
SmartDashboard.putNumber(name + ": Tolerance", tolerance);
9367
this.tolerance = tolerance;
9468
}
9569

9670
public double calculateFF(double velocity) {
9771
return kS * Math.signum(velocity) + kV * velocity;
9872
}
9973

74+
@Override
75+
public void initSendable(SendableBuilder builder) {
76+
builder.setActuator(true);
77+
builder.setSmartDashboardType("SparkVelocityPIDController");
78+
builder.addDoubleProperty("P", () -> currentP, p -> {
79+
pidController.setP(p);
80+
currentP = p;
81+
});
82+
builder.addDoubleProperty("I", () -> currentI, i -> {
83+
pidController.setI(i);
84+
currentI = i;
85+
});
86+
builder.addDoubleProperty("D", () -> currentD, d -> {
87+
pidController.setD(d);
88+
currentD = d;
89+
});
90+
builder.addDoubleProperty("Target Speed", () -> targetSpeed, newSpeed -> {
91+
if(newSpeed == targetSpeed) return;
92+
pidController.setReference(newSpeed, ControlType.kVelocity, 0, calculateFF(newSpeed));
93+
targetSpeed = newSpeed;
94+
});
95+
builder.addDoubleProperty("Tolerance", () -> tolerance, newTolerance -> tolerance = newTolerance);
96+
builder.addDoubleProperty("Current Speed", encoder::getVelocity, null);
97+
}
98+
10099
}

‎src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java‎

Lines changed: 46 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -15,33 +15,35 @@
1515
import edu.wpi.first.math.kinematics.SwerveModulePosition;
1616
import edu.wpi.first.math.kinematics.SwerveModuleState;
1717
import edu.wpi.first.math.trajectory.TrapezoidProfile;
18+
import edu.wpi.first.util.sendable.Sendable;
19+
import edu.wpi.first.util.sendable.SendableBuilder;
20+
import edu.wpi.first.util.sendable.SendableRegistry;
1821
import edu.wpi.first.wpilibj.Timer;
1922
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
2023

2124
/**
2225
* A class that stores all the variables and methods applicaple to a single swerve module,
2326
* such as moving, getting encoder values, or configuring PID.
2427
*/
25-
public class SwerveModule {
28+
public class SwerveModule implements Sendable {
2629
public enum ModuleType {FL, FR, BL, BR};
2730

2831
private SwerveConfig config;
2932
private ModuleType type;
30-
private String moduleString;
3133
private CANSparkMax drive, turn;
3234
private CANCoder turnEncoder;
3335
private PIDController drivePIDController;
3436
private ProfiledPIDController turnPIDController;
3537
private TrapezoidProfile.Constraints turnConstraints;
36-
private double driveModifier, maxSpeed, turnZero;
38+
private double driveModifier, turnZero;
3739
private Supplier<Float> pitchDegSupplier, rollDegSupplier;
3840
private boolean reversed;
3941
private Timer timer;
4042
private SimpleMotorFeedforward forwardSimpleMotorFF, backwardSimpleMotorFF, turnSimpleMotorFeedforward;
41-
private double lastAngle, maxAchievableTurnVelocityDps, maxAchievableTurnAccelerationMps2, turnToleranceDeg;
43+
private double lastAngle, maxAchievableTurnVelocityDps, maxAchievableTurnAccelerationMps2, turnToleranceDeg, angleDiff;
4244

4345
public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CANSparkMax turn, CANCoder turnEncoder, double driveModifier,
44-
double maxSpeed, int arrIndex, Supplier<Float> pitchDegSupplier, Supplier<Float> rollDegSupplier) {
46+
int arrIndex, Supplier<Float> pitchDegSupplier, Supplier<Float> rollDegSupplier) {
4547
//SmartDashboard.putNumber("Target Angle (deg)", 0.0);
4648
this.timer = new Timer();
4749
timer.start();
@@ -50,21 +52,6 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN
5052
this.type = type;
5153
this.drive = drive;
5254

53-
switch (type) {
54-
case FL:
55-
moduleString = "FL";
56-
break;
57-
case FR:
58-
moduleString = "FR";
59-
break;
60-
case BL:
61-
moduleString = "BL";
62-
break;
63-
case BR:
64-
moduleString = "BR";
65-
break;
66-
}
67-
6855
double positionConstant = config.wheelDiameterMeters * Math.PI / config.driveGearing;
6956
drive.setInverted(config.driveInversion[arrIndex]);
7057
drive.getEncoder().setPositionConversionFactor(positionConstant);
@@ -77,7 +64,7 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN
7764
final double neoFreeCurrentAmps = 1.3;
7865
final double neoStallCurrentAmps = 166;
7966
double currentLimitAmps = neoFreeCurrentAmps + 2*motorTorqueLimitNewtonMeters / neoStallTorqueNewtonMeters * (neoStallCurrentAmps-neoFreeCurrentAmps);
80-
SmartDashboard.putNumber(moduleString + " current limit (amps)", currentLimitAmps);
67+
SmartDashboard.putNumber(type.toString() + " current limit (amps)", currentLimitAmps);
8168
drive.setSmartCurrentLimit((int)Math.min(50, currentLimitAmps));
8269

8370
this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex],
@@ -121,22 +108,27 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN
121108
this.turnEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180);
122109

123110
this.driveModifier = driveModifier;
124-
this.maxSpeed = maxSpeed;
125111
this.reversed = config.reversed[arrIndex];
126112
this.turnZero = config.turnZero[arrIndex];
127113

128114
turnPIDController.reset(getModuleAngle());
129115

130116
this.rollDegSupplier = rollDegSupplier;
131117
this.pitchDegSupplier = pitchDegSupplier;
118+
119+
SendableRegistry.addLW(this, "SweverModule", type.toString());
120+
}
121+
122+
public ModuleType getType() {
123+
return type;
132124
}
133125

134126
private double prevTurnVelocity = 0;
135127
public void periodic() {
136128
double measuredAngleDegs = getModuleAngle();
137129
TrapezoidProfile.State goal = turnPIDController.getGoal();
138130
goal = new TrapezoidProfile.State(goal.position, goal.velocity);
139-
131+
140132
double period = turnPIDController.getPeriod();
141133
double optimalTurnVelocityDps = Math.abs(MathUtil.inputModulus(goal.position-measuredAngleDegs, -180, 180))/period;
142134
setMaxTurnVelocity(Math.min(maxAchievableTurnVelocityDps, optimalTurnVelocityDps));
@@ -148,9 +140,8 @@ public void periodic() {
148140
turn.setVoltage(MathUtil.clamp(turnVolts, -12.0, 12.0));
149141
} else {
150142
turn.setVoltage(turnSimpleMotorFeedforward.calculate(goal.velocity));
151-
}
143+
}
152144
prevTurnVelocity = state.velocity;
153-
SmartDashboard.putNumber(moduleString + " error (deg)", turnPIDController.getPositionError());
154145
}
155146

156147
/**
@@ -193,8 +184,6 @@ private void setSpeed(double speedMps) {
193184
// Compute desired and actual speeds in m/s
194185
double desiredSpeed = speedMps * driveModifier;
195186
double actualSpeed = getCurrentSpeed();
196-
SmartDashboard.putNumber(moduleString + " Desired Speed (mps)", desiredSpeed);
197-
SmartDashboard.putNumber(moduleString + " Actual Speed (mps)", actualSpeed);
198187
double targetVoltage = (actualSpeed >= 0 ? forwardSimpleMotorFF :
199188
backwardSimpleMotorFF).calculate(desiredSpeed, calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()));//clippedAcceleration);
200189

@@ -216,12 +205,12 @@ private void setAngle(double angle) {
216205
double maxDeltaTheta = Math.asin(deltaTime*config.autoCentripetalAccel/(Math.abs(getCurrentSpeed())+0.0001));
217206
setMaxTurnVelocity(maxDeltaTheta*180/Math.PI);
218207
//SmartDashboard.putNumber(moduleString + "Target Angle:", 360 * angle * (reversed ? -1 : 1));
219-
208+
220209
// Find the minimum distance to travel from lastAngle to angle and determine the
221210
// correct direction to trvel the minimum distance. This is used in order to accurately
222211
// calculate the goal velocity.
223-
double angleDiff = MathUtil.inputModulus(angle - lastAngle, -180, 180);
224-
SmartDashboard.putNumber(moduleString + " angleDiff (deg)", angleDiff);
212+
angleDiff = MathUtil.inputModulus(angle - lastAngle, -180, 180);
213+
// SmartDashboard.putNumber(moduleString + " angleDiff (deg)", angleDiff);
225214

226215
turnPIDController.setGoal(new TrapezoidProfile.State(angle * (reversed ? -1 : 1), 0));
227216
lastAngle = angle;
@@ -261,8 +250,12 @@ public double getCurrentSpeed() {
261250

262251
/**
263252
* Updates SmartDashboard with information about this module.
253+
*
254+
* @deprecated Put this Sendable to SmartDashboard instead
264255
*/
256+
@Deprecated
265257
public void updateSmartDashboard() {
258+
String moduleString = type.toString();
266259
// Display the position of the quadrature encoder.
267260
SmartDashboard.putNumber(moduleString + " Incremental Position", turnEncoder.getPosition());
268261
// Display the position of the analog encoder.
@@ -281,7 +274,7 @@ public void updateSmartDashboard() {
281274
SmartDashboard.putNumber(moduleString + "Antigravitational Acceleration", calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()));
282275
SmartDashboard.putBoolean(moduleString + " Turn is at Goal", turnPIDController.atGoal());
283276
}
284-
277+
285278
public void toggleMode() {
286279
if (drive.getIdleMode() == IdleMode.kBrake && turn.getIdleMode() == IdleMode.kCoast) coast();
287280
else brake();
@@ -301,4 +294,26 @@ public void setMaxTurnVelocity(double maxVel) {
301294
turnConstraints = new TrapezoidProfile.Constraints(maxVel, turnConstraints.maxAcceleration);
302295
turnPIDController.setConstraints(turnConstraints);
303296
}
297+
298+
@Override
299+
public void initSendable(SendableBuilder builder) {
300+
builder.setActuator(true);
301+
builder.setSafeState(() -> setSpeed(0));
302+
builder.setSmartDashboardType("SwerveModule");
303+
builder.addDoubleProperty("Incremental Position", turnEncoder::getPosition, null);
304+
builder.addDoubleProperty("Absolute Angle (deg)", turnEncoder::getAbsolutePosition, null);
305+
builder.addDoubleProperty("Turn Measured Pos (deg)", this::getModuleAngle, null);
306+
builder.addDoubleProperty("Encoder Position", drive.getEncoder()::getPosition, null);
307+
// Display the speed that the robot thinks it is travelling at.
308+
builder.addDoubleProperty("Current Speed", this::getCurrentSpeed, null);
309+
builder.addDoubleProperty("Turn Setpoint Pos (deg)", () -> turnPIDController.getSetpoint().position, null);
310+
builder.addDoubleProperty("Turn Setpoint Vel (dps)", () -> turnPIDController.getSetpoint().velocity, null);
311+
builder.addDoubleProperty("Turn Goal Pos (deg)", () -> turnPIDController.getGoal().position, null);
312+
builder.addDoubleProperty("Turn Goal Vel (dps)", () -> turnPIDController.getGoal().velocity, null);
313+
builder.addDoubleProperty("Antigravitational Acceleration", () -> calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()), null);
314+
builder.addBooleanProperty("Turn is at Goal", turnPIDController::atGoal, null);
315+
builder.addDoubleProperty("Error (deg)", turnPIDController::getPositionError, null);
316+
builder.addDoubleProperty("Desired Speed (mps)", drivePIDController::getSetpoint, null);
317+
builder.addDoubleProperty("Angle Diff (deg)", () -> angleDiff, null);
318+
}
304319
}

0 commit comments

Comments
 (0)