diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 933908d..d92d66b 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/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 {
diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 7735364..d2c3a6c 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/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();
}
/**
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index af19272..6252eba 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/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());
+ }
}
diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java
index a68c124..a2a6f1e 100644
--- a/src/main/java/frc4388/robot/subsystems/LED.java
+++ b/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.
diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java
new file mode 100644
index 0000000..76e1831
--- /dev/null
+++ b/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;
+ }
+
+ /**
+ * NavX:
+ *
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.
+ *
+ *
Pigeon:
+ *
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 {
+
+ }
+}
diff --git a/src/main/java/frc4388/utility/RobotTime.java b/src/main/java/frc4388/utility/RobotTime.java
new file mode 100644
index 0000000..960366b
--- /dev/null
+++ b/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;
+
+/**
+ *
Keeps track of Robot times like time passed, delta time, etc
+ *
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;
+ }
+}
diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json
new file mode 100644
index 0000000..dca1d82
--- /dev/null
+++ b/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"
+ ]
+ }
+ ]
+}
\ No newline at end of file