Arm fixed, turned out it was a gearing conversion.

Author: spderman3333

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

@@ -16,8 +16,6 @@ public class Arm extends SubsystemBase {
     private final ArmIO io;
     private final ArmIOInputsAutoLogged replayedInputs = new ArmIOInputsAutoLogged();
 
-    private ArmPositions currentArmPosition = ArmPositions.NORTH;
-
     public Arm(ArmIO io) {
         this.io = io;
     }

src/main/java/frc/robot/subsystems/arm/ArmIO.java

@@ -23,7 +23,7 @@ class ArmIOInputs {
         public double intakeMotorVelocityRotsPerSecond;
         public double intakeMotorVoltage;
 
-        // Encoder inputs
+        // Encoder inputs, should be the same as the arm position.
         public double encoderPositionDegrees;
     }
 

src/main/java/frc/robot/subsystems/arm/ArmIOReal.java

@@ -1,6 +1,6 @@
 package frc.robot.subsystems.arm;
 
-import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.*;
 import static frc.robot.Constants.*;
 import static frc.robot.subsystems.arm.ArmConstants.*;
 
@@ -12,6 +12,7 @@
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Voltage;
+import org.littletonrobotics.junction.Logger;
 
 // TODO: Test this class on LePrawn, ensure the arm won't rotate more than once in any direction.
 public class ArmIOReal implements ArmIO {
@@ -20,12 +21,9 @@ public class ArmIOReal implements ArmIO {
     TalonFX pivotMotor;
     TalonFXConfiguration pivotMotorConfig;
 
-    PositionVoltage pivotPositionControl = new PositionVoltage(0);
-
     CANcoder armEncoder;
     CANcoderConfiguration armEncoderConfig;
 
-
     // Intake/Outtake motors & configurations
     TalonFX intakeMotor;
     TalonFXConfiguration intakeMotorConfig;
@@ -36,7 +34,7 @@ public ArmIOReal() {
 
         armEncoderConfig = new CANcoderConfiguration();
         armEncoderConfig.withMagnetSensor(
-                new MagnetSensorConfigs().withSensorDirection(SensorDirectionValue.CounterClockwise_Positive));
+                new MagnetSensorConfigs().withSensorDirection(SensorDirectionValue.Clockwise_Positive));
 
         armEncoder.getConfigurator().apply(armEncoderConfig);
 
@@ -45,7 +43,13 @@ public ArmIOReal() {
 
         // Set the current position of the arm to what the position of the absolute encoder.
         // Negates the encoder position because the encoder and motor are on different sides of the arm carriage.
-        pivotMotor.setPosition(armEncoder.getAbsolutePosition().getValue().unaryMinus());
+        Angle zeroPos = armEncoder
+                .getAbsolutePosition()
+                .getValue()
+                .plus(Rotations.of(0.408691).plus(Degrees.of(90)));
+        pivotMotor.setPosition(zeroPos.times(MOTOR_TO_ARM_GEARING));
+        Logger.recordOutput("Arm/Encoder/ABS_VAL_Deg", zeroPos.in(Degrees));
+        Logger.recordOutput("Arm/Encoder/ABS_VAL_Rot", zeroPos.in(Rotations));
 
         // Start motor config.
         pivotMotorConfig = new TalonFXConfiguration();
@@ -79,22 +83,24 @@ public ArmIOReal() {
     public void updateState(ArmIOInputs inputs) {
         inputs.armPositionDegrees = pivotMotor.getPosition().getValue().in(Degrees) / MOTOR_TO_ARM_GEARING;
 
-        inputs.pivotMotorRotations = pivotMotor.getPosition().getValueAsDouble();
-        inputs.pivotMotorVelocityRotsPerSecond = pivotMotor.getVelocity().getValueAsDouble();
-        inputs.pivotMotorCurrent = pivotMotor.getStatorCurrent().getValueAsDouble();
-        inputs.pivotMotorVoltage = pivotMotor.getMotorVoltage().getValueAsDouble();
+        inputs.pivotMotorRotations = pivotMotor.getPosition().getValue().in(Rotations);
+        inputs.pivotMotorVelocityRotsPerSecond =
+                pivotMotor.getVelocity().getValue().in(RotationsPerSecond);
+        inputs.pivotMotorCurrent = pivotMotor.getStatorCurrent().getValue().in(Amp);
+        inputs.pivotMotorVoltage = pivotMotor.getMotorVoltage().getValue().in(Volts);
 
-        inputs.intakeMotorVelocityRotsPerSecond = intakeMotor.getVelocity().getValueAsDouble();
-        inputs.intakeMotorVoltage = intakeMotor.getMotorVoltage().getValueAsDouble();
+        inputs.intakeMotorVelocityRotsPerSecond =
+                intakeMotor.getVelocity().getValue().in(RotationsPerSecond);
+        inputs.intakeMotorVoltage = intakeMotor.getMotorVoltage().getValue().in(Volts);
 
         inputs.encoderPositionDegrees = armEncoder.getPosition().getValue().in(Degrees);
     }
 
     @Override
     public void setPivotMotorSetpoint(Angle setpoint) {
         // "0" in sim is facing to the east, while in real robot, it is north. No me gusta, but thus must be done.
-
-        pivotMotor.setControl(pivotPositionControl.withPosition(setpoint));
+        Logger.recordOutput("Arm/PivotMotor/SetpointDeg", setpoint.in(Degrees));
+        pivotMotor.setControl(new PositionVoltage(setpoint));
     }
 
     @Override

src/main/java/frc/robot/subsystems/arm/ArmIOSim.java

@@ -110,7 +110,8 @@ private void updateSim() {
         intakeMotorSimState.setSupplyVoltage(Volts.of(12));
         armEncoderSimState.setSupplyVoltage(Volts.of(12));
 
-        armEncoder.setPosition(Radians.of(armSim.getAngleRads()));
+        // negated because the arm and encoder are on opposite sides of the arm carriage.
+        armEncoderSimState.setRawPosition(Radians.of(armSim.getAngleRads()).unaryMinus());
 
         armSim.setInputVoltage(pivotMotorSimState.getMotorVoltage());
         armSim.update(0.02);