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

Add getRobotTilt

Browse Source
This commit is contained in:
66945
2022-01-20 16:37:57 -07:00
parent 3567cfbeb0
commit 50c83e98f2
2 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+1 -1
View File
@@ -42,7 +42,7 @@ public class RobotContainer {
private final LED m_robotLED = new LED(m_robotMap.LEDController);
private final Climber m_climber = new Climber(m_robotMap.shoulder, m_robotMap.elbow);
private final Climber m_climber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro);
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
src/main/java/frc4388/robot/subsystems/Climber.java
+8 -1
View File
@@ -44,12 +44,13 @@ public class Climber extends SubsystemBase {
// The angle to the target point
double theta = Math.atan(yTarget / xTarget) + tiltAngle; // TODO rename variable
// Correct target position for tilt
xTarget = Math.cos(theta) * mag;
yTarget = Math.sin(theta) * mag;
// Law and Order: Cosines edition
double shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) -Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) /
double shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) /
(2.d * ClimberConstants.LOWER_ARM_LENGTH * mag));
shoulderAngle = theta - shoulderAngle;
@@ -73,6 +74,12 @@ public class Climber extends SubsystemBase {
return angles;
}
public double getRobotTilt() {
double[] ypr = new double[3];
m_gyro.getYawPitchRoll(ypr);
return ypr[1]; // Pitch
}
public void setJointAngles(double[] angles) {
setJointAngles(angles[0], angles[1]);
}