From 8f4f17657a6270da9a2ca2c29955b08fc22bb199 Mon Sep 17 00:00:00 2001
From: Jack Nelson <jack@jacknelson.xyz>
Date: Wed, 28 Feb 2024 00:22:54 -0500
Subject: [PATCH] add goal update criteria

---
 src/main/java/frc/robot/ShooterState.java     |  6 +++
 .../frc/robot/subsystems/ArmSubsystem.java    | 38 +++++++++++++++----
 2 files changed, 36 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/ShooterState.java b/src/main/java/frc/robot/ShooterState.java
index f7714ff..7fe9738 100644
--- a/src/main/java/frc/robot/ShooterState.java
+++ b/src/main/java/frc/robot/ShooterState.java
@@ -62,6 +62,12 @@ public double getShooterSpeed() {
     }
   }
 
+  /**
+   * Updates the values on the SmartDashboard related to the shooter state. This method puts the
+   * values of various shooter state variables onto the SmartDashboard. The variables include
+   * whether the manual arm mode is enabled, the current arm mode, whether the shooter is loaded,
+   * whether the arm is lowered, and whether the arm is shooting.
+   */
   public void updateDash() {
     SmartDashboard.putBoolean("Manual Arm Mode Enabled", axisEnabled);
     SmartDashboard.putString("Arm Mode", mode.name());
diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
index b7eecf1..247734e 100644
--- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -68,7 +68,7 @@ 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();
-  private boolean m_stopped = true;
+  private boolean m_newGoal = true;
 
   // setup SysID for auto profiling
   private final SysIdRoutine m_sysIdRoutine;
@@ -186,11 +186,13 @@ public void moveArmToSpeaker() {
    * Move arm to global position (by updating goal)
    */
   public void MoveArmToPosition(double radians) {
-    m_stopped = false;
     // update the motion profile with new goal
     // 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_newGoal = true;
+    }
     m_goal = new TrapezoidProfile.State(radians, 0); // set the goal
   }
 
@@ -198,10 +200,9 @@ public void MoveArmToPosition(double radians) {
    * attempt to hold arm at current location
    */
   public void stop() {
-    if (!m_stopped) {
+    if (!atGoal()) {
       // update the PID controller with current encoder position
       MoveArmToPosition(getAverageEncoderPosition());
-      m_stopped = true;
     }
   }
 
@@ -209,23 +210,44 @@ public void disablePID() {
     kPIDEnabled = false;
   }
 
+  /** Matches the position of the main encoder with the absolute encoder. */
   public void matchEncoders() {
     m_MainEncoder.setPosition(m_AbsoluteEncoder.getPosition());
   }
 
+  /**
+   * Calculates the error between the position reported by the absolute encoder and the main
+   * encoder.
+   *
+   * @return The error between the two encoder positions.
+   */
   public double getError() {
     return m_AbsoluteEncoder.getPosition() - m_MainEncoder.getPosition();
   }
 
+  /**
+   * Checks if the arm is at the goal position.
+   *
+   * @return true if the arm is at the goal position, false otherwise.
+   */
   private boolean atGoal() {
     return HelperFunctions.inDeadzone(
         m_goal.position - m_MainEncoder.getPosition(), Units.degreesToRadians(3));
   }
 
+  /**
+   * Checks if the arm is stopped.
+   *
+   * @return true if the arm is stopped, false otherwise.
+   */
   private boolean ArmStopped() {
     return HelperFunctions.inDeadzone(m_goal.velocity, 0.01);
   }
 
+  private boolean getFrontLimit() {
+    return !m_frontLimitDebouncer.calculate(m_frontLimit.get());
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
@@ -238,16 +260,16 @@ public void periodic() {
           CANSparkBase.ControlType.kPosition,
           0,
           m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity));
-      if (atGoal() && ArmStopped() && !m_stopped) {
-        m_stopped = true;
+      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))) {
           matchEncoders();
         }
       }
     }
-    if (m_frontLimitState != !m_frontLimitDebouncer.calculate(m_frontLimit.get())) {
-      m_frontLimitState = !m_frontLimitDebouncer.calculate(m_frontLimit.get());
+    if (m_frontLimitState != getFrontLimit()) {
+      m_frontLimitState = getFrontLimit();
       if (m_frontLimitState) {
         matchEncoders();
       }