From a4cba19f8ad81281a025b8c3e42de8d0f1e12ac5 Mon Sep 17 00:00:00 2001
From: Nilesh Agarwalla <nilesh@alum.mit.edu>
Date: Sat, 23 Mar 2024 15:47:05 -0400
Subject: [PATCH] Fixed arm conversion factor

---
 src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java
index 09a2eec1..b16f5070 100644
--- a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java
@@ -63,6 +63,12 @@ public ArmIOSparkMax(int id, boolean inverted) {
     // encoder is at 0, set rel to 0.  Everything else is invalid and requires arm to rehome itself.
     //
     // relEncoder.setPosition(0);
+
+    // 60:1 gear box, 72 teeth on the arm cog and 14 teeth on the motor cog
+    double gearRatio = (60 * (72 / 14));
+    double rotationsToDegreesConversionFactor = 360.0 / gearRatio;
+    relEncoder.setPositionConversionFactor(rotationsToDegreesConversionFactor);
+    relEncoder.setVelocityConversionFactor(rotationsToDegreesConversionFactor / 60);
     relEncoder.setPosition(Units.radiansToDegrees(getOffsetCorrectedAbsolutePositionInRadians()));
 
     lkP = RobotConfig.ArmConstants.pidKp;