diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index bef637c..36fafd3 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -167,7 +167,8 @@ public final class Constants {
     public static final double SHOULDER_MAX_ANGLE = 135;
     public static final double ELBOW_MAX_ANGLE = 180;
 
-    public static final double GEAR_BOX_RATIO = 144.d * 60.d / 24.d;
+    public static final double ELBOW_GB_RATIO = 1.d;
+    public static final double SHOULDER_GB_RATIO = 1.d;
 
     public static final double SHOULDER_SOFT_LIMIT_FORWARD = 0;
     public static final double SHOULDER_SOFT_LIMIT_REVERSE = 0;
@@ -199,6 +200,8 @@ public final class Constants {
     public static final double OPEN_POSITION = 0;
     public static final double CLOSE_POSITION = 0;
 
+    public static final double THRESHOLD = 0;
+
     public static final double CALIBRATION_SPEED = 0;
 
     public static final double CURRENT_LIMIT = 0.0; // TODO: set actual current limit;
diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java
new file mode 100644
index 0000000..eeb3f46
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java
@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.climber;
+
+import org.opencv.core.Point;
+
+import edu.wpi.first.wpilibj.drive.Vector2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Claws;
+import frc4388.robot.subsystems.Climber;
+
+public class RunClimberPath extends CommandBase {
+  Climber climber;
+  Claws claws;
+
+  Point[] path;
+  int nextIndex;
+
+  /** Creates a new RunClimberPath. */
+  public RunClimberPath(Point[] _path, Climber _climber, Claws _claws) {
+    path = _path;
+
+    climber = _climber;
+    claws = _claws;
+    addRequirements(climber, claws);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    claws.setOpen(true);
+    nextIndex = 0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if(!claws.fullyOpen())
+      return;
+    
+    Vector2d vec = new Vector2d(1,1);
+    double mag = vec.magnitude();
+    vec.x /= mag;
+    vec.y /= mag;
+
+    climber.controlWithInput(vec.x * .02, vec.y * .02);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java
index 2382dbc..f4ef26f 100644
--- a/src/main/java/frc4388/robot/subsystems/Claws.java
+++ b/src/main/java/frc4388/robot/subsystems/Claws.java
@@ -1,5 +1,7 @@
 package frc4388.robot.subsystems;
 
+import java.nio.file.ClosedWatchServiceException;
+
 import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.SparkMaxLimitSwitch;
@@ -7,6 +9,7 @@ import com.revrobotics.SparkMaxLimitSwitch;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ClawConstants;
+import frc4388.robot.Constants.ClimberConstants;
 
 public class Claws extends SubsystemBase {
   public CANSparkMax m_leftClaw;
@@ -112,15 +115,20 @@ public class Claws extends SubsystemBase {
       m_rightClaw.set(-0.1);
     }
 
-    m_open = open;
+    // m_open = open;
   }
 
   public double[] getOffsets() {
     return new double[] {m_leftOffset, m_rightOffset};
   }
 
-  public boolean getOpen() {
-    return m_open;
+  public boolean fullyOpen() {
+    return Math.abs(m_leftClaw.getEncoder().getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD &&
+        Math.abs(m_rightClaw.getEncoder().getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD;  }
+
+  public boolean fullyClosed() {
+    return Math.abs(m_leftClaw.getEncoder().getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD &&
+        Math.abs(m_rightClaw.getEncoder().getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD;
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java
index be1e764..b97a1b0 100644
--- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java
+++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java
@@ -7,17 +7,21 @@ package frc4388.robot.subsystems;
 import java.util.HashMap;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.TalonFX;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
+import org.opencv.core.Point;
+
 import edu.wpi.first.wpilibj.motorcontrol.Talon;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants;
 import frc4388.robot.Constants.ClimberConstants;
 
 public class ClimberRewrite extends SubsystemBase {
-  private static HashMap<Double, Double> shoulderFeedMap;
-  private static HashMap<Double, Double> elbowFeedMap;
+  private static double[][] shoulderFeedMap;
+  private static double[][] elbowFeedMap;
 
   public static void configureFeedMaps() {
     
@@ -27,13 +31,12 @@ public class ClimberRewrite extends SubsystemBase {
   private WPI_TalonFX m_elbow;
   private WPI_Pigeon2 m_gyro;
 
-  private double shoulderStartPosition;
-  private double elbowStartPosition;
+  private Point tPoint;
 
   private boolean groundRelative;
 
   /** Creates a new ClimberRewrite. */
-  public ClimberRewrite(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean groundRelative_) {
+  public ClimberRewrite(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean _groundRelative) {
     m_shoulder = shoulder;
     m_shoulder.configFactoryDefault();
     m_shoulder.setNeutralMode(NeutralMode.Brake);
@@ -44,8 +47,10 @@ public class ClimberRewrite extends SubsystemBase {
 
     setClimberGains();
 
-    shoulderStartPosition = m_shoulder.getSelectedSensorPosition();
-    elbowStartPosition = m_elbow.getSelectedSensorPosition();
+    // shoulderStartPosition = m_shoulder.getSelectedSensorPosition();
+    // elbowStartPosition = m_elbow.getSelectedSensorPosition();
+    m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
+    m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
 
     m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD);
     m_elbow.configForwardSoftLimitEnable(false);
@@ -57,10 +62,12 @@ public class ClimberRewrite extends SubsystemBase {
     m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE);
     m_shoulder.configReverseSoftLimitEnable(false);
 
+    tPoint = new Point();
+
     if(groundRelative)
       m_gyro = gyro;
     
-    groundRelative = groundRelative_;
+    groundRelative = _groundRelative;
   }
 
   public void setClimberGains() {
@@ -86,11 +93,161 @@ public class ClimberRewrite extends SubsystemBase {
   }
 
   public void compensateFeedForArmAngles() {
+    double shoulderPosition = m_shoulder.getSelectedSensorPosition();
+    double elbowPosition = m_elbow.getSelectedSensorPosition();
 
+    double shoulderFeed = 0;
+    double elbowFeed = 0;
+
+    for(int i = 1; i < shoulderFeedMap.length; i++) {
+      if(shoulderFeedMap[i][0] > shoulderPosition) {
+        double percentDifference = (shoulderPosition - shoulderFeedMap[i-1][0]) / (shoulderFeedMap[i][0] - shoulderFeedMap[i-1][0]);
+        double feedDifference = shoulderFeedMap[i][1] - shoulderFeedMap[i-1][1];
+        shoulderFeed = (percentDifference * feedDifference) + shoulderFeedMap[i-1][1];
+      }
+    }
+
+    for(int i = 1; i < elbowFeedMap.length; i++) {
+      if(elbowFeedMap[i][0] > elbowPosition) {
+        double percentDifference = (elbowPosition - elbowFeedMap[i-1][0]) / (elbowFeedMap[i][0] - elbowFeedMap[i-1][0]);
+        double feedDifference = elbowFeedMap[i][1] - elbowFeedMap[i-1][1];
+        elbowFeed = (percentDifference * feedDifference) + elbowFeedMap[i-1][1];
+      }
+    }
+
+    setClimberFeedForward(shoulderFeed, elbowFeed);
+  }
+
+  public void setMotors(double shoulderOutput, double elbowOutput) {
+    m_shoulder.set(shoulderOutput);
+    m_elbow.set(elbowOutput);
+  }
+
+  public void setJointAngles(double[] angles) {
+    System.out.println(angles);
+    setJointAngles(angles[0], angles[1]);
+  }
+
+  public void setJointAngles(double shoulderAngle, double elbowAngle) {
+    shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle;
+    elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle;
+
+    shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle;
+    elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle;
+
+    // Convert radians to ticks
+    System.out.println("angles: " + shoulderAngle + ", " + elbowAngle);
+
+    double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
+    double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
+
+    shoulderPosition *= ClimberConstants.SHOULDER_GB_RATIO;
+    elbowPosition *= ClimberConstants.ELBOW_GB_RATIO;
+
+    // shoulderPosition += m_shoulderOffset;
+    // elbowPosition += m_elbowOffset;
+
+    m_shoulder.set(TalonFXControlMode.Position, shoulderPosition);
+    m_elbow.set(TalonFXControlMode.Position, elbowPosition);
+  }
+
+  public void controlWithInput(double xInput, double yInput) {
+    tPoint.x += xInput * ClimberConstants.MOVE_SPEED;
+    tPoint.y += yInput * ClimberConstants.MOVE_SPEED;
   }
 
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
+    double[] jointAngles = getJointAngles(tPoint, 0.d);
+    setJointAngles(jointAngles);
+  }
+
+  /**
+   * Gets joint angles for climber arm
+   * {@code targetPoint.x} and {@code targetPoint.y} are set in the xy plane of the climber, accounting for the
+   * rotation of the robot. Both parameters should be in cm.
+   * Does not set the motors automatically
+   * <p><p>
+   * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399
+   * 
+   * @param targetPoint Target xy point for the climber arm to go to
+   * @param tiltAngle The tilt of the robot
+   * @returns [shoulderAngle, elbowAngle] in radians */
+   public static double[] getJointAngles(Point targetPoint, double tiltAngle) {
+    double [] angles = new double[2];
+  
+    double mag = Math.hypot(targetPoint.x, targetPoint.y);
+    if(mag >= ClimberConstants.MAX_ARM_LENGTH) {
+      targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH;
+      targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH;
+      mag = ClimberConstants.MAX_ARM_LENGTH;
+    } else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) {
+      targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH;
+      targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH;
+      mag = ClimberConstants.MIN_ARM_LENGTH;
+    } else if(mag < ClimberConstants.MIN_ARM_LENGTH) {
+      targetPoint.x = ClimberConstants.MIN_ARM_LENGTH;
+      targetPoint.y = 0.d;
+      mag = ClimberConstants.MIN_ARM_LENGTH;
+    }
+  
+    // The angle to the target point
+    double theta;
+    if(targetPoint.x == 0.d) {
+      theta = Math.PI/2.d; // TODO rename variable
+    } else {
+      theta = Math.atan(targetPoint.y / targetPoint.x);
+    }
+    theta += tiltAngle;
+    
+    if(targetPoint.x < 0.d)
+        theta += Math.PI;
+    
+    
+    // Correct target position for tilt
+    targetPoint.x = Math.cos(theta) * mag;
+    targetPoint.y = Math.sin(theta) * mag;
+    
+    // Law and Order: Cosines edition
+    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 = 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;
+    // System.out.println("sa1: " + shoulderAngle);
+    // System.out.println("ea1: " + 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 = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH;
+      // System.out.println("xDiff: " + xDiff);
+  
+      elbowAngle = Math.atan(-targetPoint.y / xDiff);
+      // System.out.println("ea2: " + elbowAngle);
+      if(elbowAngle < 0) {
+        elbowAngle = Math.PI - Math.abs(elbowAngle);
+      }
+
+      if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE)
+        elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE;
+      // System.out.println("ea3: " + elbowAngle);
+  
+      // Deal with negative wraparound
+      // if(xDiff >= 0.d)
+      //   elbowAngle += Math.PI;
+      // System.out.println("ea4: " + elbowAngle);
+    }
+  
+    angles[0] = shoulderAngle;
+    angles[1] = elbowAngle;
+    return angles;
   }
 }