Finish IK, accounting for robot tilt

src/main/java/frc4388/robot/Constants.java

@@ -77,10 +77,14 @@ public final class Constants {
     /* TODO: Update motor IDS */
     public static final int SHOULDER_ID = -1;
     public static final int ELBOW_ID = -1;
+    public static final int GYRO_ID = -1;
 
     // TODO Update this stuff too
     public static final double UPPER_ARM_LENGTH = 50; // Units should be in cm
     public static final double LOWER_ARM_LENGTH = 50;
+
+    public static final double MAX_ARM_LENGTH = LOWER_ARM_LENGTH + UPPER_ARM_LENGTH;
+    public static final double MIN_ARM_LENGTH = Math.abs(LOWER_ARM_LENGTH + UPPER_ARM_LENGTH);
   }
   /**
    * The OIConstants class contains the ID for the XBox controllers

src/main/java/frc4388/robot/RobotMap.java

@@ -6,6 +6,7 @@ package frc4388.robot;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import frc4388.robot.Constants.ClimberConstants;
@@ -94,6 +95,7 @@ public class RobotMap {
   }
 
   /* Climb Subsystem */
-  public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOLDER_ID); // TODO
+  public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO
   public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
+  public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO
 }

src/main/java/frc4388/robot/subsystems/Climber.java

@@ -5,35 +5,79 @@
 package frc4388.robot.subsystems;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ClimberConstants;
 
 public class Climber extends SubsystemBase {
-  WPI_TalonFX m_shoulder;
-  WPI_TalonFX m_elbow;
+  private WPI_TalonFX m_shoulder;
+  private WPI_TalonFX m_elbow;
+
+  private WPI_PigeonIMU m_gyro;
   
-  public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow) {
+  public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) {
     m_shoulder = shoulder;
     m_elbow = elbow;
+    m_gyro = gyro;
   }
 
   /* getJointAngles
    * Gets joint angles for climber arm
-   * xTarget and yTarget are set in the xy plane of the climber, not accounting for the
+   * xTarget and yTarget are set in the xy plane of the climber, accounting for the
    * rotation of the robot. Both parameters should be in cm.
-   * returns [shoulderAngle, elbowAngle]
+   * returns [shoulderAngle, elbowAngle] in radians
    * Does not set the motors automatically
    * 
    * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */
-  public double[] getJointAngles(double xTarget, double yTarget) {
+  public static double[] getJointAngles(double xTarget, double yTarget, double tiltAngle) {
+    double [] angles = new double[2];
+
     double mag = Math.hypot(xTarget, yTarget);
-    double upperArm_2 = ClimberConstants.UPPER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH;
-    double lowerArm_2 = ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.LOWER_ARM_LENGTH;
+    if(mag > ClimberConstants.MAX_ARM_LENGTH) {
+      xTarget = (xTarget / mag) * ClimberConstants.MAX_ARM_LENGTH;
+      yTarget = (yTarget / mag) * ClimberConstants.MAX_ARM_LENGTH;
+    } else if(mag < ClimberConstants.MIN_ARM_LENGTH) {
+      xTarget = (xTarget / mag) * ClimberConstants.MIN_ARM_LENGTH;
+      yTarget = (yTarget / mag) * ClimberConstants.MIN_ARM_LENGTH;
+    }
 
-    double shoulderAngle = Math.acos((-lowerArm_2 + upperArm_2 - mag) / (2.d * ClimberConstants.UPPER_ARM_LENGTH * mag));
-    double elbowAngle = Math.acos((lowerArm_2 + upperArm_2 - mag) / (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH));
+    // The angle to the target point
+    double theta = Math.atan(yTarget / xTarget) + tiltAngle; // TODO rename variable
+    // 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)) /
+      (2.d * ClimberConstants.LOWER_ARM_LENGTH * mag));
+    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;
 
-    return null;
+    // If the climber is resting on the robot base, rotate the upper arm in the direction of the target
+    if(shoulderAngle <= 0.d) {
+      shoulderAngle = 0.d;
+      double xDiff = xTarget - ClimberConstants.LOWER_ARM_LENGTH;
+
+      elbowAngle = Math.atan(-yTarget / xDiff);
+      elbowAngle = Math.PI - elbowAngle;
+
+      if(xDiff >= 0.d) {
+        elbowAngle += Math.PI;
+      }
+    }
+
+    return angles;
+  }
+
+  public void setJointAngles(double[] angles) {
+    setJointAngles(angles[0], angles[1]);
+  }
+
+  public void setJointAngles(double shoulderAngle, double elbowAngle) {
+    // Set PIDs
   }
 }