Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 08:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

gyro wrapper class done

Browse Source
This commit is contained in:
Ryan Manley
2023-01-10 19:09:27 -07:00
parent 7fde3423d0
commit 997e804f77
1 changed files with 16 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/utility/RobotGyro.java
+16 -14
View File
@@ -28,10 +28,9 @@ public class RobotGyro implements Gyro {
private double m_lastPigeonAngle; private double m_lastPigeonAngle;
private double m_deltaPigeonAngle; private double m_deltaPigeonAngle;
private double diff_Yaw = 0; private double yawZero = 0;
private double diff_Pitch = 0; private double pitchZero = 0;
private double diff_Roll = 0; private double rollZero = 0;
/** /**
* Creates a Gyro based on a pigeon * Creates a Gyro based on a pigeon
@@ -51,12 +50,15 @@ public class RobotGyro implements Gyro {
m_isGyroAPigeon = false; m_isGyroAPigeon = false;
} }
// Sets a /**
// * Resets yaw, pitch, and roll.
public void ResetAng() { */
diff_Yaw = m_pigeon.getYaw(); public void resetAllAngles() {
diff_Pitch = m_pigeon.getPitch(); if (!m_isGyroAPigeon) return;
diff_Roll = m_pigeon.getRoll();
yawZero = m_pigeon.getYaw();
pitchZero = m_pigeon.getPitch();
rollZero = m_pigeon.getRoll();
} }
/** /**
@@ -112,10 +114,10 @@ public class RobotGyro implements Gyro {
* Roll is within [-90,+90] degrees. * Roll is within [-90,+90] degrees.
*/ */
private double[] getPigeonAngles() { private double[] getPigeonAngles() {
double yaw = m_pigeon.getYaw(); double[] ypr = new double[3];
double pitch = m_pigeon.getPitch(); m_pigeon.getYawPitchRoll(ypr);
double roll = m_pigeon.getRoll();
return double[(yaw - diff_Yaw), (pitch - diff_Pitch), (roll - diff_Roll)]; return new double[] {(ypr[0] - yawZero), (ypr[1] - pitchZero), (ypr[2] - rollZero)};
} }
@Override @Override