working arm error correction

src/main/java/frc/robot/Constants.java

@@ -63,8 +63,8 @@ public static final class CANConstants {
   public static final double ARMSTARTINGANGLE = 22.5 + ARMENCODEROFFSET; // WHY MaTH HURT
   public static final double ARMMINRELATVESTART = 0.0;
   public static final double ARMLOADANGLE = 45 - ARMSTARTINGANGLE;
-  public static final double ARMSPEAKERANGLE = 65 - ARMSTARTINGANGLE; // to go to 75 you just put 75
-  public static final double ARMAMPANGLE = 100 - ARMSTARTINGANGLE;
+  public static final double ARMSPEAKERANGLE = 75 - ARMSTARTINGANGLE; // to go to 75 you just put 75
+  public static final double ARMAMPANGLE = 105 - ARMSTARTINGANGLE;
   public static final double ARMMAXRELATIVE = 100 - ARMSTARTINGANGLE;
   // Shooter Speeds (M/s)
   public static final double SHOOTERSOURCE = -6.0;

src/main/java/frc/robot/subsystems/ArmSubsystem.java

@@ -68,18 +68,26 @@ public class ArmSubsystem extends SubsystemBase {
       new TrapezoidProfile(new TrapezoidProfile.Constraints(karmMaxVelocity, karmMaxAcceleration));
   private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
   private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
+
+  // track if goal was changed
   private boolean m_newGoal = true;
 
+  // last goal pre offset
+  private double m_lastGoal = 0.0;
+
   // setup SysID for auto profiling
   private final SysIdRoutine m_sysIdRoutine;
 
   // setup front limit switch for rest
   private final DigitalInput m_frontLimit;
 
+  // last measured state of limit switch
   private boolean m_frontLimitState = false;
+
+  // makes limit switch input less finnicky
   private final Debouncer m_frontLimitDebouncer = new Debouncer(0.2, Debouncer.DebounceType.kBoth);
 
-  // offset to match the absolute encoder with the main encoder
+  // offset to match the absolute encoder with the main encoder by adding offset to goal
   private double m_curOffset = 0.0;
 
   /** Creates a new ArmSubsystem. */
@@ -193,11 +201,12 @@ public void MoveArmToPosition(double radians) {
     // add minimum starting angle to the target angle to get the real angle
     double final_radians = Math.max(radians, kMinArmAngleRadians);
     final_radians = Math.min(final_radians, kMaxArmAngleRadians);
-    if (final_radians != (m_goal.position - m_curOffset)) {
+    if (final_radians != m_lastGoal) {
       m_newGoal = true;
+      m_lastGoal = final_radians;
       resetOffset();
     }
-    m_goal = new TrapezoidProfile.State(radians + m_curOffset, 0); // set the goal
+    m_goal = new TrapezoidProfile.State(final_radians + m_curOffset, 0); // set the goal
   }
 
   /*
@@ -234,7 +243,7 @@ private void resetOffset() {
    * @return The error between the two encoder positions.
    */
   public double getError() {
-    return m_AbsoluteEncoder.getPosition() - m_MainEncoder.getPosition();
+    return m_MainEncoder.getPosition() - m_AbsoluteEncoder.getPosition();
   }
 
   /**
@@ -244,7 +253,7 @@ public double getError() {
    */
   private boolean atGoal() {
     return HelperFunctions.inDeadzone(
-        m_goal.position - m_MainEncoder.getPosition(), Units.degreesToRadians(3));
+        m_goal.position - m_MainEncoder.getPosition(), Units.degreesToRadians(1));
   }
 
   /**
@@ -253,7 +262,7 @@ private boolean atGoal() {
    * @return true if the arm is stopped, false otherwise.
    */
   private boolean ArmStopped() {
-    return HelperFunctions.inDeadzone(m_goal.velocity, 0.01);
+    return HelperFunctions.inDeadzone(m_goal.velocity, 0.0001);
   }
 
   private boolean getFrontLimit() {
@@ -273,10 +282,11 @@ public void periodic() {
           0,
           m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity));
       if (atGoal() && ArmStopped() && m_newGoal) {
-        m_newGoal = false; // reset the new goal flag, so that we dont try resyncing encoders again
         double cur_error = getError();
-        if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(3))) {
+        if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(5))) {
           SetOffsetWithEncoder();
+          m_newGoal = false; // reset the new goal flag, so that we dont try resyncing encoders again
+          m_goal = new TrapezoidProfile.State(m_lastGoal + m_curOffset, 0); // set the goal
         }
       }
     }
@@ -294,6 +304,11 @@ public void periodic() {
         "Current Arm Angle (Degrees) (Absolute)",
         Units.radiansToDegrees(m_AbsoluteEncoder.getPosition()) + Constants.ARMSTARTINGANGLE);
     SmartDashboard.putBoolean("Front Limit Switch Pressed", !m_frontLimit.get());
+    SmartDashboard.putNumber("Current angle Offset", Units.radiansToDegrees(m_curOffset));
+    SmartDashboard.putNumber("Requested Angle", Units.radiansToDegrees(m_lastGoal) + Constants.ARMSTARTINGANGLE);
+    SmartDashboard.putNumber("Current Goal Angle", Units.radiansToDegrees(m_goal.position) + Constants.ARMSTARTINGANGLE);
+    SmartDashboard.putBoolean("At Goal", atGoal());
+
   }
 
   @Override

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -78,8 +78,8 @@ public class DriveSubsystem extends SubsystemBase {
   static final double turn_I = 0.00;
   static final double turn_D = 0.00;
   static final double MaxTurnRateDegPerS = 5;
-  static final double MaxTurnAccelerationDegPerSSquared = 10;
-  static final double TurnToleranceDeg = 3; // max diff in degrees
+  static final double MaxTurnAccelerationDegPerSSquared = 5;
+  static final double TurnToleranceDeg = 10; // max diff in degrees
   static final double TurnRateToleranceDegPerS = 10; // degrees per second
   // false when inactive, true when active / a target is set.
   private boolean turnControllerEnabled = false;

src/main/java/frc/robot/utils/HelperFunctions.java

@@ -5,4 +5,4 @@ public final class HelperFunctions {
   public static boolean inDeadzone(double value, double deadzone) {
     return Math.abs(value) < deadzone;
   }
-}
+}