mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-08 16:28:04 -06:00
Remove RobotGyro
This commit is contained in:
Michael Mikovsky
@@ -45,6 +45,8 @@ public class Robot extends LoggedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void robotInit() {
|
||||
// Start logging with AdvantageKit
|
||||
startLogging();
|
||||
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
@@ -221,7 +223,7 @@ public class Robot extends LoggedRobot {
|
||||
// Record metadata
|
||||
Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
|
||||
Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
|
||||
Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
|
||||
Logger.recordMetadata("Git+SHA", BuildConstants.GIT_SHA);
|
||||
Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
|
||||
Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
|
||||
switch (BuildConstants.DIRTY) {
|
||||
|
||||
@@ -7,12 +7,12 @@ public final class BuildConstants {
|
||||
public static final String MAVEN_GROUP = "";
|
||||
public static final String MAVEN_NAME = "2025RidgeScape";
|
||||
public static final String VERSION = "unspecified";
|
||||
public static final int GIT_REVISION = 165;
|
||||
public static final String GIT_SHA = "bf4da44338d8e91c50e34c3ac68dc12f9d335b08";
|
||||
public static final String GIT_DATE = "2025-07-11 14:07:53 MDT";
|
||||
public static final int GIT_REVISION = 166;
|
||||
public static final String GIT_SHA = "c183c08a3d92ff8561bef96dad52b8dd64d94f14";
|
||||
public static final String GIT_DATE = "2025-07-11 17:58:22 MDT";
|
||||
public static final String GIT_BRANCH = "advantagekit";
|
||||
public static final String BUILD_DATE = "2025-07-11 17:46:28 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1752277588371L;
|
||||
public static final String BUILD_DATE = "2025-07-13 19:23:17 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1752456197462L;
|
||||
public static final int DIRTY = 1;
|
||||
|
||||
private BuildConstants(){}
|
||||
|
||||
@@ -8,13 +8,13 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
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.RobotGyro;
|
||||
import frc4388.utility.compute.RobotTime;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
@@ -34,13 +34,13 @@ public class DiffDrive extends SubsystemBase implements Queryable {
|
||||
private TalonFX m_leftBackMotor;
|
||||
private TalonFX m_rightBackMotor;
|
||||
private DifferentialDrive m_driveTrain;
|
||||
private RobotGyro m_gyro;
|
||||
private Pigeon2 m_gyro;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
|
||||
TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
|
||||
TalonFX rightBackMotor, DifferentialDrive driveTrain, Pigeon2 gyro) {
|
||||
|
||||
FaultReporter.register(this);
|
||||
|
||||
@@ -56,8 +56,6 @@ public class DiffDrive extends SubsystemBase implements Queryable {
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
m_gyro.updatePigeonDeltas();
|
||||
|
||||
if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
|
||||
updateSmartDashboard();
|
||||
}
|
||||
@@ -84,9 +82,9 @@ public class DiffDrive extends SubsystemBase implements Queryable {
|
||||
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.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);
|
||||
}
|
||||
|
||||
|
||||
@@ -1,271 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.WPI_Pigeon2;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
import com.kauailabs.navx.frc.AHRS;
|
||||
|
||||
// import edu.wpi.first.wpilibj.GyroBase;
|
||||
// import edu.wpi.first.wpilibj.interfaces.Gyro;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import frc4388.utility.compute.RobotTime;
|
||||
import frc4388.utility.compute.RobotUnits;
|
||||
|
||||
/**
|
||||
* Gyro class that allows for interchangeable use between a pigeon and a navX
|
||||
*/
|
||||
public class RobotGyro {
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
|
||||
private Pigeon2 m_pigeon = null;
|
||||
private AHRS m_navX = null;
|
||||
public boolean m_isGyroAPigeon; //true if pigeon, false if navX
|
||||
|
||||
private double m_lastPigeonAngle;
|
||||
private double m_deltaPigeonAngle;
|
||||
|
||||
private double pitchZero = 0;
|
||||
private double rollZero = 0;
|
||||
|
||||
/**
|
||||
* Creates a Gyro based on a pigeon
|
||||
* @param gyro the gyroscope to use for Gyro
|
||||
*/
|
||||
public RobotGyro(Pigeon2 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;
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets yaw, pitch, and roll.
|
||||
*/
|
||||
public void resetZeroValues() {
|
||||
if (!m_isGyroAPigeon) return;
|
||||
|
||||
// pitchZero = m_pigeon.getPitch();
|
||||
// rollZero = m_pigeon.getRoll();
|
||||
}
|
||||
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
|
||||
/**
|
||||
* <p>NavX:
|
||||
* <p>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.
|
||||
*
|
||||
* <p>Pigeon:
|
||||
* <p>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.
|
||||
*/
|
||||
public void calibrate() {
|
||||
return;
|
||||
// if (m_isGyroAPigeon) {
|
||||
// m_pigeon.calibrate();
|
||||
// } else {
|
||||
// m_navX.calibrate();
|
||||
// }
|
||||
}
|
||||
|
||||
public void reset() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(0);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void reset(double val) {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(val);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetFlip() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(180);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetNinety() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(90);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetTwoSeventy() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(270);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetRightSideBlue() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(60);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetAmpSide() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(-60);
|
||||
} 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() {
|
||||
//m_pigeon.getAngle(); // This appeared to not do anything?
|
||||
var rotation = m_pigeon.getRotation3d();
|
||||
|
||||
return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())};
|
||||
}
|
||||
|
||||
public Rotation2d getRotation2d() {
|
||||
return m_pigeon.getRotation2d();
|
||||
}
|
||||
|
||||
public double getAngle() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return getPigeonAngles()[2];
|
||||
} else {
|
||||
return m_navX.getAngle();
|
||||
}
|
||||
}
|
||||
|
||||
public double getYaw() {
|
||||
return this.getAngle();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets an absolute heading of the robot
|
||||
* @return heading from -180 to 180 degrees
|
||||
*/
|
||||
public double getHeading() {
|
||||
return getHeading(getAngle());
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets an absolute heading of the robot
|
||||
* @return heading from -180 to 180 degrees
|
||||
*/
|
||||
public double getHeading(double angle) {
|
||||
return Math.IEEEremainder(angle, 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);
|
||||
}
|
||||
}
|
||||
|
||||
public double getRate() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
|
||||
} else {
|
||||
return m_navX.getRate();
|
||||
}
|
||||
}
|
||||
|
||||
public Pigeon2 getPigeon(){
|
||||
return m_pigeon;
|
||||
}
|
||||
|
||||
public AHRS getNavX(){
|
||||
return m_navX;
|
||||
}
|
||||
|
||||
public void close() throws Exception {
|
||||
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user