Create RobotTime

To keep track of deltas, frames, and times. Need it for completing pigeon gyro rates.

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

@@ -25,6 +25,8 @@ public final class Constants {
         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

@@ -15,10 +15,12 @@ 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;
 
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.utility.RobotGyro;
+import frc4388.utility.RobotTime;
 
 /**
  * Add your docs here.
@@ -61,10 +63,26 @@ public class Drive extends SubsystemBase {
     m_rightBackMotor.setInverted(InvertType.FollowMaster);
   }
 
+  @Override
+  public void periodic() {
+    if (RobotTime.frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
+      updateSmartDashboard();
+    }
+  }
+
   /**
    * Add your docs here.
    */
   public void driveWithInput(double move, double steer){
     m_driveTrain.arcadeDrive(move, steer);
   }
+
+  private void updateSmartDashboard() {
+
+    // Examples of the functionality of RobotGyro
+    SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.isGyroAPigeon);
+    SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
+    SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
+    SmartDashboard.putData(m_gyro);
+  }
 }

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 currTime = System.currentTimeMillis();
+    public static long deltaTime = 0;
+
+    private static long startRobotTime = currTime;
+    public static long robotTime = 0;
+    public static long lastRobotTime = 0;
+
+    private static long startMatchTime = 0;
+    public static long matchTime = 0;
+    public static long lastMatchTime = 0;
+
+    public static long frameNumber = 0;
+
+    /**
+     * This class should not be instantiated.
+     */
+    private RobotTime(){}
+
+    /**
+     * Call this once per periodic loop.
+     */
+    public static void updateTimes() {
+        lastRobotTime = robotTime;
+        lastMatchTime = matchTime;
+
+        currTime = System.currentTimeMillis();
+        robotTime = currTime - startRobotTime;
+        deltaTime = robotTime - lastRobotTime;
+        frameNumber++;
+
+        if (matchTime != 0) {
+            matchTime = currTime - startMatchTime;
+        }
+    }
+
+    /**
+     * Call this in both the auto and periodic inits
+     */
+    public static void startMatchTime() {
+        if (matchTime == 0) {
+            startMatchTime = currTime;
+            matchTime = 1;
+        }
+    }
+
+    /**
+     * Call this in disabled init
+     */
+    public static void endMatchTime() {
+        startMatchTime = 0;
+        matchTime = 0;
+    }
+}