diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b948df1..d92d66b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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 { 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 03ac015..bec6e92 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/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); + } } diff --git a/src/main/java/frc4388/utility/RobotTime.java b/src/main/java/frc4388/utility/RobotTime.java new file mode 100644 index 0000000..74020e6 --- /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 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; + } +}