Adding ArmSysID and some more configuration stuff

src/main/java/frc/robot/constants/ArmConstants.java

@@ -1,14 +1,24 @@
 package frc.robot.constants;
 
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Seconds;
+
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
 public class ArmConstants {
     public static final int kArmMotorID = 0;
     public static final int kCANcoderID = 0;
 
+    public static final int kMotorAmpsMax = 0;
+
     public static final double kArmMaxVelocity = 0;
 
     public static final double kPositionalP = 0;
@@ -31,11 +41,39 @@ public class ArmConstants {
     public static final double kArmL3AlgaePosition = 0;
     public static final double kArmSafeStowPosition = 0;
 
+    public static final double kMagnetOffset = 0.0;
+    public static final double kAbsoluteSensorDiscontinuityPoint = 0.0;
+
+    public static final double kSysIDRampRate = 1;
+    public static final double kSysIDStepVolts = 7;
+    public static final double kSysIDTimeout = 10;
+
+    public static final SensorDirectionValue kSensorDirection = SensorDirectionValue.CounterClockwise_Positive;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIG
+
+    public static final SysIdRoutine.Config kSysIDConfig = new Config(
+        Volts.of(kSysIDRampRate).per(Second),
+        Volts.of(kSysIDStepVolts),
+        Seconds.of(kSysIDTimeout)
+    );
+
     public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
 
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+
     static {
         canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
         canCoderConfig.MagnetSensor.MagnetOffset = 0.0;
-        canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;
+        // TODO Need to do more reading on this setting, and how to properly offset the Arm so that horizontal is 0
+        //canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;
+
+        motorConfig
+            .smartCurrentLimit(kMotorAmpsMax)
+            .idleMode(kIdleMode);
     }
+
+
 }

src/main/java/frc/robot/subsystems/Arm.java

@@ -4,6 +4,8 @@ import java.util.function.DoubleSupplier;
 
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.controller.ArmFeedforward;
@@ -14,7 +16,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ArmConstants;
 
 public class Arm extends SubsystemBase {
-    private SparkMax armMotor;
+    protected SparkMax armMotor;
 
     private CANcoder canCoder;
 
@@ -29,6 +31,8 @@ public class Arm extends SubsystemBase {
             MotorType.kBrushless
         );
 
+        armMotor.configure(ArmConstants.motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
         positionController = new PIDController(
             ArmConstants.kPositionalP, 
             ArmConstants.kPositionalI, 

src/main/java/frc/robot/subsystems/sysid/ArmSysID.java

@@ -0,0 +1,65 @@
+package frc.robot.subsystems.sysid;
+
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.MutAngle;
+import edu.wpi.first.units.measure.MutAngularVelocity;
+import edu.wpi.first.units.measure.MutDistance;
+import edu.wpi.first.units.measure.MutLinearVelocity;
+import edu.wpi.first.units.measure.MutVoltage;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.constants.ArmConstants;
+import frc.robot.subsystems.Arm;
+
+public class ArmSysID extends Arm {
+    private MutVoltage appliedVoltage;
+
+    private MutAngle armPosition;
+
+    private MutAngularVelocity armVelocity;
+
+    private SysIdRoutine routine;
+
+    public ArmSysID() {
+        super();
+
+        appliedVoltage = Volts.mutable(0);
+
+        armPosition = Radians.mutable(0);
+
+        armVelocity = RadiansPerSecond.mutable(0);
+
+        routine = new SysIdRoutine(
+            ArmConstants.kSysIDConfig,
+            new SysIdRoutine.Mechanism(
+                armMotor::setVoltage, 
+                (log) -> {
+                    log.motor("armMotor")
+                        .voltage(appliedVoltage.mut_replace(
+                            armMotor.get() * RobotController.getBatteryVoltage(), Volts
+                        ))
+                        .angularPosition(armPosition.mut_replace(
+                            getEncoderPosition(), Radians
+                        ))
+                        .angularVelocity(armVelocity.mut_replace(
+                            getEncoderVelocity(), RadiansPerSecond
+                        ));
+                }, 
+                this
+            )
+        );
+    }
+
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return routine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return routine.dynamic(direction);
+    }
+}