Source code

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.StatusSignal;
013import com.ctre.phoenix6.hardware.core.CorePigeon2;
014import com.ctre.phoenix6.jni.PlatformJNI;
015import com.ctre.phoenix6.sim.DeviceType;
016import com.ctre.phoenix6.wpiutils.AutoFeedEnable;
017import com.ctre.phoenix6.wpiutils.CallbackHelper;
018
019import edu.wpi.first.hal.HAL;
020import edu.wpi.first.hal.HALValue;
021import edu.wpi.first.hal.SimDevice;
022import edu.wpi.first.hal.SimDouble;
023import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback;
024import edu.wpi.first.hal.SimDevice.Direction;
025import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
026import edu.wpi.first.hal.simulation.SimValueCallback;
027import edu.wpi.first.math.geometry.Quaternion;
028import edu.wpi.first.math.geometry.Rotation2d;
029import edu.wpi.first.math.geometry.Rotation3d;
030import edu.wpi.first.util.sendable.Sendable;
031import edu.wpi.first.util.sendable.SendableBuilder;
032import edu.wpi.first.util.sendable.SendableRegistry;
033import edu.wpi.first.wpilibj.simulation.CallbackStore;
034import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
035
036public class Pigeon2 extends CorePigeon2 implements Sendable, AutoCloseable {
037    /**
038     * The StatusSignal getters are copies so that calls
039     * to the WPI interface do not update any references
040     */
041    private final StatusSignal<Double> m_yawGetter = getYaw().clone();
042    private final StatusSignal<Double> m_yawRateGetter = getAngularVelocityZWorld().clone();
043    private final StatusSignal<Double> m_quatWGetter = getQuatW().clone();
044    private final StatusSignal<Double> m_quatXGetter = getQuatX().clone();
045    private final StatusSignal<Double> m_quatYGetter = getQuatY().clone();
046    private final StatusSignal<Double> m_quatZGetter = getQuatZ().clone();
047
048    private static final DeviceType kSimDeviceType = DeviceType.PRO_Pigeon2Type;
049
050    private SimDevice m_simPigeon;
051    private SimDouble m_simSupplyVoltage;
052    private SimDouble m_simYaw;
053    private SimDouble m_simRawYaw;
054    private SimDouble m_simPitch;
055    private SimDouble m_simRoll;
056
057    // callbacks to register
058    private final SimValueCallback m_onValueChangedCallback = new OnValueChangedCallback();
059    private final Runnable m_onPeriodicCallback = new OnPeriodicCallback();
060
061    // returned registered callbacks
062    private final ArrayList<CallbackStore> m_simValueChangedCallbacks = new ArrayList<CallbackStore>();
063    private SimPeriodicBeforeCallback m_simPeriodicBeforeCallback = null;
064
065    /**
066     * Constructs a new Pigeon 2 sensor object.
067     * <p>
068     * Constructs the device using the default CAN bus for the system:
069     * <ul>
070     *   <li>"rio" on roboRIO
071     *   <li>"can0" on Linux
072     *   <li>"*" on Windows
073     * </ul>
074     *
075     * @param deviceId ID of the device, as configured in Phoenix Tuner.
076     */
077    public Pigeon2(int deviceId) {
078        this(deviceId, "");
079    }
080
081    /**
082     * Constructs a new Pigeon 2 sensor object.
083     *
084     * @param deviceId ID of the device, as configured in Phoenix Tuner.
085     * @param canbus   Name of the CAN bus this device is on. Possible CAN bus
086     *                 strings are:
087     *                 <ul>
088     *                   <li>"rio" for the native roboRIO CAN bus
089     *                   <li>CANivore name or serial number
090     *                   <li>SocketCAN interface (non-FRC Linux only)
091     *                   <li>"*" for any CANivore seen by the program
092     *                   <li>empty string (default) to select the default for the
093     *                       system:
094     *                   <ul>
095     *                     <li>"rio" on roboRIO
096     *                     <li>"can0" on Linux
097     *                     <li>"*" on Windows
098     *                   </ul>
099     *                 </ul>
100     */
101    public Pigeon2(int deviceId, String canbus) {
102        super(deviceId, canbus);
103
104        SendableRegistry.addLW(this, "Pigeon 2 (v6) ", deviceId);
105        m_simPigeon = SimDevice.create("CANGyro:Pigeon 2 (v6)", getDeviceID());
106        if (m_simPigeon != null) {
107            /* Simulated Pigeon2 LEDs change if it's enabled, so make sure we enable it */
108            AutoFeedEnable.getInstance().start();
109            m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(m_onPeriodicCallback);
110
111            m_simSupplyVoltage = m_simPigeon.createDouble("supplyVoltage", Direction.kInput, 12.0);
112
113            m_simYaw = m_simPigeon.createDouble("yaw", Direction.kOutput, 0);
114
115            m_simRawYaw = m_simPigeon.createDouble("rawYawInput", Direction.kInput, 0);
116            m_simPitch = m_simPigeon.createDouble("pitch", Direction.kInput, 0);
117            m_simRoll = m_simPigeon.createDouble("roll", Direction.kInput, 0);
118
119            final SimDeviceSim sim = new SimDeviceSim("CANGyro:Pigeon 2 (v6)");
120            m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simSupplyVoltage, m_onValueChangedCallback, true));
121            m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simRawYaw, m_onValueChangedCallback, true));
122            m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simPitch, m_onValueChangedCallback, true));
123            m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simRoll, m_onValueChangedCallback, true));
124        }
125    }
126
127    // ----- Auto-Closable, from Gyro ----- //
128    @Override
129    public void close() {
130        SendableRegistry.remove(this);
131        if (m_simPeriodicBeforeCallback != null) {
132            m_simPeriodicBeforeCallback.close();
133            m_simPeriodicBeforeCallback = null;
134        }
135        if (m_simPigeon != null) {
136            m_simPigeon.close();
137            m_simPigeon = null;
138        }
139
140        for (var callback : m_simValueChangedCallbacks) {
141            callback.close();
142        }
143        m_simValueChangedCallbacks.clear();
144
145        AutoFeedEnable.getInstance().stop();
146    }
147
148    // ----- Callbacks for Sim ----- //
149    private class OnValueChangedCallback implements SimValueCallback {
150        @Override
151        public void callback(String name, int handle, int direction, HALValue value) {
152            String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle));
153            String physType = deviceName + ":" + name;
154            PlatformJNI.JNI_SimSetPhysicsInput(kSimDeviceType.value, getDeviceID(),
155                    physType, CallbackHelper.getRawValue(value));
156        }
157    }
158
159    private class OnPeriodicCallback implements Runnable {
160        @Override
161        public void run() {
162            double value = 0;
163            int err = 0;
164
165            final int deviceID = getDeviceID();
166
167            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage");
168            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
169            if (err == 0) {
170                m_simSupplyVoltage.set(value);
171            }
172            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Yaw");
173            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
174            if (err == 0) {
175                m_simYaw.set(value);
176            }
177            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawYaw");
178            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
179            if (err == 0) {
180                m_simRawYaw.set(value);
181            }
182            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Pitch");
183            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
184            if (err == 0) {
185                m_simPitch.set(value);
186            }
187            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Roll");
188            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
189            if (err == 0) {
190                m_simRoll.set(value);
191            }
192        }
193    }
194
195    // ----- WPILib Gyro Interface ----- //
196    //WPILib no longer has a Gyro interface, but these methods are standard in FRC.
197
198    /**
199     * Resets the Pigeon 2 to a heading of zero.
200     * <p>
201     * This can be used if there is significant drift in the gyro,
202     * and it needs to be recalibrated after it has been running.
203     */
204    public void reset() {
205        setYaw(0);
206    }
207
208    /**
209     * Returns the heading of the robot in degrees.
210     * <p>
211     * The angle increases as the Pigeon 2 turns clockwise when looked
212     * at from the top. This follows the NED axis convention.
213     * <p>
214     * The angle is continuous; that is, it will continue from 360 to
215     * 361 degrees. This allows for algorithms that wouldn't want to
216     * see a discontinuity in the gyro output as it sweeps past from
217     * 360 to 0 on the second time around.
218     *
219     * @return The current heading of the robot in degrees
220     */
221    public double getAngle() {
222        // Negate since getAngle requires cw+ and Pigeon is ccw+
223        return -m_yawGetter.refresh().getValue();
224    }
225
226    /**
227     * Returns the rate of rotation of the Pigeon 2.
228     * <p>
229     * The rate is positive as the Pigeon 2 turns clockwise when looked
230     * at from the top.
231     *
232     * @return The current rate in degrees per second
233     */
234    public double getRate() {
235        // Negate since getAngle requires cw+ and Pigeon is ccw+
236        return -m_yawRateGetter.refresh().getValue();
237    }
238
239    /**
240     * Returns the heading of the robot as a {@link Rotation2d}.
241     * <p>
242     * The angle increases as the Pigeon 2 turns counterclockwise when
243     * looked at from the top. This follows the NWU axis convention.
244     * <p>
245     * The angle is continuous; that is, it will continue from 360 to
246     * 361 degrees. This allows for algorithms that wouldn't want to
247     * see a discontinuity in the gyro output as it sweeps past from
248     * 360 to 0 on the second time around.
249     *
250     * @return The current heading of the robot as a {@link Rotation2d}
251     */
252    public Rotation2d getRotation2d() {
253        // Rotation2d and Pigeon are both ccw+
254        return Rotation2d.fromDegrees(m_yawGetter.refresh().getValue());
255    }
256
257    /**
258     * Returns the orientation of the robot as a {@link Rotation3d}
259     * created from the quaternion signals.
260     *
261     * @return The current orientation of the robot as a {@link Rotation3d}
262     */
263    public Rotation3d getRotation3d() {
264        BaseStatusSignal.refreshAll(m_quatWGetter, m_quatXGetter, m_quatYGetter, m_quatZGetter);
265        return new Rotation3d(new Quaternion(m_quatWGetter.getValue(), m_quatXGetter.getValue(), m_quatYGetter.getValue(), m_quatZGetter.getValue()));
266    }
267
268    // ----- Sendable ----- //
269    @Override
270    public void initSendable(SendableBuilder builder) {
271        builder.setSmartDashboardType("Gyro");
272        builder.addDoubleProperty("Value", m_yawGetter.asSupplier()::get, this::setYaw);
273    }
274
275}