Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

minor angle adjustments

Browse Source
This commit is contained in:
aarav18
2022-01-24 17:55:23 -07:00
parent 0ecae16cfe
commit fe07715d95
2 changed files with 33 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+7 -7
View File
@@ -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;
}
src/main/java/frc4388/robot/subsystems/Climber.java
+26 -17
View File
@@ -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);