Merge pull request #9 from Team4388/add-gyro

Add Gyroscope to Robot Essentials

src/main/java/frc4388/robot/Constants.java

@@ -22,7 +22,11 @@ public final class Constants {
         public static final int DRIVE_LEFT_FRONT_CAN_ID = 2;
         public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4;
 	    public static final int DRIVE_LEFT_BACK_CAN_ID = 3;
-	    public static final int DRIVE_RIGHT_BACK_CAN_ID = 5;
+        public static final int DRIVE_RIGHT_BACK_CAN_ID = 5;
+        
+        public static final int DRIVE_PIGEON_ID = 6;
+
+        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
     }
 
     public static final class LEDConstants {

src/main/java/frc4388/robot/Robot.java

@@ -10,6 +10,7 @@ package frc4388.robot;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.utility.RobotTime;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -44,6 +45,7 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void robotPeriodic() {
+    RobotTime.updateTimes();
     // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
     // commands, running already-scheduled commands, removing finished or interrupted commands,
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
@@ -58,6 +60,7 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void disabledInit() {
+    RobotTime.endMatchTime();
   }
 
   @Override
@@ -82,6 +85,7 @@ public class Robot extends TimedRobot {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
     }
+    RobotTime.startMatchTime();
   }
 
   /**
@@ -100,6 +104,7 @@ public class Robot extends TimedRobot {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+    RobotTime.startMatchTime();
   }
 
   /**

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -10,11 +10,15 @@ package frc4388.robot.subsystems;
 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 edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import frc4388.robot.Constants.DriveConstants;
+import frc4388.utility.RobotGyro;
+import frc4388.utility.RobotTime;
 
 /**
  * Add your docs here.
@@ -23,12 +27,12 @@ public class Drive extends SubsystemBase {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
 
-  public static WPI_TalonSRX m_leftFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
-  public static WPI_TalonSRX m_rightFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
-  public static WPI_TalonSRX m_leftBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
-  public static WPI_TalonSRX m_rightBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
-
-  public static DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
+  private WPI_TalonSRX m_leftFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
+  private WPI_TalonSRX m_rightFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
+  private WPI_TalonSRX m_leftBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
+  private WPI_TalonSRX m_rightBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
+  private DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
+  private RobotGyro m_gyro = new RobotGyro(new PigeonIMU(DriveConstants.DRIVE_PIGEON_ID));
 
   /**
    * Add your docs here.
@@ -57,10 +61,32 @@ public class Drive extends SubsystemBase {
     m_rightBackMotor.setInverted(InvertType.FollowMaster);
   }
 
+  @Override
+  public void periodic() {
+    m_gyro.updatePigeonDeltas();
+
+    if (RobotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
+      updateSmartDashboard();
+    }
+  }
+
   /**
    * Add your docs here.
    */
   public void driveWithInput(double move, double steer){
     m_driveTrain.arcadeDrive(move, steer);
   }
+
+  public RobotGyro getRobotGyro(){
+    return m_gyro;
+  }
+
+  private void updateSmartDashboard() {
+
+    // Examples of the functionality of RobotGyro
+    SmartDashboard.putBoolean("Is Gyro a Pigeon?", getRobotGyro().m_isGyroAPigeon);
+    SmartDashboard.putNumber("Turn Rate", getRobotGyro().getRate());
+    SmartDashboard.putNumber("Gyro Pitch", getRobotGyro().getPitch());
+    SmartDashboard.putData(getRobotGyro());
+  }
 }

src/main/java/frc4388/robot/subsystems/LED.java

