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