SwerveBase code

Added Swerve to common repo

src/main/java/common/core/controllers/Controller.java

@@ -24,59 +24,24 @@ public enum Type {
      * Create a new object to control PID + FF logic for a subsystem.
      * <p>Sets kP, kI, kD, kS, kV, kG, period values.
      * 
-     * @param kP The proportional coefficient.
-     * @param kI The integral coefficient.
-     * @param kD The derivative coefficient.
-     * @param kS The static gain.
-     * @param kV The velocity gain.
-     * @param kG The gravity gain.
+     * @param config PIDFFConfig object containing PID and Feedforward constants.
      * @param type The type of setpoint used by the controller
      * @param period The controller's update rate in seconds. Must be non-zero and positive.
      */
-    public Controller(double kP, double kI, double kD, double kS, double kV, double kG, Type type, double period) {
-        super(kP, kI, kD, kS, kV, 0, kG, period);
+    public Controller(PIDFFConfig config, Type type, double period) {
+        super(config, period);
         this.type = type;
     }
 
     /**
      * Create a new object to control PID + FF logic for a subsystem.
      * <p>Sets kP, kI, kD, kS, kV, kG values.
      * 
-     * @param kP The proportional coefficient.
-     * @param kI The integral coefficient.
-     * @param kD The derivative coefficient.
-     * @param kS The static gain.
-     * @param kV The velocity gain.
-     * @param kG The gravity gain.
+     * @param config PIDFFConfig object containing PID and Feedforward constants.
      * @param type The type of setpoint used by the controller
      */
-    public Controller(double kP, double kI, double kD, double kS, double kV, double kG, Type type) {
-        this(kP, kI, kD, kS, kV, kG, type, 0.02);
-    }
-
-    /**
-     * Create a new object to control PID logic for a subsystem.
-     * <p>Sets kP, kI, kD, period values.
-     * 
-     * @param kP The proportional coefficient.
-     * @param kI The integral coefficient.
-     * @param kD The derivative coefficient.
-     * @param period The controller's update rate in seconds. Must be non-zero and positive.
-     */
-    public Controller(double kP, double kI, double kD, double period) {
-        this(kP, kI, kD, 0, 0, 0, Type.POSITION, period);
-    }
-
-    /**
-     * Create a new object to control PID logic for a subsystem.
-     * <p>Sets kP, kI, kD values.
-     * 
-     * @param kP The proportional coefficient.
-     * @param kI The integral coefficient.
-     * @param kD The derivative coefficient.
-     */
-    public Controller(double kP, double kI, double kD) {
-        this(kP, kI, kD, 0.02);
+    public Controller(PIDFFConfig config, Type type) {
+        this(config, type, 0.02);
     }
 
     /**

src/main/java/common/core/controllers/ControllerBase.java

@@ -28,21 +28,15 @@ public abstract class ControllerBase implements Sendable, AutoCloseable {
     /**
      * Creates a base controller object to control motion.
      * <p>Sets kP, kI, kD, kS, kV, kA, kG, constraints, period values.
-     * @param kP The proportional coefficient.
-     * @param kI The integral coefficient.
-     * @param kD The derivative coefficient.
-     * @param kS The static gain.
-     * @param kV The velocity gain.
-     * @param kA The acceleration gain.
-     * @param kG The gravity gain.
+     * @param config PIDFFConfig object containing PID and Feedforward constants.
      * @param period The controller's update rate in seconds. Must be non-zero and positive.
      */
-    public ControllerBase(double kP, double kI, double kD, double kS, double kV, double kA, double kG, double period) {
-        controller = new PIDController(kP, kI, kD, period);
-        this.kS = ()-> kS;
-        this.kV = ()-> kV;
-        this.kA = ()-> kA;
-        this.kG = ()-> kG;
+    public ControllerBase(PIDFFConfig config, double period) {
+        controller = new PIDController(config.kP, config.kI, config.kD, period);
+        this.kS = ()-> config.kS;
+        this.kV = ()-> config.kV;
+        this.kA = ()-> config.kA;
+        this.kG = ()-> config.kG;
 
         kG_Function = ()-> 1;
     }

src/main/java/common/core/controllers/PIDFFConfig.java

@@ -0,0 +1,58 @@
+package common.core.controllers;
+
+/**
+ * Stores PID and feedforward constants.
+ */
+public class PIDFFConfig {
+
+    public double kP;
+    public double kI;
+    public double kD;
+    public double kS;
+    public double kV;
+    public double kA;
+    public double kG;
+
+    /**
+     * Creates a config to set PID and feedforward constants.
+     * @param kP The proportional coefficient.
+     * @param kI The integral coefficient.
+     * @param kD The derivative coefficient.
+     * @param kS The static gain.
+     * @param kV The velocity gain.
+     * @param kA The acceleration gain.
+     * @param kG The gravity gain.
+     */
+    public PIDFFConfig(double kP, double kI, double kD, double kS, double kV, double kA, double kG) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kS = kS;
+        this.kV = kV;
+        this.kA = kA;
+        this.kG = kG;
+    }
+
+    /**
+     * Creates a config to set PID and feedforward constants.
+     * @param kP The proportional coefficient.
+     * @param kI The integral coefficient.
+     * @param kD The derivative coefficient.
+     * @param kS The static gain.
+     * @param kV The velocity gain.
+     * @param kG The gravity gain.
+     */
+    public PIDFFConfig(double kP, double kI, double kD, double kS, double kV, double kG) {
+        this(kP, kI, kD, kS, kV, 0, kG);
+    }
+
+    /**
+     * Creates a config to set PID constants.
+     * @param kP The proportional coefficient.
+     * @param kI The integral coefficient.
+     * @param kD The derivative coefficient.
+     */
+    public PIDFFConfig(double kP, double kI, double kD) {
+        this(kP, kI, kD, 0, 0, 0, 0);
+    }
+}

src/main/java/common/core/controllers/TrapController.java

@@ -26,18 +26,12 @@ public class TrapController extends ControllerBase {
      * Create a new object to control PID + FF logic using a trapezoid profile for a subsystem.
      * <p>Sets kP, kI, kD, kS, kV, kA, kG, constraints, period values.
      * 
-     * @param kP The proportional coefficient.
-     * @param kI The integral coefficient.
-     * @param kD The derivative coefficient.
-     * @param kS The static gain.
-     * @param kV The velocity gain.
-     * @param kA The acceleration gain.
-     * @param kG The gravity gain.
+     * @param config PIDFFConfig object containing PID and Feedforward constants.
      * @param constraints Constraints for max acceleration and velocity
      * @param period The controller's update rate in seconds. Must be non-zero and positive.
      */
-    public TrapController(double kP, double kI, double kD, double kS, double kV, double kA, double kG, TrapezoidProfile.Constraints constraints, double period) {
-        super(kP, kI, kD, kS, kV, kA, kG, period);
+    public TrapController(PIDFFConfig config, TrapezoidProfile.Constraints constraints, double period) {
+        super(config, period);
         this.constraints = constraints;
         systemVelocity = ()-> 0;
     }
@@ -46,44 +40,11 @@ public TrapController(double kP, double kI, double kD, double kS, double kV, dou
      * Create a new object to control PID + FF logic using a trapezoid profile for a subsystem.
      * <p>Sets kP, kI, kD, kS, kV, kA, kG, constraints values.
      * 
-     * @param kP The proportional coefficient.
-     * @param kI The integral coefficient.
-     * @param kD The derivative coefficient.
-     * @param kS The static gain.
-     * @param kV The velocity gain.
-     * @param kA The acceleration gain.
-     * @param kG The gravity gain.
+     * @param config PIDFFConfig object containing PID and Feedforward constants.
      * @param constraints Constraints for max acceleration and velocity
      */
-    public TrapController(double kP, double kI, double kD, double kS, double kV, double kA, double kG, TrapezoidProfile.Constraints constraints) {
-        this(kP, kI, kD, kS, kV, kA, kG, constraints, 0.02);
-    }
-
-    /**
-     * Create a new object to control PID logic using a trapezoid profile for a subsystem.
-     * <p>Sets kP, kI, kD, constraints, period values.
-     * 
-     * @param kP The proportional coefficient.
-     * @param kI The integral coefficient.
-     * @param kD The derivative coefficient.
-     * @param constraints Constraints for max acceleration and velocity
-     * @param period The controller's update rate in seconds. Must be non-zero and positive
-     */
-    public TrapController(double kP, double kI, double kD, TrapezoidProfile.Constraints constraints, double period) {
-        this(kP, kI, kD, 0, 0, 0, 0, constraints, period);
-    }
-
-    /**
-     * Create a new object to control PID logic using a trapezoid profile for a subsystem.
-     * <p>Sets kP, kI, kD, constraints, values.
-     * 
-     * @param kP The proportional coefficient.
-     * @param kI The integral coefficient.
-     * @param kD The derivative coefficient.
-     * @param constraints Constraints for max acceleration and velocity
-     */
-    public TrapController(double kP, double kI, double kD, TrapezoidProfile.Constraints constraints) {
-        this(kP, kI, kD, constraints, 0.02);
+    public TrapController(PIDFFConfig config, TrapezoidProfile.Constraints constraints) {
+        this(config, constraints, 0.02);
     }
 
     /**

src/main/java/common/core/NAR_Robot.java → src/main/java/common/core/misc/NAR_Robot.java

@@ -1,4 +1,4 @@
-package common.core;
+package common.core.misc;
 
 import java.lang.reflect.Method;
 import java.util.PriorityQueue;

src/main/java/common/core/swerve/CTREConfigs.java

@@ -1,9 +1,5 @@
 package common.core.swerve;
 
-import static common.core.swerve.SwerveConstants.*;
-
-import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.sensors.AbsoluteSensorRange;
 import com.ctre.phoenix.sensors.CANCoderConfiguration;
 import com.ctre.phoenix.sensors.SensorInitializationStrategy;
@@ -13,46 +9,10 @@
  * Team 3128's defaults
  */
 public class CTREConfigs {
-
-    public static TalonFXConfiguration swerveDriveFXConfig() {
-        TalonFXConfiguration config = new TalonFXConfiguration();
-        SupplyCurrentLimitConfiguration driveSupplyLimit = new SupplyCurrentLimitConfiguration(
-            driveEnableCurrentLimit, 
-            driveContinuousCurrentLimit, 
-            drivePeakCurrentLimit, 
-            drivePeakCurrentDuration);
-
-        config.slot0.kP = driveKP;
-        config.slot0.kI = driveKI;
-        config.slot0.kD = driveKD;
-        config.slot0.kF = driveKF;        
-        config.supplyCurrLimit = driveSupplyLimit;
-        config.initializationStrategy = SensorInitializationStrategy.BootToZero;
-        config.closedloopRamp = closedLoopRamp;
-        return config;
-    }
-
-    public static TalonFXConfiguration swerveAngleFXConfig() {
-        TalonFXConfiguration angleConfig = new TalonFXConfiguration();
-        SupplyCurrentLimitConfiguration angleSupplyLimit = new SupplyCurrentLimitConfiguration(
-            angleEnableCurrentLimit, 
-            angleContinuousCurrentLimit, 
-            anglePeakCurrentLimit, 
-            anglePeakCurrentDuration);
-
-        angleConfig.slot0.kP = angleKP;
-        angleConfig.slot0.kI = angleKI;
-        angleConfig.slot0.kD = angleKD;
-        angleConfig.slot0.kF = angleKF;
-        angleConfig.supplyCurrLimit = angleSupplyLimit;
-        angleConfig.initializationStrategy = SensorInitializationStrategy.BootToZero;
-        return angleConfig;
-    }
 
     public static CANCoderConfiguration swerveCancoderConfig() {
         CANCoderConfiguration config = new CANCoderConfiguration();
         config.absoluteSensorRange = AbsoluteSensorRange.Signed_PlusMinus180; // used to be unsigned 
-        config.sensorDirection = SwerveConstants.canCoderInvert;         //Changes cc vs cw as positive angle
         config.initializationStrategy = SensorInitializationStrategy.BootToAbsolutePosition;
         config.sensorTimeBase = SensorTimeBase.PerSecond;
         return config;