fix ratios

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

@@ -6,13 +6,15 @@ public interface ArmIO {
 
   @AutoLog
   public static class ArmIOInputs {
+    public double elbowAbsolutePositionRaw = 0.0;
     public double elbowAbsolutePositionRad = 0.0;
     public boolean elbowAbsoluteEncoderConnected = false;
     public double elbowRelativePositionRad = 0.0;
     public double elbowVelocityRadPerSec = 0.0;
     public double elbowAppliedVolts = 0.0;
     public double[] elbowCurrentAmps = new double[] {};
 
+    public double wristAbsolutePositionRaw = 0.0;
     public double wristAbsolutePositionRad = 0.0;
     public boolean wristAbsoluteEncoderConnected = false;
     public double wristRelativePositionRad = 0.0;

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

@@ -18,6 +18,7 @@ public void updateInputs(ArmIOInputs inputs) {
     m_elbowWristSimStates =
         m_model.simulate(
             m_elbowWristSimStates, VecBuilder.fill(m_elbowAppliedVolts, m_wristAppliedVolts), 0.02);
+    inputs.elbowAbsolutePositionRaw = 0.0;
     inputs.elbowAbsolutePositionRad = m_elbowWristSimStates.get(0, 0);
     inputs.elbowAbsoluteEncoderConnected = true;
     inputs.elbowRelativePositionRad = m_elbowWristSimStates.get(0, 0);
@@ -28,6 +29,7 @@ public void updateInputs(ArmIOInputs inputs) {
           ArmModel.kElbowGearbox.getCurrent(m_elbowWristSimStates.get(2, 0), m_elbowAppliedVolts)
         };
 
+    inputs.wristAbsolutePositionRaw = 0.0;
     inputs.wristAbsolutePositionRad = m_elbowWristSimStates.get(1, 0);
     inputs.wristAbsoluteEncoderConnected = true;
     inputs.wristRelativePositionRad = m_elbowWristSimStates.get(1, 0);

src/main/java/frc/robot/arm/ArmIOTalonFX.java

@@ -12,13 +12,12 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import frc.robot.Constants;
-import org.littletonrobotics.junction.Logger;
 
 public class ArmIOTalonFX implements ArmIO {
 
   // Offsets to the horizontal
-  private final double kElbowAbsoluteEncoderOffsetRad = 1.439;
-  private final double kWristAbsoluteEncoderOffsetRad = 0.681;
+  private final double kElbowAbsoluteEncoderOffsetRaw = 0.645;
+  private final double kWristAbsoluteEncoderOffsetRaw = 0.227;
   private final double kElbowRelativeEncoderOffsetRad = 0.0;
   private final double kWristRelativeEncoderOffsetRad = 0.0;
   private final boolean kUseAbsoluteEncoders = true;
@@ -49,8 +48,6 @@ public ArmIOTalonFX() {
     m_elbowAbsoluteEncoder = new DutyCycleEncoder(8);
     m_wristAbsoluteEncoder = new DutyCycleEncoder(9);
 
-    // m_elbowAbsoluteEncoder.setDistancePerRotation(kCurrentLimitAmps);
-
     final CurrentLimitsConfigs currentLimitsConfig = new CurrentLimitsConfigs();
     currentLimitsConfig.StatorCurrentLimit = kCurrentLimitAmps;
     currentLimitsConfig.StatorCurrentLimitEnable = true;
@@ -116,18 +113,14 @@ public ArmIOTalonFX() {
     m_wristRightFollowerMotor.optimizeBusUtilization();
 
     if (kUseAbsoluteEncoders) {
-      m_elbowAbsoluteEncoder.setPositionOffset(
-          Units.radiansToRotations(MathUtil.angleModulus(kElbowAbsoluteEncoderOffsetRad))
-              * ArmModel.kElbowChainReduction);
+      m_elbowAbsoluteEncoder.setPositionOffset(kElbowAbsoluteEncoderOffsetRaw);
       m_elbowLeftMotor.setPosition(m_elbowAbsoluteEncoder.get() * ArmModel.kElbowGearboxReduction);
     } else
       m_elbowLeftMotor.setPosition(
           Units.radiansToRotations(kElbowRelativeEncoderOffsetRad) * ArmModel.kElbowFinalReduction);
 
     if (kUseAbsoluteEncoders) {
-      m_wristAbsoluteEncoder.setPositionOffset(
-          Units.radiansToRotations(MathUtil.angleModulus(kWristAbsoluteEncoderOffsetRad))
-              * ArmModel.kWristChainReduction);
+      m_wristAbsoluteEncoder.setPositionOffset(kWristAbsoluteEncoderOffsetRaw);
       m_wristLeftMotor.setPosition(m_wristAbsoluteEncoder.get() * ArmModel.kWristGearboxReduction);
     } else
       m_wristLeftMotor.setPosition(
@@ -139,11 +132,6 @@ public ArmIOTalonFX() {
 
   public void updateInputs(ArmIOInputs inputs) {
 
-    Logger.recordOutput(
-        "Arm/Elbow Absolute Native Position", m_elbowAbsoluteEncoder.getAbsolutePosition());
-    Logger.recordOutput(
-        "Arm/Wrist Absolute Native Position", m_wristAbsoluteEncoder.getAbsolutePosition());
-
     BaseStatusSignal.refreshAll(
         m_elbowPositionSignal,
         m_elbowVelocitySignal,
@@ -156,28 +144,26 @@ public void updateInputs(ArmIOInputs inputs) {
         m_wristCurrentSignal,
         m_wristFollowerCurrentSignal);
 
+    inputs.elbowAbsolutePositionRaw = m_elbowAbsoluteEncoder.getAbsolutePosition();
     inputs.elbowAbsolutePositionRad =
-        Units.rotationsToRadians(m_elbowAbsoluteEncoder.get() / ArmModel.kElbowChainReduction);
+        MathUtil.angleModulus(
+            Units.rotationsToRadians(m_elbowAbsoluteEncoder.get() / ArmModel.kElbowChainReduction));
     inputs.elbowAbsoluteEncoderConnected = m_elbowAbsoluteEncoder.isConnected();
     inputs.elbowRelativePositionRad =
-        Units.rotationsToRadians(
-            BaseStatusSignal.getLatencyCompensatedValue(
-                    m_elbowPositionSignal, m_elbowVelocitySignal)
-                / ArmModel.kElbowFinalReduction);
+        Units.rotationsToRadians(m_elbowPositionSignal.getValue() / ArmModel.kElbowFinalReduction);
     inputs.elbowVelocityRadPerSec =
         Units.rotationsToRadians(m_elbowVelocitySignal.getValue() / ArmModel.kElbowFinalReduction);
     inputs.elbowAppliedVolts = m_elbowAppliedVoltsSignal.getValue();
     inputs.elbowCurrentAmps =
         new double[] {m_elbowCurrentSignal.getValue(), m_elbowFollowerCurrentSignal.getValue()};
 
+    inputs.wristAbsolutePositionRaw = m_wristAbsoluteEncoder.getAbsolutePosition();
     inputs.wristAbsolutePositionRad =
-        Units.rotationsToRadians(m_wristAbsoluteEncoder.get() / ArmModel.kWristChainReduction);
+        MathUtil.angleModulus(
+            Units.rotationsToRadians(m_wristAbsoluteEncoder.get() / ArmModel.kWristChainReduction));
     inputs.wristAbsoluteEncoderConnected = m_wristAbsoluteEncoder.isConnected();
     inputs.wristRelativePositionRad =
-        Units.rotationsToRadians(
-            BaseStatusSignal.getLatencyCompensatedValue(
-                    m_wristPositionSignal, m_wristVelocitySignal)
-                / ArmModel.kWristFinalReduction);
+        Units.rotationsToRadians(m_wristPositionSignal.getValue() / ArmModel.kWristFinalReduction);
     inputs.wristVelocityRadPerSec =
         Units.rotationsToRadians(m_wristVelocitySignal.getValue() / ArmModel.kWristFinalReduction);
     inputs.wristAppliedVolts = m_wristAppliedVoltsSignal.getValue();

src/main/java/frc/robot/arm/ArmModel.java

@@ -15,8 +15,8 @@ public class ArmModel {
   public static final Translation2d kOrigin =
       new Translation2d(Units.inchesToMeters(-9.2), Units.inchesToMeters(20));
 
-  public static final double kElbowGearboxReduction = 5.0;
-  public static final double kElbowChainReduction = 48.0 / 17.0;
+  public static final double kElbowGearboxReduction = 15.0;
+  public static final double kElbowChainReduction = 36.0 / 22.0;
   public static final double kElbowFinalReduction = kElbowGearboxReduction * kElbowChainReduction;
   public static final double kElbowLengthMeters = Units.inchesToMeters(19.0);
   public static final double kElbowMinAngleRad = -Math.PI;
@@ -28,8 +28,8 @@ public class ArmModel {
   private final double elbowkG = 0.53;
   private final double elbowkV = 0.27;
 
-  public static final double kWristGearboxReduction = 5.0;
-  public static final double kWristChainReduction = 36.0 / 17.0;
+  public static final double kWristGearboxReduction = 15.0;
+  public static final double kWristChainReduction = 36.0 / 22.0;
   public static final double kWristFinalReduction = kWristGearboxReduction * kWristChainReduction;
   public static final double kWristLengthMeters = Units.inchesToMeters(15.0);
   public static final double kWristMinAngleRad = -Math.PI;

src/main/java/frc/robot/arm/ArmVisualizer.java

@@ -47,7 +47,7 @@ public ArmVisualizer(String logKey) {
 
   public void update(double elbowAngle, double wristAngle) {
     m_elbowLigament.setAngle(Units.radiansToDegrees(elbowAngle) - kShoulderAngleDegrees);
-    m_wristLigament.setAngle(Units.radiansToDegrees(wristAngle));
+    m_wristLigament.setAngle(Units.radiansToDegrees(wristAngle - elbowAngle));
     Logger.recordOutput("Mechanism2d/" + m_logKey, m_mechanism);
 
     Pose3d elbowPose =