@@ -20,8 +20,8 @@ import frc4388.utility.LEDPatterns;
  */
 public class LED extends SubsystemBase {
 
-  public static float currentLED;
-  public static Spark LEDController;
+  private float currentLED;
+  private Spark LEDController;
 
   /**
    * Add your docs here.

src/main/java/frc4388/utility/RobotGyro.java

@@ -0,0 +1,170 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.utility;
+
+import com.ctre.phoenix.sensors.PigeonIMU;
+import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
+import com.kauailabs.navx.frc.AHRS;
+
+import edu.wpi.first.wpilibj.GyroBase;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * Gyro class that allows for interchangeable use between a pigeon and a navX
+ */
+public class RobotGyro extends GyroBase {
+    private PigeonIMU m_pigeon;
+    private AHRS m_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
+     * @param gyro the gyroscope to use for Gyro
+     */
+    public RobotGyro(PigeonIMU gyro) {
+        m_pigeon = gyro;
+        m_isGyroAPigeon = true;
+    }
+
+    /**
+     * Creates a Gyro based on a navX
+     * @param gyro the gyroscope to use for Gyro
+     */
+    public RobotGyro(AHRS gyro){
+        m_navX = gyro;
+        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;
+    }
+
+    /**
+     * <p>NavX:
+     * <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
+     * the center value as the Accumulator center value for subsequent measurements. It's important to
+     * make sure that the robot is not moving while the centering calculations are in progress, this
+     * is typically done when the robot is first turned on while it's sitting at rest before the
+     * competition starts.
+     * 
+     * <p>Pigeon:
+     * <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
+     * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
+     * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
+     * make sure that the robot is not moving while the tempurature calculations are in progress, this
+     * is typically done when the robot is first turned on while it's sitting at rest before the
+     * competition starts.
+     */
+    @Override
+    public void calibrate() {
+        if (m_isGyroAPigeon) {
+            m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
+        } else {
+            m_navX.calibrate();
+        }
+    }
+
+    @Override
+    public void reset() {
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(0);
+        } else {
+            m_navX.reset();
+        }
+    }
+
+    /**
+	 * Get Yaw, Pitch, and Roll data.
+	 *
+	 * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
+	 *					Yaw is within [-368,640, +368,640] degrees.
+	 *					Pitch is within [-90,+90] degrees.
+	 *					Roll is within [-90,+90] degrees.
+     */
+    private double[] getPigeonAngles() {
+        double[] angles = new double[3];
+        m_pigeon.getYawPitchRoll(angles);
+        return angles;
+    }
+
+    @Override
+    public double getAngle() {
+        if (m_isGyroAPigeon) {
+            return getPigeonAngles()[0];
+        } else {
+            return m_navX.getAngle();
+        }
+    }
+
+    /**
+     * Gets an absolute heading of the robot
+     * @return heading from -180 to 180 degrees
+     */
+    public double getHeading() {
+        return Math.IEEEremainder(getAngle(), 360);
+    }
+
+    /**
+     * Returns the current pitch value (in degrees, from -90 to 90)
+     * reported by the sensor.  Pitch is a measure of rotation around
+     * the Y Axis.
+     * @return The current pitch value in degrees (-90 to 90).
+     */
+    public double getPitch() {
+        if (m_isGyroAPigeon) {
+            return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
+        } else {
+            return MathUtil.clamp(m_navX.getPitch(), -90, 90);
+        }
+    }
+
+    /**
+     * Returns the current roll value (in degrees, from -90 to 90)
+     * reported by the sensor.  Roll is a measure of rotation around
+     * the X Axis.
+     * @return The current roll value in degrees (-90 to 90).
+     */
+    public double getRoll() {
+        if (m_isGyroAPigeon) {
+            return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
+        } else {
+            return MathUtil.clamp(m_navX.getRoll(), -90, 90);
+        }
+    }
+
+    @Override
+    public double getRate() {
+        if (m_isGyroAPigeon) {
+            return m_deltaPigeonAngle / (RobotTime.m_deltaTime * 1000);
+        } else {
+            return m_navX.getRate();
+        }
+    }
+
+    public PigeonIMU getPigeon(){
+        return m_pigeon;
+    }
+
+    public AHRS getNavX(){
+        return m_navX;
+    }
+
+    @Override
+    public void close() throws Exception {
+
+    }
+}

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

@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.utility;
+
+/**
+ * <p>Keeps track of Robot times like time passed, delta time, etc
+ * <p>All times are in milliseconds
+ */
+public final class RobotTime {
+    private static long m_currTime = System.currentTimeMillis();
+    public static long m_deltaTime = 0;
+
+    private static long m_startRobotTime = m_currTime;
+    public static long m_robotTime = 0;
+    public static long m_lastRobotTime = 0;
+
+    private static long m_startMatchTime = 0;
+    public static long m_matchTime = 0;
+    public static long m_lastMatchTime = 0;
+
+    public static long m_frameNumber = 0;
+
+    /**
+     * This class should not be instantiated.
+     */
+    private RobotTime(){}
+
+    /**
+     * Call this once per periodic loop.
+     */
+    public static void updateTimes() {
+        m_lastRobotTime = m_robotTime;
+        m_lastMatchTime = m_matchTime;
+
+        m_currTime = System.currentTimeMillis();
+        m_robotTime = m_currTime - m_startRobotTime;
+        m_deltaTime = m_robotTime - m_lastRobotTime;
+        m_frameNumber++;
+
+        if (m_matchTime != 0) {
+            m_matchTime = m_currTime - m_startMatchTime;
+        }
+    }
+
+    /**
+     * Call this in both the auto and periodic inits
+     */
+    public static void startMatchTime() {
+        if (m_matchTime == 0) {
+            m_startMatchTime = m_currTime;
+            m_matchTime = 1;
+        }
+    }
+
+    /**
+     * Call this in disabled init
+     */
+    public static void endMatchTime() {
+        m_startMatchTime = 0;
+        m_matchTime = 0;
+    }
+}

vendordeps/navx_frc.json

@@ -0,0 +1,35 @@
+{
+    "fileName": "navx_frc.json",
+    "name": "KauaiLabs_navX_FRC",
+    "version": "3.1.413",
+    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "mavenUrls": [
+        "https://repo1.maven.org/maven2/"
+    ],
+    "jsonUrl": "https://www.kauailabs.com/dist/frc/2020/navx_frc.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-java",
+            "version": "3.1.413"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-cpp",
+            "version": "3.1.413",
+            "headerClassifier": "headers",
+            "sourcesClassifier": "sources",
+            "sharedLibrary": false,
+            "libName": "navx_frc",
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxraspbian",
+                "windowsx86-64"
+            ]
+        }
+    ]
+}