Climber auto

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

@@ -187,6 +187,8 @@ public final class Constants {
 
     public static final int CLIMBER_TIMEOUT_MS = 50;
 
+    public static final double THRESHOLD = 0;
+
     // TODO: Update Constants
     // Robot Angle
     public static final double ROBOT_ANGLE_ID = 0;

src/main/java/frc4388/robot/commands/climber/RunClimberPath.java

@@ -8,8 +8,8 @@ import org.opencv.core.Point;
 
 import edu.wpi.first.wpilibj.drive.Vector2d;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ClimberConstants;
 import frc4388.robot.subsystems.Claws;
-import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.ClimberRewrite;
 import frc4388.utility.Vector2D;
 
@@ -20,9 +20,12 @@ public class RunClimberPath extends CommandBase {
   Point[] path;
   int nextIndex;
 
+  boolean endPath;
+
   /** Creates a new RunClimberPath. */
   public RunClimberPath(Point[] _path, ClimberRewrite _climber, Claws _claws) {
     path = _path;
+    endPath = false;
 
     climber = _climber;
     claws = _claws;
@@ -42,10 +45,20 @@ public class RunClimberPath extends CommandBase {
     if(!claws.fullyOpen())
       return;
     
-    Point climberPos = climber.getClimberPosition();
-    Vector2D dir = new Vector2D();
+    Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles());
 
-    climber.controlWithInput(dir.x * .02, dir.y * .02);
+    Vector2D dir = new Vector2D(path[nextIndex]);
+    dir.subtract(new Vector2D(climberPos));
+
+    if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1)
+      nextIndex++;
+    else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) {
+      endPath = true;
+      claws.setOpen(false);
+    } else if(!endPath) {
+      dir = dir.unit();
+      climber.controlWithInput(dir.x, dir.y);
+    }
   }
 
   // Called once the command ends or is interrupted.
@@ -55,6 +68,6 @@ public class RunClimberPath extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return endPath && claws.fullyClosed();
   }
 }

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

@@ -123,6 +123,13 @@ public class ClimberRewrite extends SubsystemBase {
     m_elbow.set(elbowOutput);
   }
 
+  public double[] getJointAngles() {
+    return new double[] {
+      (m_shoulder.getSelectedSensorPosition() * Math.PI) / (Constants.TICKS_PER_ROTATION_FX/2.d) / ClimberConstants.SHOULDER_GB_RATIO,
+      (m_elbow.getSelectedSensorPosition() * Math.PI) / (Constants.TICKS_PER_ROTATION_FX/2.d) / ClimberConstants.ELBOW_GB_RATIO
+    };
+  }
+
   public void setJointAngles(double[] angles) {
     System.out.println(angles);
     setJointAngles(angles[0], angles[1]);
@@ -152,13 +159,13 @@ public class ClimberRewrite extends SubsystemBase {
   }
 
   public void controlWithInput(double xInput, double yInput) {
-    tPoint.x += xInput * ClimberConstants.MOVE_SPEED;
-    tPoint.y += yInput * ClimberConstants.MOVE_SPEED;
+    tPoint.x += xInput * ClimberConstants.MOVE_SPEED * .02;
+    tPoint.y += yInput * ClimberConstants.MOVE_SPEED * .02;
   }
 
   @Override
   public void periodic() {
-    double[] jointAngles = getJointAngles(tPoint, 0.d);
+    double[] jointAngles = getTargetJointAngles(tPoint, 0.d);
     setJointAngles(jointAngles);
   }
 
@@ -172,8 +179,8 @@ public class ClimberRewrite extends SubsystemBase {
    * 
    * @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) {
+   * @return [shoulderAngle, elbowAngle] in radians */
+   public static double[] getTargetJointAngles(Point targetPoint, double tiltAngle) {
     double [] angles = new double[2];
   
     double mag = Math.hypot(targetPoint.x, targetPoint.y);
@@ -251,9 +258,19 @@ public class ClimberRewrite extends SubsystemBase {
     return angles;
   }
 
-  public static Point getClimberPoint(double shoulderAngle, double elbowAngle) {
-    return null;
+  public static Point getClimberPosition(double shoulderAngle, double elbowAngle) {
+    Point climberPoint = new Point(0, 0);
+
+    climberPoint.x += Math.sin(shoulderAngle);
+    climberPoint.y += Math.cos(shoulderAngle);
+
+    climberPoint.x -= Math.sin(elbowAngle - shoulderAngle);
+    climberPoint.y += Math.cos(elbowAngle - shoulderAngle);
+
+    return climberPoint;
   }
 
-  public static Point getClimberPoint
+  public static Point getClimberPosition(double[] jointAngles) {
+    return getClimberPosition(jointAngles[0], jointAngles[1]);
+  }
 }