Add basic getRate() functionallity for Pigeons

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);

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) {
+        if (m_isGyroAPigeon) {
-            return 0;
+            return m_deltaPigeonAngle / (RobotTime.m_deltaTime * 1000);
         } else {
             return m_navX.getRate();
         }

src/main/java/frc4388/utility/RobotTime.java

@@ -12,18 +12,18 @@ package frc4388.utility;
  * <p>All times are in milliseconds
  */
 public final class RobotTime {
-    private static long currTime = System.currentTimeMillis();
+    private static long m_currTime = System.currentTimeMillis();
-    public static long deltaTime = 0;
+    public static long m_deltaTime = 0;
 
-    private static long startRobotTime = currTime;
+    private static long m_startRobotTime = m_currTime;
-    public static long robotTime = 0;
+    public static long m_robotTime = 0;
-    public static long lastRobotTime = 0;
+    public static long m_lastRobotTime = 0;
 
-    private static long startMatchTime = 0;
+    private static long m_startMatchTime = 0;
-    public static long matchTime = 0;
+    public static long m_matchTime = 0;
-    public static long lastMatchTime = 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;
+        m_lastRobotTime = m_robotTime;
-        lastMatchTime = matchTime;
+        m_lastMatchTime = m_matchTime;
 
-        currTime = System.currentTimeMillis();
+        m_currTime = System.currentTimeMillis();
-        robotTime = currTime - startRobotTime;
+        m_robotTime = m_currTime - m_startRobotTime;
-        deltaTime = robotTime - lastRobotTime;
+        m_deltaTime = m_robotTime - m_lastRobotTime;
-        frameNumber++;
+        m_frameNumber++;
 
-        if (matchTime != 0) {
+        if (m_matchTime != 0) {
-            matchTime = currTime - startMatchTime;
+            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) {
+        if (m_matchTime == 0) {
-            startMatchTime = currTime;
+            m_startMatchTime = m_currTime;
-            matchTime = 1;
+            m_matchTime = 1;
         }
     }
 
@@ -61,7 +61,7 @@ public final class RobotTime {
      * Call this in disabled init
      */
     public static void endMatchTime() {
-        startMatchTime = 0;
+        m_startMatchTime = 0;
-        matchTime = 0;
+        m_matchTime = 0;
     }
 }