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

Added limits

Browse Source
This commit is contained in:
66945
2022-01-24 19:09:20 -07:00
parent fe07715d95
commit 38387e3173
2 changed files with 11 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+5
View File
@@ -93,6 +93,11 @@ public final class Constants {
public static final double SHOULDER_RESTING_ANGLE = 0;
public static final double ELBOW_RESTING_ANGLE = 0;
public static final double SHOULDER_MAX_ANGLE = 135;
public static final double ELBOW_MAX_ANGLE = 180;
public static final double GEAR_BOX_RATIO = 144.d * 60.d / 24.d;
// PID Constants
public static final int SHOULDER_SLOT_IDX = 0;
public static final int SHOULDER_PID_LOOP_IDX = 1;
src/main/java/frc4388/robot/subsystems/Climber.java
+6
View File
@@ -165,12 +165,18 @@ public class Climber extends SubsystemBase {
}
public void setJointAngles(double shoulderAngle, double elbowAngle) {
shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle;
elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle;
// Convert radians to ticks
System.out.println("angles: " + shoulderAngle + ", " + elbowAngle);
double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
shoulderAngle *= ClimberConstants.GEAR_BOX_RATIO;
elbowAngle *= ClimberConstants.GEAR_BOX_RATIO;
shoulderPosition += m_shoulderOffset;
elbowPosition += m_elbowOffset;