From fe07715d95d1b640c167c21ad7df53494560f2be Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 24 Jan 2022 17:55:23 -0700 Subject: [PATCH] minor angle adjustments --- src/main/java/frc4388/robot/Constants.java | 14 +++--- .../frc4388/robot/subsystems/Climber.java | 43 +++++++++++-------- 2 files changed, 33 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e694ac0..4076a30 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -82,13 +82,13 @@ public final class Constants { 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 UPPER_ARM_LENGTH = 26; // Units should be in cm + public static final double LOWER_ARM_LENGTH = 27; - public static final double MAX_ARM_LENGTH = 100; - public static final double MIN_ARM_LENGTH = 0; + public static final double MAX_ARM_LENGTH = 53; + public static final double MIN_ARM_LENGTH = 1; - public static final double MOVE_SPEED = .2; // cm per second + public static final double MOVE_SPEED = .1; // cm per second public static final double SHOULDER_RESTING_ANGLE = 0; public static final double ELBOW_RESTING_ANGLE = 0; @@ -100,8 +100,8 @@ public final class Constants { public static final int ELBOW_SLOT_IDX = 0; public static final int ELBOW_PID_LOOP_IDX = 1; - public static final Gains SHOULDER_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains ELBOW_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOULDER_GAINS = new Gains(.4, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains ELBOW_GAINS = new Gains(.4, 0.0, 0.0, 0.0, 0, 1.0); public static final int CLIMBER_TIMEOUT_MS = 50; } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index a3de194..7f9b883 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -23,6 +23,9 @@ public class Climber extends SubsystemBase { private WPI_TalonFX m_shoulder; private WPI_TalonFX m_elbow; + private double m_shoulderOffset; + private double m_elbowOffset; + private WPI_PigeonIMU m_gyro; private boolean m_groundRelative; @@ -39,6 +42,9 @@ public class Climber extends SubsystemBase { setClimberGains(); + m_shoulderOffset = m_shoulder.getSelectedSensorPosition(); + m_elbowOffset = m_elbow.getSelectedSensorPosition(); + if(groundRelative) m_gyro = gyro; @@ -101,24 +107,24 @@ public class Climber extends SubsystemBase { 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); + // 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 = xTarget - ClimberConstants.LOWER_ARM_LENGTH; - System.out.println("xDiff: " + xDiff); + // System.out.println("xDiff: " + xDiff); elbowAngle = Math.atan(-yTarget / xDiff); - System.out.println("ea2: " + elbowAngle); + // 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); + // System.out.println("ea3: " + elbowAngle); // Deal with negative wraparound // if(xDiff >= 0.d) @@ -165,6 +171,9 @@ public class Climber extends SubsystemBase { 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 += m_shoulderOffset; + elbowPosition += m_elbowOffset; + m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); m_elbow.set(TalonFXControlMode.Position, elbowPosition); } @@ -177,23 +186,23 @@ public class Climber extends SubsystemBase { double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d; - double[] testAngles = getJointAngles(0, 0, 0); - System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]); + // double[] testAngles = getJointAngles(0, 0, 0); + // System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]); - double[] testAngles2 = getJointAngles(5000, 5000, 0); - System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]); + // double[] testAngles2 = getJointAngles(5000, 5000, 0); + // System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]); - double[] testAngles3 = getJointAngles(0, 75, 0); - System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]); + // double[] testAngles3 = getJointAngles(0, 75, 0); + // System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]); - double[] testAngles4 = getJointAngles(75, 0, 0); - System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]); + // double[] testAngles4 = getJointAngles(75, 0, 0); + // System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]); - double[] testAngles5 = getJointAngles(-75, 0, 0); - System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]); + // double[] testAngles5 = getJointAngles(-75, 0, 0); + // System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]); - double[] testAngles6 = getJointAngles(60, 25, 0); - System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]); + // double[] testAngles6 = getJointAngles(60, 25, 0); + // System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]); double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle); setJointAngles(jointAngles);