From 20f4bdf5badc59184b7a1d5658b6ec9e50e88d5b Mon Sep 17 00:00:00 2001
From: 66945 <66945@psdschools.org>
Date: Sat, 22 Jan 2022 14:14:35 -0700
Subject: [PATCH] Fixed IK math

origin: 0.0, 0.0
extended: 0.7853981633974483, 3.141592653589793
just y: 0.1367898269242458, 0.27357965384849187
just x: 0.0, 0.0
just x: 1.7075861537191424, 0.27357965384849187
---
 .../frc4388/robot/subsystems/Climber.java     | 45 ++++++++++++-------
 1 file changed, 29 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java
index d9d006e..419e8d2 100644
--- a/src/main/java/frc4388/robot/subsystems/Climber.java
+++ b/src/main/java/frc4388/robot/subsystems/Climber.java
@@ -55,54 +55,67 @@ public class Climber extends SubsystemBase {
    * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */
   public static double[] getJointAngles(double xTarget, double yTarget, double tiltAngle) {
     double [] angles = new double[2];
-
+  
     double mag = Math.hypot(xTarget, yTarget);
-    if(mag > ClimberConstants.MAX_ARM_LENGTH) {
+    if(mag >= ClimberConstants.MAX_ARM_LENGTH) {
       xTarget = (xTarget / mag) * ClimberConstants.MAX_ARM_LENGTH;
       yTarget = (yTarget / mag) * ClimberConstants.MAX_ARM_LENGTH;
       mag = ClimberConstants.MAX_ARM_LENGTH;
-    } else if(mag < ClimberConstants.MIN_ARM_LENGTH) {
+    } else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) {
       xTarget = (xTarget / mag) * ClimberConstants.MIN_ARM_LENGTH;
       yTarget = (yTarget / mag) * ClimberConstants.MIN_ARM_LENGTH;
       mag = ClimberConstants.MIN_ARM_LENGTH;
+    } else if(mag < ClimberConstants.MIN_ARM_LENGTH) {
+      xTarget = ClimberConstants.MIN_ARM_LENGTH;
+      yTarget = 0.d;
+      mag = ClimberConstants.MIN_ARM_LENGTH;
     }
-
+  
     // The angle to the target point
-    double theta = Math.atan(yTarget / xTarget) + tiltAngle; // TODO rename variable
+    double theta;
+    if(xTarget == 0.d) {
+      theta = Math.PI/2.d; // TODO rename variable
+    } else {
+      theta = Math.atan(yTarget / xTarget);
+    }
+    theta += tiltAngle;
+    
     if(xTarget < 0.d)
-      theta += Math.PI;
+        theta += Math.PI;
+    
     
     // Correct target position for tilt
     xTarget = Math.cos(theta) * mag;
     yTarget = Math.sin(theta) * mag;
     
     // Law and Order: Cosines edition
-    double shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) /
+    double shoulderAngle;
+    if(mag == 0) {
+      shoulderAngle = 0;
+    } else {
+      shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) /
       (2.d * ClimberConstants.LOWER_ARM_LENGTH * mag));
-      //shoulderAngle = acos(LowerArmLength^2 + mag^2)
+    }
     shoulderAngle = theta - shoulderAngle;
     
     double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) /
       (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH));
-    elbowAngle = Math.PI - elbowAngle;
-
+    //elbowAngle = Math.PI - elbowAngle;
+  
     // If the climber is resting on the robot base, rotate the upper arm in the direction of the target
     if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) {
       shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE;
       double xDiff = xTarget - ClimberConstants.LOWER_ARM_LENGTH;
-
+  
       elbowAngle = Math.atan(-yTarget / xDiff);
       if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE)
         elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE;
-
-
-      elbowAngle = Math.PI - elbowAngle;
-
+  
       // Deal with negative wraparound
       if(xDiff >= 0.d)
         elbowAngle += Math.PI;
     }
-
+  
     angles[0] = shoulderAngle;
     angles[1] = elbowAngle;
     return angles;