Started IK calc

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

@@ -75,8 +75,12 @@ public final class Constants {
 
   public static final class ClimberConstants {
     /* TODO: Update motor IDS */
-    public static final int MOTOR_1_ID = -1;
-    public static final int MOTOR_2_ID = -1;
+    public static final int SHOULDER_ID = -1;
+    public static final int ELBOW_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;
   }
   /**
    * The OIConstants class contains the ID for the XBox controllers

src/main/java/frc4388/robot/RobotContainer.java

@@ -42,7 +42,7 @@ public class RobotContainer {
 
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
-  private final Climber m_climber = new Climber();
+  private final Climber m_climber = new Climber(m_robotMap.shoulder, m_robotMap.elbow);
   /* Controllers */
   private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);

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

@@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import frc4388.robot.Constants.ClimberConstants;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 
@@ -92,4 +93,7 @@ public class RobotMap {
     //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
   }
 
+  /* Climb Subsystem */
+  public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOLDER_ID); // TODO
+  public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
 }

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

@@ -4,16 +4,36 @@
 
 package frc4388.robot.subsystems;
 
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.ClimberConstants;
 
 public class Climber extends SubsystemBase {
-  /** Creates a new Climber. */
+  WPI_TalonFX m_shoulder;
+  WPI_TalonFX m_elbow;
   
-  public Climber() {
+  public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow) {
+    m_shoulder = shoulder;
+    m_elbow = elbow;
   }
 
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
+  /* getJointAngles
+   * Gets joint angles for climber arm
+   * xTarget and yTarget are set in the xy plane of the climber, not accounting for the
+   * rotation of the robot. Both parameters should be in cm.
+   * returns [shoulderAngle, elbowAngle]
+   * 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) {
+    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;
+
+    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));
+
+    return null;
   }
 }