Team4388/Robot-Essentials
1
0
Fork 0
mirror of https://github.com/Team4388/Robot-Essentials.git synced 2026-06-09 00:38:01 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add basic getRate() functionallity for Pigeons

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-03-27 12:32:46 -06:00
parent ecff989745
commit 4271a66d1a
3 changed files with 50 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Drive.java
+4 -4
View File
@@ -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);
src/main/java/frc4388/utility/RobotGyro.java
+24 -12
View File
@@ -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();
}
src/main/java/frc4388/utility/RobotTime.java
+22 -22
View File
@@ -12,18 +12,18 @@ package frc4388.utility;
* <p>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;
}
}