spark max utilities

Author: CoolSpy3

src/main/java/frc/robot/lib/CachedSparkMax.java

@@ -0,0 +1,28 @@
+package frc.robot.lib;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
+
+public class CachedSparkMax extends CANSparkMax {
+
+    private RelativeEncoder encoder;
+    private SparkMaxPIDController pidController;
+
+    public CachedSparkMax(int deviceId, MotorType type) {
+        super(deviceId, type);
+        this.encoder = super.getEncoder();
+        this.pidController = super.getPIDController();
+    }
+
+    @Override
+    public RelativeEncoder getEncoder() {
+        return encoder;
+    }
+
+    @Override
+    public SparkMaxPIDController getPIDController() {
+        return pidController;
+    }
+
+}

src/main/java/frc/robot/lib/MotorControllerFactory.java

@@ -73,13 +73,13 @@ public static WPI_TalonSRX createTalon(int id) {
 
   public static CANSparkMax createSparkMax(int id) {
     CANSparkMax spark;
-    if (RobotBase.isReal()) { 
-        spark = new CANSparkMax(id, CANSparkMaxLowLevel.MotorType.kBrushless);
+    if (RobotBase.isReal()) {
+        spark = new CachedSparkMax(id, CANSparkMaxLowLevel.MotorType.kBrushless);
         if (spark.getFirmwareVersion() == 0) {
-            spark.close();
-            System.err.println("SparkMax on port: " + id + " is not connected!");
-            return MotorErrors.createDummySparkMax();
-          }
+          spark.close();
+          System.err.println("SparkMax on port: " + id + " is not connected!");
+          return MotorErrors.createDummySparkMax();
+        }
     } else {
         spark = MockSparkMax.createMockSparkMax(id, CANSparkMaxLowLevel.MotorType.kBrushless);
     }

src/main/java/frc/robot/lib/SparkVelocityPIDController.java

@@ -0,0 +1,100 @@
+package frc.robot.lib;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
+import com.revrobotics.CANSparkMax.ControlType;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class SparkVelocityPIDController {
+
+    private final CANSparkMax spark;
+    private final SparkMaxPIDController pidController;
+    private final RelativeEncoder encoder;
+    private final String name;
+    private double targetSpeed, tolerance;
+    private double currentP, currentI, currentD, kS, kV;
+
+    public SparkVelocityPIDController(String name, CANSparkMax spark, double defaultP, double defaultI, double defaultD, double kS, double kV, double targetSpeed, double tolerance) {
+        this.spark = spark;
+        this.pidController = spark.getPIDController();
+        this.encoder = spark.getEncoder();
+        this.name = name;
+        this.targetSpeed = targetSpeed;
+        this.tolerance = tolerance;
+        pidController.setP(this.currentP = defaultP);
+        pidController.setI(this.currentI = defaultI);
+        pidController.setD(this.currentD = defaultD);
+        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;
+    }
+
+    public boolean isAtTargetSpeed() {
+        return encoder.getVelocity() > targetSpeed - tolerance;
+    }
+
+    public String getName() {
+        return name;
+    }
+
+    public double getTargetSpeed() {
+        return targetSpeed;
+    }
+
+    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));
+    }
+
+    public double getTolerance() {
+        return tolerance;
+    }
+
+    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;
+    }
+
+}