make swerve and SparkVelocityPIDController Sendables

Author: CoolSpy3

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

@@ -1,14 +1,16 @@
 package org.carlmontrobotics.lib199;
 
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.ControlType;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkMaxPIDController;
-import com.revrobotics.CANSparkMax.ControlType;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
 
-public class SparkVelocityPIDController {
+public class SparkVelocityPIDController implements Sendable {
 
+    @SuppressWarnings("unused")
     private final CANSparkMax spark;
     private final SparkMaxPIDController pidController;
     private final RelativeEncoder encoder;
@@ -29,38 +31,9 @@ public SparkVelocityPIDController(String name, CANSparkMax spark, double default
         this.kS = kS;
         this.kV = kV;
 
-        SmartDashboard.putNumber(name + ": P", currentP);
-        SmartDashboard.putNumber(name + ": I", currentI);
-        SmartDashboard.putNumber(name + ": D", currentD);
-        SmartDashboard.putNumber(name + ": Target Speed", targetSpeed);
-        SmartDashboard.putNumber(name + ": Tolerance", tolerance);
-
         pidController.setReference(targetSpeed, ControlType.kVelocity, 0, calculateFF(targetSpeed));
     }
 
-    public void periodic() {
-        double p = SmartDashboard.getNumber(name + ": P", currentP);
-        double i = SmartDashboard.getNumber(name + ": I", currentI);
-        double d = SmartDashboard.getNumber(name + ": D", currentD);
-
-        if(p != currentP) {
-            pidController.setP(p);
-            currentP = p;
-        }
-        if(i != currentI) {
-            pidController.setI(i);
-            currentI = i;
-        }
-        if(d != currentD) {
-            pidController.setD(d);
-            currentD = d;
-        }
-
-        setTargetSpeed(SmartDashboard.getNumber(name + ": Target Speed", targetSpeed));
-        setTolerance(SmartDashboard.getNumber(name + ": Tolerance", tolerance));
-        SmartDashboard.putNumber(name + ": Current Speed", encoder.getVelocity());
-    }
-
     public RelativeEncoder getEncoder() {
         return encoder;
     }
@@ -79,7 +52,6 @@ public double getTargetSpeed() {
 
     public void setTargetSpeed(double targetSpeed) {
         if(targetSpeed == this.targetSpeed) return;
-        SmartDashboard.putNumber(name + ": Target Speed", targetSpeed);
         this.targetSpeed = targetSpeed;
         pidController.setReference(targetSpeed, ControlType.kVelocity, 0, calculateFF(targetSpeed));
     }
@@ -89,12 +61,36 @@ public double getTolerance() {
     }
 
     public void setTolerance(double tolerance) {
-        SmartDashboard.putNumber(name + ": Tolerance", tolerance);
         this.tolerance = tolerance;
     }
 
     public double calculateFF(double velocity) {
         return kS * Math.signum(velocity) + kV * velocity;
     }
 
+    @Override
+    public void initSendable(SendableBuilder builder) {
+        builder.setActuator(true);
+        builder.setSmartDashboardType("SparkVelocityPIDController");
+        builder.addDoubleProperty("P", () -> currentP, p -> {
+            pidController.setP(p);
+            currentP = p;
+        });
+        builder.addDoubleProperty("I", () -> currentI, i -> {
+            pidController.setI(i);
+            currentI = i;
+        });
+        builder.addDoubleProperty("D", () -> currentD, d -> {
+            pidController.setD(d);
+            currentD = d;
+        });
+        builder.addDoubleProperty("Target Speed", () -> targetSpeed, newSpeed -> {
+            if(newSpeed == targetSpeed) return;
+            pidController.setReference(newSpeed, ControlType.kVelocity, 0, calculateFF(newSpeed));
+            targetSpeed = newSpeed;
+        });
+        builder.addDoubleProperty("Tolerance", () -> tolerance, newTolerance -> tolerance = newTolerance);
+        builder.addDoubleProperty("Current Speed", encoder::getVelocity, null);
+    }
+
 }

…arlmontrobotics/lib199/SwerveConfig.java …robotics/lib199/swerve/SwerveConfig.javasrc/main/java/org/carlmontrobotics/lib199/SwerveConfig.java renamed to src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java src/main/java/org/carlmontrobotics/lib199/SwerveConfig.java renamed to src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java

@@ -1,4 +1,4 @@
-package org.carlmontrobotics.lib199;
+package org.carlmontrobotics.lib199.swerve;
 
 public final class SwerveConfig {
 

…arlmontrobotics/lib199/SwerveModule.java …robotics/lib199/swerve/SwerveModule.javasrc/main/java/org/carlmontrobotics/lib199/SwerveModule.java renamed to src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java src/main/java/org/carlmontrobotics/lib199/SwerveModule.java renamed to src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java

@@ -1,4 +1,4 @@
-package org.carlmontrobotics.lib199;
+package org.carlmontrobotics.lib199.swerve;
 
 import java.util.function.Supplier;
 
@@ -14,33 +14,34 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * A class that stores all the variables and methods applicaple to a single swerve module,
  * such as moving, getting encoder values, or configuring PID.
  */
-public class SwerveModule {
+public class SwerveModule implements Sendable {
     public enum ModuleType {FL, FR, BL, BR};
 
     private SwerveConfig config;
     private ModuleType type;
-    private String moduleString;
     private CANSparkMax drive, turn;
     private CANCoder turnEncoder;
     private PIDController drivePIDController;
     private ProfiledPIDController turnPIDController;
     private TrapezoidProfile.Constraints turnConstraints;
-    private double driveModifier, maxSpeed, turnZero;
+    private double driveModifier, turnZero;
     private Supplier<Float> pitchDegSupplier, rollDegSupplier;
     private boolean reversed;
     private Timer timer;
     private SimpleMotorFeedforward forwardSimpleMotorFF, backwardSimpleMotorFF, turnSimpleMotorFeedforward;
-    private double lastAngle, maxAchievableTurnVelocityDps, maxAchievableTurnAccelerationMps2, turnToleranceDeg;
+    private double lastAngle, maxAchievableTurnVelocityDps, maxAchievableTurnAccelerationMps2, turnToleranceDeg, angleDiff;
 
     public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CANSparkMax turn, CANCoder turnEncoder, double driveModifier,
-                        double maxSpeed, int arrIndex, Supplier<Float> pitchDegSupplier, Supplier<Float> rollDegSupplier) {
+                        int arrIndex, Supplier<Float> pitchDegSupplier, Supplier<Float> rollDegSupplier) {
         //SmartDashboard.putNumber("Target Angle (deg)", 0.0);
         this.timer = new Timer();
         timer.start();
@@ -49,21 +50,6 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN
         this.type = type;
         this.drive = drive;
 
-        switch (type) {
-            case FL:
-                moduleString = "FL";
-                break;
-            case FR:
-                moduleString = "FR";
-                break;
-            case BL:
-                moduleString = "BL";
-                break;
-            case BR:
-                moduleString = "BR";
-                break;
-        }
-
         double positionConstant = config.wheelDiameterMeters * Math.PI / config.driveGearing;
         drive.setInverted(config.driveInversion[arrIndex]);
         drive.getEncoder().setPositionConversionFactor(positionConstant);
@@ -76,7 +62,7 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN
         final double neoFreeCurrentAmps = 1.3;
         final double neoStallCurrentAmps = 166;
         double currentLimitAmps = neoFreeCurrentAmps + 2*motorTorqueLimitNewtonMeters / neoStallTorqueNewtonMeters * (neoStallCurrentAmps-neoFreeCurrentAmps);
-        SmartDashboard.putNumber(moduleString + " current limit (amps)", currentLimitAmps);
+        SmartDashboard.putNumber(type.toString() + " current limit (amps)", currentLimitAmps);
         drive.setSmartCurrentLimit((int)Math.min(50, currentLimitAmps));
 
         this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex],
@@ -120,7 +106,6 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN
         this.turnEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180);
 
         this.driveModifier = driveModifier;
-        this.maxSpeed = maxSpeed;
         this.reversed = config.reversed[arrIndex];
         this.turnZero = config.turnZero[arrIndex];
 
@@ -147,9 +132,8 @@ public void periodic() {
             turn.setVoltage(MathUtil.clamp(turnVolts, -12.0, 12.0));
         } else {
             turn.setVoltage(turnSimpleMotorFeedforward.calculate(goal.velocity));
-        }        
+        }
         prevTurnVelocity = state.velocity;
-        SmartDashboard.putNumber(moduleString + " error (deg)", turnPIDController.getPositionError());
     }
 
     /**
@@ -192,8 +176,6 @@ private void setSpeed(double speedMps) {
         // Compute desired and actual speeds in m/s
         double desiredSpeed = speedMps * driveModifier;
         double actualSpeed = getCurrentSpeed();
-        SmartDashboard.putNumber(moduleString + " Desired Speed (mps)", desiredSpeed);
-        SmartDashboard.putNumber(moduleString + " Actual Speed (mps)", actualSpeed);
         double targetVoltage = (actualSpeed >= 0 ? forwardSimpleMotorFF :
                                  backwardSimpleMotorFF).calculate(desiredSpeed, calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()));//clippedAcceleration);
 
@@ -215,12 +197,12 @@ private void setAngle(double angle) {
         double maxDeltaTheta = Math.asin(deltaTime*config.autoCentripetalAccel/(Math.abs(getCurrentSpeed())+0.0001));
         setMaxTurnVelocity(maxDeltaTheta*180/Math.PI);
         //SmartDashboard.putNumber(moduleString + "Target Angle:", 360 * angle * (reversed ? -1 : 1));
-		
+
         // Find the minimum distance to travel from lastAngle to angle and determine the
         // correct direction to trvel the minimum distance. This is used in order to accurately
         // calculate the goal velocity.
-        double angleDiff = MathUtil.inputModulus(angle - lastAngle, -180, 180);
-        SmartDashboard.putNumber(moduleString + " angleDiff (deg)", angleDiff);
+        angleDiff = MathUtil.inputModulus(angle - lastAngle, -180, 180);
+        // SmartDashboard.putNumber(moduleString + " angleDiff (deg)", angleDiff);
 
         turnPIDController.setGoal(new TrapezoidProfile.State(angle * (reversed ? -1 : 1), 0));
         lastAngle = angle;
@@ -248,8 +230,12 @@ public double getCurrentSpeed() {
 
     /**
      * Updates SmartDashboard with information about this module.
+     *
+     * @deprecated Put this Sendable to SmartDashboard instead
      */
+    @Deprecated
     public void updateSmartDashboard() {
+        String moduleString = type.toString();
         // Display the position of the quadrature encoder.
         SmartDashboard.putNumber(moduleString + " Incremental Position", turnEncoder.getPosition());
         // Display the position of the analog encoder.
@@ -268,7 +254,7 @@ public void updateSmartDashboard() {
         SmartDashboard.putNumber(moduleString + "Antigravitational Acceleration", calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()));
         SmartDashboard.putBoolean(moduleString + " Turn is at Goal", turnPIDController.atGoal());
     }
-    
+
     public void toggleMode() {
         if (drive.getIdleMode() == IdleMode.kBrake && turn.getIdleMode() == IdleMode.kCoast) coast();
         else brake();
@@ -288,4 +274,26 @@ public void setMaxTurnVelocity(double maxVel) {
         turnConstraints = new TrapezoidProfile.Constraints(maxVel, turnConstraints.maxAcceleration);
         turnPIDController.setConstraints(turnConstraints);
     }
-}
+
+    @Override
+    public void initSendable(SendableBuilder builder) {
+        builder.setActuator(true);
+        builder.setSafeState(() -> setSpeed(0));
+        builder.setSmartDashboardType("SwerveModule");
+        builder.addDoubleProperty("Incremental Position", turnEncoder::getPosition, null);
+        builder.addDoubleProperty("Absolute Angle (deg)", turnEncoder::getAbsolutePosition, null);
+        builder.addDoubleProperty("Turn Measured Pos (deg)", this::getModuleAngle, null);
+        builder.addDoubleProperty("Encoder Position", drive.getEncoder()::getPosition, null);
+        // Display the speed that the robot thinks it is travelling at.
+        builder.addDoubleProperty("Current Speed", this::getCurrentSpeed, null);
+        builder.addDoubleProperty("Turn Setpoint Pos (deg)", () -> turnPIDController.getSetpoint().position, null);
+        builder.addDoubleProperty("Turn Setpoint Vel (dps)", () -> turnPIDController.getSetpoint().velocity, null);
+        builder.addDoubleProperty("Turn Goal Pos (deg)", () -> turnPIDController.getGoal().position, null);
+        builder.addDoubleProperty("Turn Goal Vel (dps)", () -> turnPIDController.getGoal().velocity, null);
+        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("Angle Diff (deg)", () -> angleDiff, null);
+    }
+}