From e8be8ef16e425b3d4119e6d0a7b8c2f328650a44 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 27 Mar 2020 08:54:56 -0600 Subject: [PATCH 1/6] Remove static declaration where applicable Using these objects should instead be done by passing the needed subsystem reference in RobotContainer --- src/main/java/frc4388/robot/subsystems/Drive.java | 11 ++++++----- src/main/java/frc4388/robot/subsystems/LED.java | 4 ++-- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index af19272..780ce83 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -23,12 +23,13 @@ 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 WPI_TalonSRX m_leftFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); + public WPI_TalonSRX m_rightFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); + public WPI_TalonSRX m_leftBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); + public WPI_TalonSRX m_rightBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); + public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + - public static DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); /** * Add your docs here. diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index a68c124..e550623 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; + public float currentLED; + public Spark LEDController; /** * Add your docs here. From f310c8d90ab1caddada83a757b0fd9cfb153de96 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 27 Mar 2020 11:36:35 -0600 Subject: [PATCH 2/6] Create the RobotGyro Object Will allow for easily interchangable use of a pigeon or a navX. Just swap which gyro you pass RobotGyro. More advanced functionality like RemoteSensors with the pigeon and collision detection with the navX will have to be done by getting the gyro from RobotGyro and then using it. --- src/main/java/frc4388/robot/Constants.java | 4 +- .../java/frc4388/robot/subsystems/Drive.java | 7 +- src/main/java/frc4388/utility/RobotGyro.java | 158 ++++++++++++++++++ vendordeps/navx_frc.json | 35 ++++ 4 files changed, 201 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc4388/utility/RobotGyro.java create mode 100644 vendordeps/navx_frc.json diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 933908d..b948df1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -22,7 +22,9 @@ 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 class LEDConstants { diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 780ce83..03ac015 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 com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.wpilibj.GyroBase; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.DriveConstants; +import frc4388.utility.RobotGyro; /** * Add your docs here. @@ -28,8 +32,7 @@ public class Drive extends SubsystemBase { public WPI_TalonSRX m_leftBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); public WPI_TalonSRX m_rightBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - - + public RobotGyro m_gyro = new RobotGyro(new PigeonIMU(DriveConstants.DRIVE_PIGEON_ID)); /** * 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..40b08b7 --- /dev/null +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -0,0 +1,158 @@ +/*----------------------------------------------------------------------------*/ +/* 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.ctre.phoenix.sensors.PigeonIMU.PigeonState; +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 isGyroAPigeon; //true if pigeon, false if navX + + /** + * Creates a Gyro based on a pigeon + * @param gyro the gyroscope to use for Gyro + */ + public RobotGyro(PigeonIMU gyro) { + m_pigeon = gyro; + isGyroAPigeon = true; + } + + /** + * Creates a Gyro based on a navX + * @param gyro the gyroscope to use for Gyro + */ + public RobotGyro(AHRS gyro){ + m_navX = gyro; + isGyroAPigeon = false; + } + + /** + *

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 (isGyroAPigeon) { + m_pigeon.enterCalibrationMode(CalibrationMode.Temperature); + } else { + m_navX.calibrate(); + } + } + + @Override + public void reset() { + if (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. + */ + public double[] getPigeonAngles() { + double[] angles = new double[3]; + m_pigeon.getYawPitchRoll(angles); + return angles; + } + + @Override + public double getAngle() { + if (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 (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 (isGyroAPigeon) { + return MathUtil.clamp(getPigeonAngles()[2], -90, 90); + } else { + return MathUtil.clamp(m_navX.getRoll(), -90, 90); + } + } + + @Override + public double getRate() { + if (isGyroAPigeon) { + return 0; + } 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/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 From ecff98974506d6255361d13d8ff633a12966ace7 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 27 Mar 2020 12:20:22 -0600 Subject: [PATCH 3/6] Create RobotTime To keep track of deltas, frames, and times. Need it for completing pigeon gyro rates. --- src/main/java/frc4388/robot/Constants.java | 2 + src/main/java/frc4388/robot/Robot.java | 5 ++ .../java/frc4388/robot/subsystems/Drive.java | 18 +++++ src/main/java/frc4388/utility/RobotTime.java | 67 +++++++++++++++++++ 4 files changed, 92 insertions(+) create mode 100644 src/main/java/frc4388/utility/RobotTime.java 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; + } +} From 4271a66d1ad35629457b4f16c9cb435744bbaab7 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 27 Mar 2020 12:32:46 -0600 Subject: [PATCH 4/6] Add basic getRate() functionallity for Pigeons --- .../java/frc4388/robot/subsystems/Drive.java | 8 ++-- src/main/java/frc4388/utility/RobotGyro.java | 36 ++++++++++----- src/main/java/frc4388/utility/RobotTime.java | 44 +++++++++---------- 3 files changed, 50 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index bec6e92..a0444f3 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/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); diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 40b08b7..76e1831 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/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) { - return 0; + if (m_isGyroAPigeon) { + return m_deltaPigeonAngle / (RobotTime.m_deltaTime * 1000); } else { return m_navX.getRate(); } diff --git a/src/main/java/frc4388/utility/RobotTime.java b/src/main/java/frc4388/utility/RobotTime.java index 74020e6..960366b 100644 --- a/src/main/java/frc4388/utility/RobotTime.java +++ b/src/main/java/frc4388/utility/RobotTime.java @@ -12,18 +12,18 @@ package frc4388.utility; *

All times are in milliseconds */ public final class RobotTime { - private static long currTime = System.currentTimeMillis(); - public static long deltaTime = 0; + private static long m_currTime = System.currentTimeMillis(); + public static long m_deltaTime = 0; - private static long startRobotTime = currTime; - public static long robotTime = 0; - public static long lastRobotTime = 0; + private static long m_startRobotTime = m_currTime; + public static long m_robotTime = 0; + public static long m_lastRobotTime = 0; - private static long startMatchTime = 0; - public static long matchTime = 0; - public static long lastMatchTime = 0; + private static long m_startMatchTime = 0; + public static long m_matchTime = 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; - lastMatchTime = matchTime; + m_lastRobotTime = m_robotTime; + m_lastMatchTime = m_matchTime; - currTime = System.currentTimeMillis(); - robotTime = currTime - startRobotTime; - deltaTime = robotTime - lastRobotTime; - frameNumber++; + m_currTime = System.currentTimeMillis(); + m_robotTime = m_currTime - m_startRobotTime; + m_deltaTime = m_robotTime - m_lastRobotTime; + m_frameNumber++; - if (matchTime != 0) { - matchTime = currTime - startMatchTime; + if (m_matchTime != 0) { + 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) { - startMatchTime = currTime; - matchTime = 1; + if (m_matchTime == 0) { + m_startMatchTime = m_currTime; + m_matchTime = 1; } } @@ -61,7 +61,7 @@ public final class RobotTime { * Call this in disabled init */ public static void endMatchTime() { - startMatchTime = 0; - matchTime = 0; + m_startMatchTime = 0; + m_matchTime = 0; } } From 17fbc039997709602fb54c2164f527ff138ace54 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 27 Mar 2020 13:42:20 -0600 Subject: [PATCH 5/6] Changed subsystem variables to private where applicable May need to be changed later, but try to use methods to access these variables instead of accessing them directly, as it fits better with the command based model and will be easier to rewrite should motors change, usage changes, etc. --- .../java/frc4388/robot/subsystems/Drive.java | 16 ++++++++++------ src/main/java/frc4388/robot/subsystems/LED.java | 4 ++-- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index a0444f3..3323992 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -27,12 +27,12 @@ public class Drive extends SubsystemBase { // Put methods for controlling this subsystem // here. Call these from Commands. - public WPI_TalonSRX m_leftFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); - public WPI_TalonSRX m_rightFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); - public WPI_TalonSRX m_leftBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); - public WPI_TalonSRX m_rightBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); - public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - public RobotGyro m_gyro = new RobotGyro(new PigeonIMU(DriveConstants.DRIVE_PIGEON_ID)); + 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. @@ -77,6 +77,10 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } + public RobotGyro getRobotGyro(){ + return m_gyro; + } + private void updateSmartDashboard() { // Examples of the functionality of RobotGyro diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index e550623..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 float currentLED; - public Spark LEDController; + private float currentLED; + private Spark LEDController; /** * Add your docs here. From 2c5ae300b4b7fc8779950a096bc16b1df20156ac Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 27 Mar 2020 13:44:10 -0600 Subject: [PATCH 6/6] Example Usage of getRobotGyro If you need to access an object from outside the subsystem, you should do this. --- src/main/java/frc4388/robot/subsystems/Drive.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 3323992..6252eba 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -84,9 +84,9 @@ public class Drive extends SubsystemBase { private void updateSmartDashboard() { // Examples of the functionality of RobotGyro - 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); + SmartDashboard.putBoolean("Is Gyro a Pigeon?", getRobotGyro().m_isGyroAPigeon); + SmartDashboard.putNumber("Turn Rate", getRobotGyro().getRate()); + SmartDashboard.putNumber("Gyro Pitch", getRobotGyro().getPitch()); + SmartDashboard.putData(getRobotGyro()); } }