From 86e5b6ad536f70b541fba1b40ed60b58130e3069 Mon Sep 17 00:00:00 2001
From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com>
Date: Mon, 2 Sep 2024 16:15:25 -0700
Subject: [PATCH] Arm subystem fixes (#25)

* Arm subystem fixes

* arm homing fix

* doglog name change
---
 src/main/java/frc/robot/arm/ArmSubsystem.java | 42 ++++++-------------
 1 file changed, 13 insertions(+), 29 deletions(-)

diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java
index 8aeecbf..055fabe 100644
--- a/src/main/java/frc/robot/arm/ArmSubsystem.java
+++ b/src/main/java/frc/robot/arm/ArmSubsystem.java
@@ -19,7 +19,7 @@ public class ArmSubsystem extends StateMachine<ArmState> {
   private double distanceToSpeaker;
   private double distanceToFeedSpot;
   private double lowestSeenAngleLeft = Double.MAX_VALUE;
-  private double lowestSeenAngleRight = Double.MIN_VALUE;
+  private double lowestSeenAngleRight = Double.MAX_VALUE;
   private InterpolatingDoubleTreeMap speakerDistanceToAngle = new InterpolatingDoubleTreeMap();
   private InterpolatingDoubleTreeMap feedSpotDistanceToAngle = new InterpolatingDoubleTreeMap();
   private final PositionVoltage positionRequest =
@@ -60,7 +60,7 @@ public void setState(ArmState newState) {
   // public void disabledPeriodic()
   // {
   //   rightMotor.setControl(staticBrake);
-  //   rightMotor.setControl(staticBrake);
+  //   leftMotor.setControl(staticBrake);
   // }
   // IF withOverrideBrakeDurNeutral DOESNT WORK
   public boolean atGoal() {
@@ -166,40 +166,24 @@ public void robotPeriodic() {
     DogLog.log("Arm/Left/SupplyCurrent", leftMotor.getSupplyCurrent().getValueAsDouble());
     DogLog.log("Arm/Left/ArmAngle", leftMotor.getPosition().getValueAsDouble());
     DogLog.log("Arm/Left/AppliedVoltage", leftMotor.getMotorVoltage().getValueAsDouble());
+    DogLog.log("Arm/Left/LowestSeenAngle", lowestSeenAngleLeft);
 
     DogLog.log("Arm/Right/StatorCurrent", rightMotor.getStatorCurrent().getValueAsDouble());
     DogLog.log("Arm/Right/SupplyCurrent", rightMotor.getSupplyCurrent().getValueAsDouble());
     DogLog.log("Arm/Right/ArmAngle", rightMotor.getPosition().getValueAsDouble());
     DogLog.log("Arm/Right/AppliedVoltage", rightMotor.getMotorVoltage().getValueAsDouble());
+    DogLog.log("Arm/Right/LowestSeenAngle", lowestSeenAngleRight);
 
     if (DriverStation.isEnabled() && getState() == ArmState.PRE_MATCH_HOMING) {
-      Double homedAngle = getHomeAngleFromLowestSeen();
-
-      leftMotor.setPosition(Units.degreesToRotations(homedAngle));
-      rightMotor.setPosition(Units.degreesToRotations(homedAngle));
-      setState(ArmState.IDLE);
-    }
-
-    // We are enabled and still in pre match homing
-    // Reset the motor positions, and then transition to idle state
-  }
-
-  private double getHomeAngleFromLowestSeen() {
-    return RobotConfig.get().arm().minAngle() + (getAngle() - lowestSeenAngleLeft);
-  }
-
-  public double getAngle() {
-
-    return Units.rotationsToDegrees(leftMotor.getPosition().getValueAsDouble());
-  }
-
-  private static double clampAngle(double angle) {
-    if (angle < RobotConfig.get().arm().minAngle()) {
-      angle = RobotConfig.get().arm().minAngle();
-
-    } else if (angle > RobotConfig.get().arm().maxAngle()) {
-      angle = RobotConfig.get().arm().maxAngle();
+      // We are enabled and still in pre match homing
+      // Reset the motor positions, and then transition to idle state
+      leftMotor.setPosition(
+          Units.degreesToRotations(
+              RobotConfig.get().arm().minAngle() + (leftMotorAngle - lowestSeenAngleLeft)));
+      rightMotor.setPosition(
+          Units.degreesToRotations(
+              RobotConfig.get().arm().minAngle() + (rightMotorAngle - lowestSeenAngleRight)));
+      setStateFromRequest(ArmState.IDLE);
     }
-    return angle;
   }
 }