Motor/PIDSubsystem changes

Fixed motor bugs, fixed PIDSubsystem bug which broke code
Team3128 · Sep 25, 2023 · af64043 · af64043
1 parent 55f8d4d
commit af64043
Show file tree

Hide file tree

Showing 8 changed files with 54 additions and 47 deletions.
diff --git a/src/main/java/common/core/NAR_Robot.java b/src/main/java/common/core/NAR_Robot.java
@@ -14,7 +14,6 @@
 import edu.wpi.first.wpilibj.Timer;
 
 public class NAR_Robot extends IterativeRobotBase {
-
     @SuppressWarnings("MemberName")
   static class Callback implements Comparable<Callback> {
     public Runnable func;

diff --git a/src/main/java/common/utility/Controller.java → ...a/common/core/controllers/Controller.java b/src/main/java/common/utility/Controller.java → ...a/common/core/controllers/Controller.java
@@ -1,4 +1,4 @@
-package common.utility;
+package common.core.controllers;
 
 import java.util.LinkedList;
 import java.util.function.DoubleConsumer;
@@ -12,8 +12,8 @@ public static class VController extends Controller {
 
         public VController(double kS, double kV, double kP, double kI, double kD, double period) {
             super(kP, kI, kD, period);
-            this.kS = kS;
-            this.kV = kV;
+            this.kS = ()-> kS;
+            this.kV = ()-> kV;
         }
 
         public VController(double kS, double kV, double kP, double kI, double kD) {
@@ -23,7 +23,8 @@ public VController(double kS, double kV, double kP, double kI, double kD) {
         @Override
         public double calculate(double measurement) {
             final double pid = super.calculate(measurement);
-            return pid + Math.copySign(kS, pid) + kV * getSetpoint();
+            final double ff = !atSetpoint() ? Math.copySign(getkS(), pid) : 0 + getkV() * getSetpoint();
+            return pid + ff;
         }
 
     }
@@ -32,13 +33,13 @@ public static class PController extends Controller {
 
         public PController(double kS, double kG, double kP, double kI, double kD, double period) {
             super(kP, kI, kD, period);
-            this.kS = kS;
-            this.kG = kG;
+            this.kS = ()-> kS;
+            this.kG = ()-> kG;
             kG_Function = () -> 1;
         }
 
-        public PController(double kS, double kV, double kP, double kI, double kD) {
-            this(kS, kV, kP, kI, kD, 0.02);
+        public PController(double kS, double kG, double kP, double kI, double kD) {
+            this(kS, kG, kP, kI, kD, 0.02);
         }
 
         public void setkG_Function(DoubleSupplier kG_Function) {
@@ -48,22 +49,23 @@ public void setkG_Function(DoubleSupplier kG_Function) {
         @Override
         public double calculate(double measurement) {
             final double pid = super.calculate(measurement);
-            return pid + Math.copySign(kS, pid) + kG * kG_Function.getAsDouble();
+            final double ff = !atSetpoint() ? Math.copySign(getkS(), pid) : 0 + getkG() * kG_Function.getAsDouble();
+            return pid + ff;
         }
 
     }
 
     private final LinkedList<DoubleConsumer> consumers;
     private DoubleSupplier measurement;
 
-    protected double kS, kV, kG;
+    protected DoubleSupplier kS, kV, kG;
     protected DoubleSupplier kG_Function;
 
     public Controller(double kP, double kI, double kD, double period) {
         super(kP, kI, kD, period);
-        kS = 0;
-        kV = 0;
-        kG = 0;
+        kS = ()-> 0;
+        kV = ()-> 0;
+        kG = ()-> 0;
         kG_Function = () -> 1;
         consumers = new LinkedList<DoubleConsumer>();
     }
@@ -98,26 +100,38 @@ public void setkG_Function(DoubleSupplier kG_Function) {
     }
 
     public void setkS(double kS) {
+        setkS(()-> kS);
+    }
+
+    public void setkS(DoubleSupplier kS) {
         this.kS = kS;
     }
 
     public void setkV(double kV) {
+        setkV(()-> kV);
+    }
+
+    public void setkV(DoubleSupplier kV) {
         this.kV = kV;
     }
 
     public void setkG(double kG) {
+        this.kG = ()-> kG;
+    }
+
+    public void setkG(DoubleSupplier kG) {
         this.kG = kG;
     }
 
     public double getkS() {
-        return kS;
+        return kS.getAsDouble();
     }
 
     public double getkV() {
-        return kV;
+        return kV.getAsDouble();
     }
 
     public double getkG() {
-        return kG;
+        return kG.getAsDouble();
     }
 }
diff --git a/...a/common/subsystems/NAR_PIDSubsystem.java → ...mon/core/subsystems/NAR_PIDSubsystem.java b/...a/common/subsystems/NAR_PIDSubsystem.java → ...mon/core/subsystems/NAR_PIDSubsystem.java
@@ -1,12 +1,12 @@
-package common.subsystems;
+package common.core.subsystems;
 
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
-import common.utility.Controller;
+import common.core.controllers.Controller;
+import common.core.controllers.Controller.PController;
+import common.core.controllers.Controller.VController;
 import common.utility.NAR_Shuffleboard;
-import common.utility.Controller.PController;
-import common.utility.Controller.VController;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.PIDSubsystem;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -19,7 +19,6 @@
 public abstract class NAR_PIDSubsystem extends SubsystemBase {
     protected final Controller m_controller;
     protected boolean m_enabled;
-    private DoubleSupplier kS, kV, kG;
     private BooleanSupplier debug;
     private DoubleSupplier setpoint;
     private double min, max;
@@ -42,12 +41,6 @@ public void periodic() {
         if (m_enabled) {
             m_controller.useOutput();
         }
-
-        if (kS.getAsDouble() != m_controller.getkS()) m_controller.setkS(kS.getAsDouble());
-        if (kV != null) 
-            if (kV.getAsDouble() != m_controller.getkV()) m_controller.setkV(kV.getAsDouble());
-        if (kG != null) 
-            if (kG.getAsDouble() != m_controller.getkG()) m_controller.setkG(kG.getAsDouble());
     }
 
     public void initShuffleboard() {
@@ -63,12 +56,12 @@ public void initShuffleboard() {
         setpoint = NAR_Shuffleboard.debug(getName(), "Debug_Setpoint", 0, 2,2);
 
         if (m_controller instanceof VController) {
-            this.kS = NAR_Shuffleboard.debug(getName(), "kS", m_controller.getkS(), 3, 0);
-            this.kV = NAR_Shuffleboard.debug(getName(), "kV", m_controller.getkV(), 3, 1);
+            m_controller.setkS(NAR_Shuffleboard.debug(getName(), "kS", m_controller.getkS(), 3, 0));
+            m_controller.setkV(NAR_Shuffleboard.debug(getName(), "kV", m_controller.getkV(), 3, 1));
         }
         if (m_controller instanceof PController) {
-            this.kS = NAR_Shuffleboard.debug(getName(), "kS", m_controller.getkS(), 3, 0);
-            this.kG = NAR_Shuffleboard.debug(getName(), "kG", m_controller.getkV(), 3, 1);
+            m_controller.setkS(NAR_Shuffleboard.debug(getName(), "kS", m_controller.getkS(), 3, 0));
+            m_controller.setkG(NAR_Shuffleboard.debug(getName(), "kG", m_controller.getkG(), 3, 1));
         }
     }
 
@@ -81,6 +74,15 @@ public Controller getController() {
         return m_controller;
     }
 
+    /**
+     * Sets the error which is considered tolerable for use with atSetpoint().
+     *
+     * @param positionTolerance Position error which is tolerable.
+     */
+    public void setTolerance(double positionTolerance) {
+        m_controller.setTolerance(positionTolerance);
+    }
+
     /**
      * Sets constraints for the setpoint of the PID subsystem.
      * @param min The minimum setpoint for the subsystem

diff --git a/.../subsystems/NAR_ProfiledPIDSubsystem.java → .../subsystems/NAR_ProfiledPIDSubsystem.java b/.../subsystems/NAR_ProfiledPIDSubsystem.java → .../subsystems/NAR_ProfiledPIDSubsystem.java
@@ -1,4 +1,4 @@
-package common.subsystems;
+package common.core.subsystems;
 
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;

diff --git a/...n/subsystems/NAR_StateSpaceSubsystem.java → ...e/subsystems/NAR_StateSpaceSubsystem.java b/...n/subsystems/NAR_StateSpaceSubsystem.java → ...e/subsystems/NAR_StateSpaceSubsystem.java
@@ -1,4 +1,4 @@
-package common.subsystems;
+package common.core.subsystems;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.system.LinearSystemLoop;

diff --git a/src/main/java/common/hardware/motorcontroller/NAR_CANSparkMax.java b/src/main/java/common/hardware/motorcontroller/NAR_CANSparkMax.java
@@ -54,7 +54,7 @@ public enum EncoderType {
 	private SparkMaxRelativeEncoder relativeEncoder;
 	private SparkMaxAbsoluteEncoder absoluteEncoder;
 	private final SparkMaxPIDController controller;
-    private final CANSparkMax motor;
+    protected final CANSparkMax motor;
 
     /**
 	 * Create a new object to control a SPARK MAX motor
@@ -186,15 +186,6 @@ public void setCurrentLimit(int limit) {
 		motor.setSmartCurrentLimit(limit);
 	}
 
-	/**
-   	 * Enables the brake mode setting for the SPARK MAX.
-  	 *
-   	 * @param enable true for brake, false for coast
-   	 */
-	public void setBrakeMode(boolean enable) {
-		motor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
-	}
-
     /**
 	 * Set the rate of transmission for periodic frames from the SPARK MAX
 	 *

diff --git a/src/main/java/common/hardware/motorcontroller/NAR_TalonFX.java b/src/main/java/common/hardware/motorcontroller/NAR_TalonFX.java
@@ -22,7 +22,7 @@
  */
 public class NAR_TalonFX extends NAR_Motor{
 
-    private final WPI_TalonFX motor;
+    protected final WPI_TalonFX motor;
     private double kP, kI, kD;
 
     /**
@@ -38,6 +38,7 @@ public NAR_TalonFX(int deviceNumber, String canBus, double kP, double kI, double
         motor = new WPI_TalonFX(deviceNumber, canBus);
 
         enableVoltageCompensation(12);
+		motor.configFactoryDefault();
 
         this.kP = kP;
 		this.kI = kI;

diff --git a/src/main/java/common/hardware/motorcontroller/NAR_TalonSRX.java b/src/main/java/common/hardware/motorcontroller/NAR_TalonSRX.java
@@ -8,7 +8,7 @@
 
 public class NAR_TalonSRX extends NAR_Motor {
 
-	private WPI_TalonSRX motor;
+	protected WPI_TalonSRX motor;
 
 	/**
 	 * @param deviceNumber device id
@@ -20,7 +20,7 @@ public NAR_TalonSRX(int deviceNumber) {
 	@Override
 	public void enableVoltageCompensation(double volts) {
 		motor.enableVoltageCompensation(true);
-		motor.configVoltageCompSaturation(12);
+		motor.configVoltageCompSaturation(volts);
 	}
 
 	@Override