Merge pull request #97 from DeepBlueRobotics/freshie-orientation

new encoder offset

Author: FriedLongJohns

src/main/java/org/carlmontrobotics/Constants.java

@@ -120,7 +120,8 @@ public static final class Armc {
 		public static final boolean ENCODER_INVERTED = true;
 
 		// difference between CoG and arm is .328 rad
-		public static final double ENCODER_OFFSET_RAD = -0.08 + .328; // - 0.6095;
+		public static final double ENCODER_OFFSET_RAD = 6.132;
+		// 0.08 + .328 - 0.404; // - 0.6095;
 
 		// TODO: finish understand why this is broken public static final Measure<Angle>
 		// INTAKE_ANGLE = Degrees.to(-1);

src/main/java/org/carlmontrobotics/subsystems/Arm.java

@@ -396,12 +396,12 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
     // #region Getters
 
     public double getArmPos() {
+
         return MathUtil.inputModulus(armMasterEncoder.getPosition(),
                 ARM_DISCONT_RAD, ARM_DISCONT_RAD + 2 * Math.PI);// armMasterEncoder.getPosition();//MathUtil.inputModulus(armMasterEncoder.getPosition(),
                                                                 // ARM_DISCONT_RAD,
         // ARM_DISCONT_RAD + 2 * Math.PI);
     }
-
     public double getArmVel() {
         return armMasterEncoder.getVelocity();
     }