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