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}