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

Added input controls

Browse Source
This commit is contained in:
dj12ha
2022-01-20 17:10:07 -07:00
parent 50c83e98f2
commit 94a0af64d0
3 changed files with 21 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Climber.java
+13 -1
View File
@@ -15,6 +15,8 @@ public class Climber extends SubsystemBase {
private WPI_TalonFX m_elbow;
private WPI_PigeonIMU m_gyro;
private double[] position = {0.d, 0.d};
public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) {
m_shoulder = shoulder;
@@ -77,7 +79,7 @@ public class Climber extends SubsystemBase {
public double getRobotTilt() {
double[] ypr = new double[3];
m_gyro.getYawPitchRoll(ypr);
return ypr[1]; // Pitch
return Math.toRadians(ypr[1]); // Pitch
}
public void setJointAngles(double[] angles) {
@@ -87,4 +89,14 @@ public class Climber extends SubsystemBase {
public void setJointAngles(double shoulderAngle, double elbowAngle) {
// Set PIDs
}
public void controlWithInput(double xInput, double yInput) {
position[0] += xInput * ClimberConstants.MOVE_SPEED;
position[1] += yInput * ClimberConstants.MOVE_SPEED;
double tiltAngle = getRobotTilt();
double[] jointAngles = getJointAngles(position[0], position[1], tiltAngle);
setJointAngles(jointAngles);
}
}