Recharacterize arm

src/main/java/frc/robot/RobotContainer.java

@@ -174,7 +174,7 @@ public RobotContainer() {
     autoChooser.addOption(
         "Arm Quasistatic Forward", arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
     autoChooser.addOption("Arm Dynamic Forward", arm.sysIdDynamic(SysIdRoutine.Direction.kForward));
-    autoChooser.addOption("Arm get static current", arm.getKs());
+    autoChooser.addOption("Arm get static current", arm.getStaticCurrent());
 
     // Testing autos paths
     Function<File, Optional<Command>> trajectoryCommandFactory =

src/main/java/frc/robot/RobotState.java

@@ -35,7 +35,7 @@ public static RobotState getInstance() {
   private Pose2d estimatedPose = new Pose2d();
   private final TimeInterpolatableBuffer<Pose2d> poseBuffer =
       TimeInterpolatableBuffer.createBuffer(poseBufferSizeSeconds);
-  private final Matrix<N3, N1> odometryStdDevs = new Matrix<>(Nat.N3(), Nat.N1());
+  private final Matrix<N3, N1> qStdDevs = new Matrix<>(Nat.N3(), Nat.N1());
   // odometry
   private final SwerveDriveKinematics kinematics;
   private SwerveDriveWheelPositions lastWheelPositions =
@@ -50,7 +50,9 @@ public static RobotState getInstance() {
   private Twist2d robotVelocity = new Twist2d();
 
   private RobotState() {
-    odometryStdDevs.setColumn(0, DriveConstants.odometryStateStdDevs.extractColumnVector(0));
+    for (int i = 0; i < 3; ++i) {
+      qStdDevs.set(i, 0, Math.pow(qStdDevs.get(i, 0), 2));
+    }
     kinematics = DriveConstants.kinematics;
   }
 
@@ -106,7 +108,7 @@ public void addVisionObservation(VisionObservation observation) {
     // and C = I. See wpimath/algorithms.md.
     Matrix<N3, N3> visionK = new Matrix<>(Nat.N3(), Nat.N3());
     for (int row = 0; row < 3; ++row) {
-      double stdDev = odometryStdDevs.get(row, 0);
+      double stdDev = qStdDevs.get(row, 0);
       if (stdDev == 0.0) {
         visionK.set(row, row, 0.0);
       } else {

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

@@ -191,9 +191,9 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
     return routine.dynamic(direction).finallyDo(SignalLogger::stop);
   }
 
-  public Command getKs() {
+  public Command getStaticCurrent() {
     Timer timer = new Timer();
-    return run(() -> armIO.setCurrent(0.1 * timer.get()))
+    return run(() -> armIO.setCurrent(0.5 * timer.get()))
         .beforeStarting(timer::restart)
         .until(() -> Math.abs(inputs.armVelocityRadsPerSec) >= Units.degreesToRadians(10))
         .andThen(() -> Logger.recordOutput("Arm/staticCurrent", inputs.armTorqueCurrentAmps[0]))

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

@@ -34,10 +34,8 @@ public ArmIOKrakenFOC() {
 
     // Leader motor configs
     TalonFXConfiguration leaderConfig = new TalonFXConfiguration();
-    leaderConfig.CurrentLimits.StatorCurrentLimit = 60.0;
     leaderConfig.CurrentLimits.SupplyCurrentLimit = 40.0;
-    //    leaderConfig.Voltage.PeakForwardVoltage = 4.0;
-    //    leaderConfig.Voltage.PeakReverseVoltage = -4.0;
+    leaderConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
     leaderConfig.MotorOutput.Inverted =
         leaderInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;
     leaderConfig.Feedback.SensorToMechanismRatio = reduction;

src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java

@@ -95,13 +95,13 @@ public static class ArmConstants {
     public static ControllerConstants controllerConstants =
         switch (Constants.getRobot()) {
           case SIMBOT -> new ControllerConstants(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
-          case RAINBOWT -> new ControllerConstants(1200, 0.0,120, 1.71, 0.0, 0.0, 10.79);
+          case RAINBOWT -> new ControllerConstants(1200, 0.0, 120, 6.22, 0.0, 0.0, 8.12);
           case COMPBOT -> new ControllerConstants(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
         };
 
     public static ProfileConstraints profileConstraints =
         switch (Constants.getRobot()) {
-          default -> new ProfileConstraints(2.0 * Math.PI, 2.0 * Math.PI);
+          default -> new ProfileConstraints(2.0 * Math.PI, 10);
         };
 
     public record ProfileConstraints(