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.phoenixpro.hardware; 008 009import java.util.ArrayList; 010 011import com.ctre.phoenixpro.StatusSignalValue; 012import com.ctre.phoenixpro.hardware.core.CorePigeon2; 013import com.ctre.phoenixpro.jni.PlatformJNI; 014import com.ctre.phoenixpro.sim.DeviceType; 015import com.ctre.phoenixpro.wpiutils.AutoFeedEnable; 016import com.ctre.phoenixpro.wpiutils.CallbackHelper; 017 018import edu.wpi.first.hal.HAL; 019import edu.wpi.first.hal.HALValue; 020import edu.wpi.first.hal.SimDevice; 021import edu.wpi.first.hal.SimDouble; 022import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback; 023import edu.wpi.first.hal.SimDevice.Direction; 024import edu.wpi.first.hal.simulation.SimDeviceDataJNI; 025import edu.wpi.first.hal.simulation.SimValueCallback; 026import edu.wpi.first.math.geometry.Rotation2d; 027import edu.wpi.first.util.sendable.Sendable; 028import edu.wpi.first.util.sendable.SendableBuilder; 029import edu.wpi.first.util.sendable.SendableRegistry; 030import edu.wpi.first.wpilibj.interfaces.Gyro; 031import edu.wpi.first.wpilibj.simulation.CallbackStore; 032import edu.wpi.first.wpilibj.simulation.SimDeviceSim; 033 034public class Pigeon2 extends CorePigeon2 implements Sendable, Gyro { 035 /** 036 * The StatusSignalValue getters are copies so that calls 037 * to the WPI interface do not update any references 038 */ 039 private final StatusSignalValue<Double> m_yawGetter = getYaw().clone(); 040 private final StatusSignalValue<Double> m_yawRateGetter = getAngularVelocityZ().clone(); 041 042 private static final DeviceType kSimDeviceType = DeviceType.PRO_Pigeon2Type; 043 044 private SimDevice m_simPigeon; 045 private SimDouble m_simSupplyVoltage; 046 private SimDouble m_simYaw; 047 private SimDouble m_simRawYaw; 048 private SimDouble m_simPitch; 049 private SimDouble m_simRoll; 050 051 // callbacks to register 052 private final SimValueCallback m_onValueChangedCallback = new OnValueChangedCallback(); 053 private final Runnable m_onPeriodicCallback = new OnPeriodicCallback(); 054 055 // returned registered callbacks 056 private final ArrayList<CallbackStore> m_simValueChangedCallbacks = new ArrayList<CallbackStore>(); 057 private SimPeriodicBeforeCallback m_simPeriodicBeforeCallback = null; 058 059 /** 060 * Constructs a new Pigeon 2 sensor object. 061 * <p> 062 * Constructs the device using the default CAN bus for the system: 063 * <ul> 064 * <li>"rio" on roboRIO 065 * <li>"can0" on Linux 066 * <li>"*" on Windows 067 * </ul> 068 * 069 * @param deviceId ID of the device, as configured in Phoenix Tuner. 070 */ 071 public Pigeon2(int deviceId) { 072 this(deviceId, ""); 073 } 074 075 /** 076 * Constructs a new Pigeon 2 sensor object. 077 * 078 * @param deviceId ID of the device, as configured in Phoenix Tuner. 079 * @param canbus Name of the CAN bus this device is on. Possible CAN bus 080 * strings are: 081 * <ul> 082 * <li>"rio" for the native roboRIO CAN bus 083 * <li>CANivore name or serial number 084 * <li>SocketCAN interface (non-FRC Linux only) 085 * <li>"*" for any CANivore seen by the program 086 * <li>empty string (default) to select the default for the 087 * system: 088 * <ul> 089 * <li>"rio" on roboRIO 090 * <li>"can0" on Linux 091 * <li>"*" on Windows 092 * </ul> 093 * </ul> 094 */ 095 public Pigeon2(int deviceId, String canbus) { 096 super(deviceId, canbus); 097 098 SendableRegistry.addLW(this, "Pigeon 2 (Pro) ", deviceId); 099 m_simPigeon = SimDevice.create("CANGyro:Pigeon 2 (Pro)", getDeviceID()); 100 if (m_simPigeon != null) { 101 /* Simulated Pigeon2 LEDs change if it's enabled, so make sure we enable it */ 102 AutoFeedEnable.getInstance(); 103 m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(m_onPeriodicCallback); 104 105 m_simSupplyVoltage = m_simPigeon.createDouble("supplyVoltage", Direction.kInput, 12.0); 106 107 m_simYaw = m_simPigeon.createDouble("yaw", Direction.kOutput, 0); 108 109 m_simRawYaw = m_simPigeon.createDouble("rawYawInput", Direction.kInput, 0); 110 m_simPitch = m_simPigeon.createDouble("pitch", Direction.kInput, 0); 111 m_simRoll = m_simPigeon.createDouble("roll", Direction.kInput, 0); 112 113 final SimDeviceSim sim = new SimDeviceSim("CANGyro:Pigeon 2 (Pro)"); 114 m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simSupplyVoltage, m_onValueChangedCallback, true)); 115 m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simRawYaw, m_onValueChangedCallback, true)); 116 m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simPitch, m_onValueChangedCallback, true)); 117 m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simRoll, m_onValueChangedCallback, true)); 118 } 119 } 120 121 // ----- Auto-Closable, from Gyro ----- // 122 @Override 123 public void close() { 124 SendableRegistry.remove(this); 125 if (m_simPeriodicBeforeCallback != null) { 126 m_simPeriodicBeforeCallback.close(); 127 m_simPeriodicBeforeCallback = null; 128 } 129 if (m_simPigeon != null) { 130 m_simPigeon.close(); 131 m_simPigeon = null; 132 } 133 134 for (var callback : m_simValueChangedCallbacks) { 135 callback.close(); 136 } 137 m_simValueChangedCallbacks.clear(); 138 } 139 140 // ----- Callbacks for Sim ----- // 141 private class OnValueChangedCallback implements SimValueCallback { 142 @Override 143 public void callback(String name, int handle, int direction, HALValue value) { 144 String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle)); 145 String physType = deviceName + ":" + name; 146 PlatformJNI.JNI_SimSetPhysicsInput(kSimDeviceType.value, getDeviceID(), 147 physType, CallbackHelper.getRawValue(value)); 148 } 149 } 150 151 private class OnPeriodicCallback implements Runnable { 152 @Override 153 public void run() { 154 double value = 0; 155 int err = 0; 156 157 final int deviceID = getDeviceID(); 158 159 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage"); 160 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 161 if (err == 0) { 162 m_simSupplyVoltage.set(value); 163 } 164 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Yaw"); 165 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 166 if (err == 0) { 167 m_simYaw.set(value); 168 } 169 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawYaw"); 170 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 171 if (err == 0) { 172 m_simRawYaw.set(value); 173 } 174 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Pitch"); 175 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 176 if (err == 0) { 177 m_simPitch.set(value); 178 } 179 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Roll"); 180 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 181 if (err == 0) { 182 m_simRoll.set(value); 183 } 184 } 185 } 186 187 // ----- WPILib Gyro Interface ----- // 188 189 /** 190 * This function does nothing; it exists 191 * to satisfy the WPILib Gyro interface. 192 * <p> 193 * Pigeon 2 does not require manual calibration. 194 */ 195 @Override 196 public void calibrate() {} 197 198 @Override 199 public void reset() { 200 setYaw(0); 201 } 202 203 @Override 204 public double getAngle() { 205 // Negate since getAngle requires cw+ and Pigeon is ccw+ 206 return -m_yawGetter.refresh().getValue(); 207 } 208 209 @Override 210 public double getRate() { 211 // Negate since getAngle requires cw+ and Pigeon is ccw+ 212 return -m_yawRateGetter.refresh().getValue(); 213 } 214 215 @Override 216 public Rotation2d getRotation2d() { 217 // Rotation2d and Pigeon are both ccw+ 218 return Rotation2d.fromDegrees(m_yawGetter.refresh().getValue()); 219 } 220 221 // ----- Sendable ----- // 222 @Override 223 public void initSendable(SendableBuilder builder) { 224 builder.setSmartDashboardType("Gyro"); 225 builder.addDoubleProperty("Value", m_yawGetter.asSupplier()::get, this::setYaw); 226 } 227 228}