diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index bec6e92..a0444f3 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -11,9 +11,7 @@ import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.sensors.PigeonIMU; -import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.wpilibj.GyroBase; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -65,7 +63,9 @@ public class Drive extends SubsystemBase { @Override public void periodic() { - if (RobotTime.frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { + m_gyro.updatePigeonDeltas(); + + if (RobotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { updateSmartDashboard(); } } @@ -80,7 +80,7 @@ public class Drive extends SubsystemBase { private void updateSmartDashboard() { // Examples of the functionality of RobotGyro - SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.isGyroAPigeon); + SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon); SmartDashboard.putNumber("Turn Rate", m_gyro.getRate()); SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); SmartDashboard.putData(m_gyro); diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 40b08b7..76e1831 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -9,7 +9,6 @@ package frc4388.utility; import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode; -import com.ctre.phoenix.sensors.PigeonIMU.PigeonState; import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.GyroBase; @@ -21,7 +20,10 @@ import edu.wpi.first.wpiutil.math.MathUtil; public class RobotGyro extends GyroBase { private PigeonIMU m_pigeon; private AHRS m_navX; - public boolean isGyroAPigeon; //true if pigeon, false if navX + public boolean m_isGyroAPigeon; //true if pigeon, false if navX + + private double m_lastPigeonAngle; + private double m_deltaPigeonAngle; /** * Creates a Gyro based on a pigeon @@ -29,7 +31,7 @@ public class RobotGyro extends GyroBase { */ public RobotGyro(PigeonIMU gyro) { m_pigeon = gyro; - isGyroAPigeon = true; + m_isGyroAPigeon = true; } /** @@ -38,7 +40,17 @@ public class RobotGyro extends GyroBase { */ public RobotGyro(AHRS gyro){ m_navX = gyro; - isGyroAPigeon = false; + m_isGyroAPigeon = false; + } + + /** + * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note + * that the getRate() method for a navX will likely be much more accurate than for a pigeon. + */ + public void updatePigeonDeltas() { + double currentPigeonAngle = getAngle(); + m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle; + m_lastPigeonAngle = currentPigeonAngle; } /** @@ -59,7 +71,7 @@ public class RobotGyro extends GyroBase { */ @Override public void calibrate() { - if (isGyroAPigeon) { + if (m_isGyroAPigeon) { m_pigeon.enterCalibrationMode(CalibrationMode.Temperature); } else { m_navX.calibrate(); @@ -68,7 +80,7 @@ public class RobotGyro extends GyroBase { @Override public void reset() { - if (isGyroAPigeon) { + if (m_isGyroAPigeon) { m_pigeon.setYaw(0); } else { m_navX.reset(); @@ -83,7 +95,7 @@ public class RobotGyro extends GyroBase { * Pitch is within [-90,+90] degrees. * Roll is within [-90,+90] degrees. */ - public double[] getPigeonAngles() { + private double[] getPigeonAngles() { double[] angles = new double[3]; m_pigeon.getYawPitchRoll(angles); return angles; @@ -91,7 +103,7 @@ public class RobotGyro extends GyroBase { @Override public double getAngle() { - if (isGyroAPigeon) { + if (m_isGyroAPigeon) { return getPigeonAngles()[0]; } else { return m_navX.getAngle(); @@ -113,7 +125,7 @@ public class RobotGyro extends GyroBase { * @return The current pitch value in degrees (-90 to 90). */ public double getPitch() { - if (isGyroAPigeon) { + if (m_isGyroAPigeon) { return MathUtil.clamp(getPigeonAngles()[1], -90, 90); } else { return MathUtil.clamp(m_navX.getPitch(), -90, 90); @@ -127,7 +139,7 @@ public class RobotGyro extends GyroBase { * @return The current roll value in degrees (-90 to 90). */ public double getRoll() { - if (isGyroAPigeon) { + if (m_isGyroAPigeon) { return MathUtil.clamp(getPigeonAngles()[2], -90, 90); } else { return MathUtil.clamp(m_navX.getRoll(), -90, 90); @@ -136,8 +148,8 @@ public class RobotGyro extends GyroBase { @Override public double getRate() { - if (isGyroAPigeon) { - return 0; + if (m_isGyroAPigeon) { + return m_deltaPigeonAngle / (RobotTime.m_deltaTime * 1000); } else { return m_navX.getRate(); } diff --git a/src/main/java/frc4388/utility/RobotTime.java b/src/main/java/frc4388/utility/RobotTime.java index 74020e6..960366b 100644 --- a/src/main/java/frc4388/utility/RobotTime.java +++ b/src/main/java/frc4388/utility/RobotTime.java @@ -12,18 +12,18 @@ package frc4388.utility; *

All times are in milliseconds */ public final class RobotTime { - private static long currTime = System.currentTimeMillis(); - public static long deltaTime = 0; + private static long m_currTime = System.currentTimeMillis(); + public static long m_deltaTime = 0; - private static long startRobotTime = currTime; - public static long robotTime = 0; - public static long lastRobotTime = 0; + private static long m_startRobotTime = m_currTime; + public static long m_robotTime = 0; + public static long m_lastRobotTime = 0; - private static long startMatchTime = 0; - public static long matchTime = 0; - public static long lastMatchTime = 0; + private static long m_startMatchTime = 0; + public static long m_matchTime = 0; + public static long m_lastMatchTime = 0; - public static long frameNumber = 0; + public static long m_frameNumber = 0; /** * This class should not be instantiated. @@ -34,16 +34,16 @@ public final class RobotTime { * Call this once per periodic loop. */ public static void updateTimes() { - lastRobotTime = robotTime; - lastMatchTime = matchTime; + m_lastRobotTime = m_robotTime; + m_lastMatchTime = m_matchTime; - currTime = System.currentTimeMillis(); - robotTime = currTime - startRobotTime; - deltaTime = robotTime - lastRobotTime; - frameNumber++; + m_currTime = System.currentTimeMillis(); + m_robotTime = m_currTime - m_startRobotTime; + m_deltaTime = m_robotTime - m_lastRobotTime; + m_frameNumber++; - if (matchTime != 0) { - matchTime = currTime - startMatchTime; + if (m_matchTime != 0) { + m_matchTime = m_currTime - m_startMatchTime; } } @@ -51,9 +51,9 @@ public final class RobotTime { * Call this in both the auto and periodic inits */ public static void startMatchTime() { - if (matchTime == 0) { - startMatchTime = currTime; - matchTime = 1; + if (m_matchTime == 0) { + m_startMatchTime = m_currTime; + m_matchTime = 1; } } @@ -61,7 +61,7 @@ public final class RobotTime { * Call this in disabled init */ public static void endMatchTime() { - startMatchTime = 0; - matchTime = 0; + m_startMatchTime = 0; + m_matchTime = 0; } }