From 970fd819b5af104d67c8d7761d3ac88ace10c8d5 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Wed, 16 Mar 2022 15:46:39 -0600 Subject: [PATCH] Climber auto --- src/main/java/frc4388/robot/Constants.java | 2 ++ .../commands/climber/RunClimberPath.java | 23 +++++++++--- .../robot/subsystems/ClimberRewrite.java | 33 ++++++++++++----- src/main/java/frc4388/utility/Vector2D.java | 36 +++++++++++++++++++ 4 files changed, 81 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bc8776b..52c0845 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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; diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java index 3fc832a..1866be3 100644 --- a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java +++ b/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(); } } diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index c1886c6..dd75f58 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/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]); + } } diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 266d49e..3a3619c 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -58,6 +58,15 @@ public class Vector2D extends Vector2d { return new Vector2D(v1.x + v2.x, v1.y + v2.y); } + /** + * Adds vector to current object + * @param v Vector to add + */ + public void add(Vector2D v) { + x += v.x; + y += v.x; + } + /** * Subtract two vectors, component-wise. * @param v1 First vector in the subtraction. @@ -68,6 +77,15 @@ public class Vector2D extends Vector2d { return new Vector2D(v1.x - v2.x, v1.y - v2.y); } + /** + * Subtracts vector from current object + * @param v Vector to subtract + */ + public void subtract(Vector2D v) { + x -= v.x; + y -= v.x; + } + /** * Multiply a vector with a scalar, component-wise. * @param v1 Vector to multiply. @@ -78,6 +96,15 @@ public class Vector2D extends Vector2d { return new Vector2D(scalar * v1.x, scalar * v1.y); } + /** + * Multiply a vector with a scalar, component-wise. + * @param scalar Scalar to multiply + */ + public void multiply(double scalar) { + x *= scalar; + y *= scalar; + } + /** * Divide a vector with a scalar, component-wise. * @param v1 Vector to divide. @@ -88,6 +115,15 @@ public class Vector2D extends Vector2d { return new Vector2D(v1.x / scalar, v1.y / scalar); } + /** + * Divide a vector with a scalar, component-wise. + * @param scalar Scalar to divide + */ + public void divide(double scalar) { + x /= scalar; + y /= scalar; + } + /** * Find unit vector. * @return The unit vector.