From 3567cfbeb097a19507e6d6146605abeb6943814f Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Tue, 18 Jan 2022 20:00:03 -0700 Subject: [PATCH] Finish IK, accounting for robot tilt --- src/main/java/frc4388/robot/Constants.java | 4 ++ src/main/java/frc4388/robot/RobotMap.java | 4 +- .../frc4388/robot/subsystems/Climber.java | 66 +++++++++++++++---- 3 files changed, 62 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 950a574..f514404 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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 diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 90c5dc1..3aa9b0a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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 } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 0deb63f..8a170b3 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/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 } }