From bc8e97a7be1dc06a2b3eea06fef5866efc8fb46b Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Tue, 14 Mar 2023 10:58:27 -0600
Subject: [PATCH] finished dynamic limits (NOTE: fix actual soft limits)

---
 src/main/java/frc4388/robot/subsystems/Arm.java | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java
index f6b86a1..b68e5df 100644
--- a/src/main/java/frc4388/robot/subsystems/Arm.java
+++ b/src/main/java/frc4388/robot/subsystems/Arm.java
@@ -120,8 +120,11 @@ public class Arm extends SubsystemBase {
     }
 
     public void runPivotTele(double pivot, double tele) {
-        // TODO: tele has to go through some kind of transformation
-        if (pivot > 0 || tele < 0 || checkLimits(tele, getArmRotation())) {
+        double abs_pivot = Math.toRadians(getArmRotation() - 135);
+        double abs_tele  = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) /
+                           (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
+
+        if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) {
             setRotVel(pivot);
             setTeleVel(tele);
         }