-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
7 changed files
with
210 additions
and
33 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,81 @@ | ||
| /* | ||
| * This is from FRC 2910, 2023 season with some | ||
| * updates for this year's version of the CTRE Phoenix library. | ||
| */ | ||
|
|
||
| package frc.robot.experimental; | ||
|
|
||
| import com.ctre.phoenix6.BaseStatusSignal; | ||
| import com.ctre.phoenix6.StatusSignal; | ||
| import com.ctre.phoenix6.configs.GyroTrimConfigs; | ||
| import com.ctre.phoenix6.configs.MountPoseConfigs; | ||
| import com.ctre.phoenix6.configs.Pigeon2Configuration; | ||
| import com.ctre.phoenix6.hardware.Pigeon2; | ||
| import edu.wpi.first.math.util.Units; | ||
| import frc.robot.util.Constants; | ||
|
|
||
|
|
||
| @SuppressWarnings("unused") | ||
|
|
||
| /** | ||
| * Implementation of the gyroscope class. Uses the Pigeon2 gyroscope to receive data about the robots yaw, pitch, and roll values. | ||
| */ | ||
| public class GyroIOPigeon2 implements GyroIO { | ||
|
|
||
| /** | ||
| * Pigeon2 gyroscope object. Receives direct data from the gyroscope about the yaw, pitch, and roll. | ||
| */ | ||
| private final Pigeon2 gyro; | ||
|
|
||
| /** | ||
| * Pigeon2 gyroIO implementation. | ||
| * | ||
| * @param canId Is the unique identifier to the gyroscope used on the robot. | ||
| * @param canBus The name of the CAN bus the device is connected to. | ||
| */ | ||
| private StatusSignal<Double> yawSignal; | ||
|
|
||
| private StatusSignal<Double> angularVelocitySignal; | ||
|
|
||
| private BaseStatusSignal[] signals; | ||
|
|
||
| public GyroIOPigeon2(int mountPose, double error, String canBus) { | ||
|
|
||
| gyro = new Pigeon2(mountPose, canBus); | ||
|
|
||
| Pigeon2Configuration config = new Pigeon2Configuration(); | ||
| MountPoseConfigs mountPoseConfigs = new MountPoseConfigs(); | ||
| mountPoseConfigs.MountPoseYaw = mountPose; | ||
| GyroTrimConfigs gyroTrimConfigs = new GyroTrimConfigs(); | ||
| gyroTrimConfigs.GyroScalarZ = error; | ||
| config.MountPose = mountPoseConfigs; | ||
| config.GyroTrim = gyroTrimConfigs; | ||
| gyro.getConfigurator().apply(config); | ||
|
|
||
| yawSignal = gyro.getYaw(); | ||
| // angularVelocitySignal = gyro.getAngularVelocityZ(); | ||
| angularVelocitySignal = gyro.getAngularVelocityZDevice(); // Not sure if this is the correct method to use or ZWorld | ||
|
|
||
| signals = new BaseStatusSignal[2]; | ||
| signals[0] = yawSignal; | ||
| signals[1] = angularVelocitySignal; | ||
| } | ||
|
|
||
|
|
||
| /* | ||
| @Override | ||
| public void updateInputs(GyroIOInputs inputs) { | ||
| inputs.connected = true; | ||
| inputs.yaw = Units.degreesToRadians( | ||
| BaseStatusSignalValue.getLatencyCompensatedValue(yawSignal, angularVelocitySignal)); | ||
| inputs.pitch = Units.degreesToRadians(gyro.getPitch().getValue()); | ||
| inputs.roll = Units.degreesToRadians(gyro.getRoll().getValue()); | ||
| inputs.angularVelocity = Units.degreesToRadians(angularVelocitySignal.getValue()); | ||
| } | ||
| */ | ||
| @Override | ||
| public BaseStatusSignal[] getSignals() { | ||
| return signals; | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,99 @@ | ||
| package frc.robot.subsystems; | ||
|
|
||
| import com.ctre.phoenix6.configs.MountPoseConfigs; | ||
| import com.ctre.phoenix6.configs.Pigeon2Configuration; | ||
| import com.ctre.phoenix6.hardware.Pigeon2; | ||
|
|
||
| /* | ||
| Pigeon2 treats +X as the forward axis | ||
| +Y as the left axis | ||
| +Z towards the sky | ||
| This is a common way to define the world frame reference for groundbased vehicles | ||
| */ | ||
|
|
||
| public class Gyro { | ||
|
|
||
| private final Pigeon2 pidgey; | ||
|
|
||
| public Gyro() { | ||
| pidgey = new Pigeon2(14); // ID from Change the ID as needed | ||
|
|
||
| // StatusSignal<Double> yawSignal; | ||
|
|
||
| /* User can change the configs if they want, or leave it empty for factory-default */ | ||
| // Mimicing ArmSubsystem.java | ||
| pidgey.getConfigurator().apply(new Pigeon2Configuration()); | ||
|
|
||
| var pidgeyConfigs = new Pigeon2Configuration(); | ||
| /* Speed up signals to an appropriate rate */ | ||
| // pidgey.getYaw().setUpdateFrequency(100); | ||
| // pidgey.getGravityVectorZ().setUpdateFrequency(100); | ||
| // pidgey.getAccelerationX().setUpdateFrequency(100); | ||
| var pidgeyMountConfigs = new MountPoseConfigs(); | ||
| // pidgeyMountConfigs.MountPose = MountPoseConfigs.MountedPose.TwoDegreesPitch; | ||
|
|
||
| pidgey.getConfigurator().apply(pidgeyConfigs); | ||
| pidgey.getConfigurator().apply(pidgeyMountConfigs); | ||
| } | ||
|
|
||
| /* Not sure what difference is between these three | ||
| * Values will be posted to the Dashboard | ||
| */ | ||
| public double getYaw() { | ||
| return pidgey.getYaw().getValue(); | ||
| } | ||
|
|
||
| public double getAngle() { | ||
| return pidgey.getAngle(); | ||
| } | ||
|
|
||
| public double Rotation2d() { | ||
| return pidgey.getRotation2d().getDegrees(); | ||
| } | ||
|
|
||
| /* | ||
| * The Pigeon2 class has a default reset method `reset()` | ||
| * So can it be called directly without this method? | ||
| * Maybe, but not 100% sure | ||
| */ | ||
|
|
||
| public void resetGyro() { | ||
| pidgey.reset(); | ||
| } | ||
|
|
||
|
|
||
|
|
||
| } // end of class Gyro | ||
|
|
||
|
|
||
| /* | ||
| * Mount Calibration | ||
| * It’s recommended to perform a mount calibration when placement of the Pigeon 2.0 has been finalized. | ||
| * This can be done via the Calibration page in Tuner X. | ||
| */ | ||
|
|
||
| /* Documentation | ||
| https://api.ctr-electronics.com/phoenix6/release/java/com/ctre/phoenix6/hardware/Pigeon2.html | ||
| Iterative Example | ||
| https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/main/java/Pigeon2/src/main/java/frc/robot/Robot.java | ||
| * Method Summary | ||
| Modifier & Type | Method | Description | ||
| ----------------|--------------- |------------------- | ||
| void | close() | | ||
| double | getAngle() | Returns the heading of the robot in degrees. | ||
| double | getRate() | Returns the rate of rotation of the Pigeon 2. | ||
| Rotation2d | getRotation2d() | Returns the heading of the robot as a Rotation2d. | ||
| Rotation3d | getRotation3d() | Returns the orientation of the robot as a Rotation3d created from the quaternion signals. | ||
| void | initSendable(SendableBuilder builder) | | ||
| void | reset() | Resets the Pigeon 2 to a heading of zero. | ||
| */ | ||
|
|
||
| /* | ||
| * public double getAngle() | ||
| Returns the heading of the robot in degrees. | ||
| The angle increases as the Pigeon 2 turns clockwise when looked at from the top. This follows the NED axis convention. | ||
| The angle is continuous; that is, it will continue from 360 to 361 degrees. This allows for algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from 360 to 0 on the second time around. | ||
| Returns: The current heading of the robot in degrees | ||
| */ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters