mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Did a few things
This commit is contained in:
66945
@@ -1,245 +1,245 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.utility;
|
||||
|
||||
import com.ctre.phoenix.sensors.PigeonIMU;
|
||||
import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
|
||||
import com.kauailabs.navx.frc.AHRS;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.util.sendable.Sendable;
|
||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
import edu.wpi.first.wpilibj.PIDSource;
|
||||
import edu.wpi.first.wpilibj.PIDSourceType;
|
||||
import edu.wpi.first.wpilibj.interfaces.Gyro;
|
||||
|
||||
/**
|
||||
* Gyro class that allows for interchangeable use between a pigeon and a navX
|
||||
*/
|
||||
public class RobotGyro implements Gyro, PIDSource, Sendable {
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
|
||||
private PigeonIMU 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;
|
||||
|
||||
/**
|
||||
* Creates a Gyro based on a pigeon
|
||||
*
|
||||
* @param gyro the gyroscope to use for Gyro
|
||||
*/
|
||||
public RobotGyro(PigeonIMU 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;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
@Override
|
||||
public void calibrate() {
|
||||
if (m_isGyroAPigeon)
|
||||
m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
|
||||
else
|
||||
m_navX.calibrate();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void reset() {
|
||||
if (m_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.
|
||||
*/
|
||||
private double[] getPigeonAngles() {
|
||||
double[] angles = new double[3];
|
||||
m_pigeon.getYawPitchRoll(angles);
|
||||
return angles;
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getAngle() {
|
||||
if (m_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 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);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getRate() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
|
||||
} else {
|
||||
return m_navX.getRate();
|
||||
}
|
||||
}
|
||||
|
||||
public PigeonIMU getPigeon() {
|
||||
return m_pigeon;
|
||||
}
|
||||
|
||||
public AHRS getNavX() {
|
||||
return m_navX;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void close() throws Exception {
|
||||
|
||||
}
|
||||
|
||||
// Begin old GyroBase class
|
||||
private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
|
||||
|
||||
/**
|
||||
* Set which parameter of the gyro you are using as a process control variable.
|
||||
* The Gyro class
|
||||
* supports the rate and displacement parameters
|
||||
*
|
||||
* @param pidSource An enum to select the parameter.
|
||||
*/
|
||||
@Override
|
||||
public void setPIDSourceType(PIDSourceType pidSource) {
|
||||
m_pidSource = pidSource;
|
||||
}
|
||||
|
||||
@Override
|
||||
public PIDSourceType getPIDSourceType() {
|
||||
return m_pidSource;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the output of the gyro for use with PIDControllers. May be the angle or
|
||||
* rate depending on
|
||||
* the set PIDSourceType
|
||||
*
|
||||
* @return the output according to the gyro
|
||||
*/
|
||||
@Override
|
||||
public double pidGet() {
|
||||
switch (m_pidSource) {
|
||||
case kRate:
|
||||
return getRate();
|
||||
case kDisplacement:
|
||||
return getAngle();
|
||||
default:
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.setSmartDashboardType("Gyro");
|
||||
builder.addDoubleProperty("Value", this::getAngle, null);
|
||||
}
|
||||
}
|
||||
//// Copyright (c) FIRST and other WPILib contributors.
|
||||
//// Open Source Software; you can modify and/or share it under the terms of
|
||||
//// the WPILib BSD license file in the root directory of this project.
|
||||
//
|
||||
//package frc4388.utility;
|
||||
//
|
||||
//import com.ctre.phoenix.sensors.PigeonIMU;
|
||||
//import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
|
||||
//import com.kauailabs.navx.frc.AHRS;
|
||||
//
|
||||
//import edu.wpi.first.math.MathUtil;
|
||||
//import edu.wpi.first.util.sendable.Sendable;
|
||||
//import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
//import edu.wpi.first.wpilibj.PIDSource;
|
||||
//import edu.wpi.first.wpilibj.PIDSourceType;
|
||||
//import edu.wpi.first.wpilibj.interfaces.Gyro;
|
||||
//
|
||||
///**
|
||||
// * Gyro class that allows for interchangeable use between a pigeon and a navX
|
||||
// */
|
||||
//public class RobotGyro implements Gyro, PIDSource, Sendable {
|
||||
// private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
//
|
||||
// private PigeonIMU 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;
|
||||
//
|
||||
// /**
|
||||
// * Creates a Gyro based on a pigeon
|
||||
// *
|
||||
// * @param gyro the gyroscope to use for Gyro
|
||||
// */
|
||||
// public RobotGyro(PigeonIMU 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;
|
||||
// }
|
||||
//
|
||||
// /**
|
||||
// * 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.
|
||||
// */
|
||||
// @Override
|
||||
// public void calibrate() {
|
||||
// if (m_isGyroAPigeon)
|
||||
// m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
|
||||
// else
|
||||
// m_navX.calibrate();
|
||||
// }
|
||||
//
|
||||
// @Override
|
||||
// public void reset() {
|
||||
// if (m_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.
|
||||
// */
|
||||
// private double[] getPigeonAngles() {
|
||||
// double[] angles = new double[3];
|
||||
// m_pigeon.getYawPitchRoll(angles);
|
||||
// return angles;
|
||||
// }
|
||||
//
|
||||
// @Override
|
||||
// public double getAngle() {
|
||||
// if (m_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 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);
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// @Override
|
||||
// public double getRate() {
|
||||
// if (m_isGyroAPigeon) {
|
||||
// return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
|
||||
// } else {
|
||||
// return m_navX.getRate();
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// public PigeonIMU getPigeon() {
|
||||
// return m_pigeon;
|
||||
// }
|
||||
//
|
||||
// public AHRS getNavX() {
|
||||
// return m_navX;
|
||||
// }
|
||||
//
|
||||
// @Override
|
||||
// public void close() throws Exception {
|
||||
//
|
||||
// }
|
||||
//
|
||||
// // Begin old GyroBase class
|
||||
// private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
|
||||
//
|
||||
// /**
|
||||
// * Set which parameter of the gyro you are using as a process control variable.
|
||||
// * The Gyro class
|
||||
// * supports the rate and displacement parameters
|
||||
// *
|
||||
// * @param pidSource An enum to select the parameter.
|
||||
// */
|
||||
// @Override
|
||||
// public void setPIDSourceType(PIDSourceType pidSource) {
|
||||
// m_pidSource = pidSource;
|
||||
// }
|
||||
//
|
||||
// @Override
|
||||
// public PIDSourceType getPIDSourceType() {
|
||||
// return m_pidSource;
|
||||
// }
|
||||
//
|
||||
// /**
|
||||
// * Get the output of the gyro for use with PIDControllers. May be the angle or
|
||||
// * rate depending on
|
||||
// * the set PIDSourceType
|
||||
// *
|
||||
// * @return the output according to the gyro
|
||||
// */
|
||||
// @Override
|
||||
// public double pidGet() {
|
||||
// switch (m_pidSource) {
|
||||
// case kRate:
|
||||
// return getRate();
|
||||
// case kDisplacement:
|
||||
// return getAngle();
|
||||
// default:
|
||||
// return 0.0;
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// @Override
|
||||
// public void initSendable(SendableBuilder builder) {
|
||||
// builder.setSmartDashboardType("Gyro");
|
||||
// builder.addDoubleProperty("Value", this::getAngle, null);
|
||||
// }
|
||||
//}
|
||||
|
||||
Reference in New Issue
Block a user