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

speed limiters for elbow and shoulder

Browse Source
This commit is contained in:
aarav18
2022-03-18 14:45:27 -06:00
parent e920a2a668
commit cb3f1942b6
4 changed files with 52 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+6 -5
View File
@@ -175,7 +175,8 @@ public final class Constants {
public static final int GYRO_ID = 14;
public static final double INPUT_MULTIPLIER = 0.7;
public static final double CLIMBER_SOFT_LIMIT_TOLERANCE = 20.0;
public static final double ELBOW_SOFT_LIMIT_TOLERANCE = 20000.0;
public static final double SHOULDER_SOFT_LIMIT_TOLERANCE = 12000.0;
// TODO Update this stuff too
public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm
@@ -195,10 +196,10 @@ public final class Constants {
public static final double ELBOW_GB_RATIO = 1.d;
public static final double SHOULDER_GB_RATIO = 1.d;
public static final double SHOULDER_SOFT_LIMIT_FORWARD = 53869;
public static final double SHOULDER_SOFT_LIMIT_REVERSE = 0;
public static final double ELBOW_SOFT_LIMIT_FORWARD = 281717;
public static final double ELBOW_SOFT_LIMIT_REVERSE = 0;
public static final double SHOULDER_FORWARD_SOFT_LIMIT = 53869;
public static final double SHOULDER_REVERSE_SOFT_LIMIT = 0;
public static final double ELBOW_FORWARD_SOFT_LIMIT = 281717;
public static final double ELBOW_REVERSE_SOFT_LIMIT = 0;
// PID Constants
public static final int SHOULDER_SLOT_IDX = 0;
src/main/java/frc4388/robot/subsystems/ClimberRewrite.java
+42 -6
View File
@@ -37,6 +37,8 @@ public class ClimberRewrite extends SubsystemBase {
private Point tPoint;
private boolean groundRelative;
private double shoulderSpeedLimiter;
private double elbowSpeedLimiter;
/** Creates a new ClimberRewrite. */
public ClimberRewrite(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean _groundRelative) {
@@ -55,14 +57,14 @@ public class ClimberRewrite extends SubsystemBase {
// m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
// m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD);
m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT);
m_elbow.configForwardSoftLimitEnable(true);
// m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE);
// m_elbow.configReverseSoftLimitEnable(true);
m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD);
m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT);
m_shoulder.configForwardSoftLimitEnable(true);
m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE);
m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT);
m_shoulder.configReverseSoftLimitEnable(false);
m_shoulder.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
@@ -78,6 +80,7 @@ public class ClimberRewrite extends SubsystemBase {
m_gyro = gyro;
groundRelative = _groundRelative;
this.elbowSpeedLimiter = 1.0;
}
public void setClimberGains() {
@@ -129,8 +132,8 @@ public class ClimberRewrite extends SubsystemBase {
}
public void setMotors(double shoulderOutput, double elbowOutput) {
m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER);
m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER);
m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter);
m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter);
}
public double[] getJointAngles() {
@@ -179,7 +182,40 @@ public class ClimberRewrite extends SubsystemBase {
SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition());
// double[] jointAngles = getTargetJointAngles(tPoint, 0.d);
// setJointAngles(jointAngles);
// * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output).
double currentElbowPos = this.m_elbow.getSelectedSensorPosition();
double forwardElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT);
double reverseElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_REVERSE_SOFT_LIMIT);
if (forwardElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) {
this.elbowSpeedLimiter = 0.15 + (forwardElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE));
}
if (reverseElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) {
this.elbowSpeedLimiter = 0.15 + (reverseElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE));
}
if ((forwardElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) && (reverseElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)) {
this.elbowSpeedLimiter = 1.0;
}
// * speed limiting near SHOULDER soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output).
double currentShoulderPos = this.m_shoulder.getSelectedSensorPosition();
double forwardShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT);
double reverseShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT);
if (forwardShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) {
this.shoulderSpeedLimiter = 0.15 + (forwardShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE));
}
if (reverseShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) {
this.shoulderSpeedLimiter = 0.15 + (reverseShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE));
}
if ((forwardShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) && (reverseShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)) {
this.shoulderSpeedLimiter = 1.0;
}
}
/**
src/main/java/frc4388/robot/subsystems/Hood.java
+2 -5
View File
@@ -68,19 +68,16 @@ public class Hood extends SubsystemBase {
double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT);
if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (forwardDistance * 0.05);
this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * 0.05);
this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) {
this.speedLimiter = 1.0;
}
double hoodCurrent = m_angleAdjusterMotor.getOutputCurrent();
}
/**
src/main/java/frc4388/robot/subsystems/Turret.java
+2 -2
View File
@@ -152,11 +152,11 @@ public class Turret extends SubsystemBase {
double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT);
if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (forwardDistance * 0.05);
this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
}
if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * 0.05);
this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
}
if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) {