001/* 002 * Copyright (C) Cross The Road Electronics. All rights reserved. 003 * License information can be found in CTRE_LICENSE.txt 004 * For support and suggestions contact support@ctr-electronics.com or file 005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases 006 */ 007package com.ctre.phoenix6.hardware; 008 009import java.util.ArrayList; 010 011import com.ctre.phoenix6.BaseStatusSignal; 012import com.ctre.phoenix6.CANBus; 013import com.ctre.phoenix6.StatusSignal; 014import com.ctre.phoenix6.Utils; 015import com.ctre.phoenix6.hardware.core.CorePigeon2; 016import com.ctre.phoenix6.jni.PlatformJNI; 017import com.ctre.phoenix6.sim.DeviceType; 018import com.ctre.phoenix6.wpiutils.AutoFeedEnable; 019import com.ctre.phoenix6.wpiutils.CallbackHelper; 020import com.ctre.phoenix6.wpiutils.ReplayAutoEnable; 021 022import edu.wpi.first.hal.HAL; 023import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback; 024import edu.wpi.first.hal.HALValue; 025import edu.wpi.first.hal.SimDevice; 026import edu.wpi.first.hal.SimDevice.Direction; 027import edu.wpi.first.hal.SimDouble; 028import edu.wpi.first.hal.simulation.SimDeviceDataJNI; 029import edu.wpi.first.math.geometry.Quaternion; 030import edu.wpi.first.math.geometry.Rotation2d; 031import edu.wpi.first.math.geometry.Rotation3d; 032import edu.wpi.first.units.measure.Angle; 033import edu.wpi.first.units.measure.AngularVelocity; 034import edu.wpi.first.util.sendable.Sendable; 035import edu.wpi.first.util.sendable.SendableBuilder; 036import edu.wpi.first.util.sendable.SendableRegistry; 037import edu.wpi.first.wpilibj.RobotBase; 038import edu.wpi.first.wpilibj.simulation.CallbackStore; 039import edu.wpi.first.wpilibj.simulation.SimDeviceSim; 040 041public class Pigeon2 extends CorePigeon2 implements Sendable, AutoCloseable { 042 /** 043 * The StatusSignal getters are copies so that calls 044 * to the WPI interface do not update any references 045 */ 046 private final StatusSignal<Angle> m_yawGetter = getYaw(false).clone(); 047 private final StatusSignal<AngularVelocity> m_yawRateGetter = getAngularVelocityZWorld(false).clone(); 048 private final StatusSignal<Double> m_quatWGetter = getQuatW(false).clone(); 049 private final StatusSignal<Double> m_quatXGetter = getQuatX(false).clone(); 050 private final StatusSignal<Double> m_quatYGetter = getQuatY(false).clone(); 051 private final StatusSignal<Double> m_quatZGetter = getQuatZ(false).clone(); 052 053 private static final DeviceType kSimDeviceType = DeviceType.P6_Pigeon2Type; 054 055 private SimDevice m_simPigeon; 056 private SimDouble m_simSupplyVoltage; 057 private SimDouble m_simYaw; 058 private SimDouble m_simRawYaw; 059 private SimDouble m_simPitch; 060 private SimDouble m_simRoll; 061 private SimDouble m_simAngularVelocityX; 062 private SimDouble m_simAngularVelocityY; 063 private SimDouble m_simAngularVelocityZ; 064 065 // returned registered callbacks 066 private final ArrayList<CallbackStore> m_simValueChangedCallbacks = new ArrayList<CallbackStore>(); 067 private SimPeriodicBeforeCallback m_simPeriodicBeforeCallback = null; 068 069 /** 070 * Constructs a new Pigeon 2 sensor object. 071 * <p> 072 * Constructs the device using the default CAN bus for the system: 073 * <ul> 074 * <li>"rio" on roboRIO 075 * <li>"can0" on Linux 076 * <li>"*" on Windows 077 * </ul> 078 * 079 * @param deviceId ID of the device, as configured in Phoenix Tuner. 080 */ 081 public Pigeon2(int deviceId) { 082 this(deviceId, ""); 083 } 084 085 /** 086 * Constructs a new Pigeon 2 sensor object. 087 * 088 * @param deviceId ID of the device, as configured in Phoenix Tuner. 089 * @param canbus The CAN bus this device is on. 090 */ 091 public Pigeon2(int deviceId, CANBus canbus) { 092 this(deviceId, canbus.getName()); 093 } 094 095 /** 096 * Constructs a new Pigeon 2 sensor object. 097 * 098 * @param deviceId ID of the device, as configured in Phoenix Tuner. 099 * @param canbus Name of the CAN bus this device is on. Possible CAN bus 100 * strings are: 101 * <ul> 102 * <li>"rio" for the native roboRIO CAN bus 103 * <li>CANivore name or serial number 104 * <li>SocketCAN interface (non-FRC Linux only) 105 * <li>"*" for any CANivore seen by the program 106 * <li>empty string (default) to select the default for the 107 * system: 108 * <ul> 109 * <li>"rio" on roboRIO 110 * <li>"can0" on Linux 111 * <li>"*" on Windows 112 * </ul> 113 * </ul> 114 */ 115 public Pigeon2(int deviceId, String canbus) { 116 super(deviceId, canbus); 117 SendableRegistry.addLW(this, "Pigeon 2 (v6) ", deviceId); 118 119 if (RobotBase.isSimulation()) { 120 /* run in both swsim and hwsim */ 121 AutoFeedEnable.getInstance().start(); 122 } 123 if (Utils.isReplay()) { 124 ReplayAutoEnable.getInstance().start(); 125 } 126 127 m_simPigeon = SimDevice.create("CANGyro:Pigeon 2 (v6)", deviceId); 128 if (m_simPigeon != null) { 129 /* Simulated Pigeon2 LEDs change if it's enabled, so make sure we enable it */ 130 m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(this::onPeriodic); 131 132 m_simSupplyVoltage = m_simPigeon.createDouble("supplyVoltage", Direction.kInput, 12.0); 133 134 m_simYaw = m_simPigeon.createDouble("yaw", Direction.kOutput, 0); 135 136 m_simRawYaw = m_simPigeon.createDouble("rawYawInput", Direction.kInput, 0); 137 m_simPitch = m_simPigeon.createDouble("pitch", Direction.kInput, 0); 138 m_simRoll = m_simPigeon.createDouble("roll", Direction.kInput, 0); 139 m_simAngularVelocityX = m_simPigeon.createDouble("angularVelX", Direction.kInput, 0); 140 m_simAngularVelocityY = m_simPigeon.createDouble("angularVelY", Direction.kInput, 0); 141 m_simAngularVelocityZ = m_simPigeon.createDouble("angularVelZ", Direction.kInput, 0); 142 143 final SimDeviceSim sim = new SimDeviceSim("CANGyro:Pigeon 2 (v6)"); 144 m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simSupplyVoltage, this::onValueChanged, true)); 145 m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simRawYaw, this::onValueChanged, true)); 146 m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simPitch, this::onValueChanged, true)); 147 m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simRoll, this::onValueChanged, true)); 148 m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simAngularVelocityX, this::onValueChanged, true)); 149 m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simAngularVelocityY, this::onValueChanged, true)); 150 m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simAngularVelocityZ, this::onValueChanged, true)); 151 } 152 } 153 154 // ----- Auto-Closable, from Gyro ----- // 155 @Override 156 public void close() { 157 SendableRegistry.remove(this); 158 if (m_simPeriodicBeforeCallback != null) { 159 m_simPeriodicBeforeCallback.close(); 160 m_simPeriodicBeforeCallback = null; 161 } 162 if (m_simPigeon != null) { 163 m_simPigeon.close(); 164 m_simPigeon = null; 165 } 166 167 for (var callback : m_simValueChangedCallbacks) { 168 callback.close(); 169 } 170 m_simValueChangedCallbacks.clear(); 171 172 AutoFeedEnable.getInstance().stop(); 173 ReplayAutoEnable.getInstance().stop(); 174 } 175 176 // ----- Callbacks for Sim ----- // 177 private void onValueChanged(String name, int handle, int direction, HALValue value) { 178 String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle)); 179 String physType = deviceName + ":" + name; 180 PlatformJNI.JNI_SimSetPhysicsInput( 181 kSimDeviceType.value, getDeviceID(), 182 physType, CallbackHelper.getRawValue(value) 183 ); 184 } 185 186 private void onPeriodic() { 187 double value = 0; 188 int err = 0; 189 190 final int deviceID = getDeviceID(); 191 192 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage"); 193 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 194 if (err == 0) { 195 m_simSupplyVoltage.set(value); 196 } 197 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Yaw"); 198 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 199 if (err == 0) { 200 m_simYaw.set(value); 201 } 202 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawYaw"); 203 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 204 if (err == 0) { 205 m_simRawYaw.set(value); 206 } 207 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Pitch"); 208 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 209 if (err == 0) { 210 m_simPitch.set(value); 211 } 212 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Roll"); 213 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 214 if (err == 0) { 215 m_simRoll.set(value); 216 } 217 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "AngularVelocityX"); 218 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 219 if (err == 0) { 220 m_simAngularVelocityX.set(value); 221 } 222 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "AngularVelocityY"); 223 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 224 if (err == 0) { 225 m_simAngularVelocityY.set(value); 226 } 227 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "AngularVelocityZ"); 228 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 229 if (err == 0) { 230 m_simAngularVelocityZ.set(value); 231 } 232 } 233 234 // ----- WPILib Gyro Interface ----- // 235 //WPILib no longer has a Gyro interface, but these methods are standard in FRC. 236 237 /** 238 * Resets the Pigeon 2 to a heading of zero. 239 * <p> 240 * This can be used if there is significant drift in the gyro, 241 * and it needs to be recalibrated after it has been running. 242 */ 243 public void reset() { 244 setYaw(0); 245 } 246 247 /** 248 * Returns the heading of the robot in degrees. 249 * <p> 250 * The angle increases as the Pigeon 2 turns clockwise when looked 251 * at from the top. This follows the NED axis convention. 252 * <p> 253 * The angle is continuous; that is, it will continue from 360 to 254 * 361 degrees. This allows for algorithms that wouldn't want to 255 * see a discontinuity in the gyro output as it sweeps past from 256 * 360 to 0 on the second time around. 257 * 258 * @deprecated This API is deprecated for removal in the 2026 season. 259 * Users should use {@link #getYaw} instead. Note that Yaw is CCW+, 260 * whereas this API is CW+. 261 * 262 * @return The current heading of the robot in degrees 263 */ 264 @Deprecated(forRemoval = true) 265 public double getAngle() { 266 // Negate since getAngle requires cw+ and Pigeon is ccw+ 267 return -m_yawGetter.refresh().getValueAsDouble(); 268 } 269 270 /** 271 * Returns the rate of rotation of the Pigeon 2. 272 * <p> 273 * The rate is positive as the Pigeon 2 turns clockwise when looked 274 * at from the top. 275 * 276 * @deprecated This API is deprecated for removal in the 2026 season. 277 * Users should use {@link #getAngularVelocityZWorld} instead. Note 278 * that AngularVelocityZWorld is CCW+, whereas this API is CW+. 279 * 280 * @return The current rate in degrees per second 281 */ 282 @Deprecated(forRemoval = true) 283 public double getRate() { 284 // Negate since getAngle requires cw+ and Pigeon is ccw+ 285 return -m_yawRateGetter.refresh().getValueAsDouble(); 286 } 287 288 /** 289 * Returns the heading of the robot as a {@link Rotation2d}. 290 * <p> 291 * The angle increases as the Pigeon 2 turns counterclockwise when 292 * looked at from the top. This follows the NWU axis convention. 293 * <p> 294 * The angle is continuous; that is, it will continue from 360 to 295 * 361 degrees. This allows for algorithms that wouldn't want to 296 * see a discontinuity in the gyro output as it sweeps past from 297 * 360 to 0 on the second time around. 298 * 299 * @return The current heading of the robot as a {@link Rotation2d} 300 */ 301 public Rotation2d getRotation2d() { 302 // Rotation2d and Pigeon are both ccw+ 303 return Rotation2d.fromDegrees(m_yawGetter.refresh().getValueAsDouble()); 304 } 305 306 /** 307 * Returns the orientation of the robot as a {@link Rotation3d} 308 * created from the quaternion signals. 309 * 310 * @return The current orientation of the robot as a {@link Rotation3d} 311 */ 312 public Rotation3d getRotation3d() { 313 BaseStatusSignal.refreshAll(m_quatWGetter, m_quatXGetter, m_quatYGetter, m_quatZGetter); 314 return new Rotation3d(new Quaternion(m_quatWGetter.getValue(), m_quatXGetter.getValue(), m_quatYGetter.getValue(), m_quatZGetter.getValue())); 315 } 316 317 // ----- Sendable ----- // 318 @Override 319 public void initSendable(SendableBuilder builder) { 320 builder.setSmartDashboardType("Gyro"); 321 builder.addDoubleProperty("Value", () -> m_yawGetter.refresh().getValueAsDouble(), this::setYaw); 322 } 323 324}