001/**
002 * WPI Compliant motor controller class.
003 * WPILIB's object model requires many interfaces to be implemented to use
004 * the various features.
005 * This includes...
006 * - LiveWindow/Test mode features
007 * - getRotation2d/Gyro Interface
008 * - Simulation Hooks
009 */
010
011package com.ctre.phoenix.sensors;
012
013import com.ctre.phoenix.motorcontrol.can.TalonSRX;
014import com.ctre.phoenix.platform.DeviceType;
015import com.ctre.phoenix.platform.PlatformJNI;
016import com.ctre.phoenix.sensors.PigeonIMU;
017import com.ctre.phoenix.WPI_CallbackHelper;
018
019import edu.wpi.first.wpilibj.DriverStation;
020import edu.wpi.first.util.sendable.Sendable;
021import edu.wpi.first.util.sendable.SendableBuilder;
022import edu.wpi.first.util.sendable.SendableRegistry;
023import edu.wpi.first.math.geometry.Rotation2d;
024
025import edu.wpi.first.hal.SimDevice.Direction;
026import edu.wpi.first.hal.HALValue;
027import edu.wpi.first.hal.SimDevice;
028import edu.wpi.first.hal.SimDouble;
029import edu.wpi.first.wpilibj.simulation.CallbackStore;
030import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
031import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
032import edu.wpi.first.hal.simulation.SimValueCallback;
033import edu.wpi.first.hal.simulation.SimulatorJNI;
034import edu.wpi.first.hal.HAL;
035import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback;
036
037import java.util.ArrayList;
038
039public class WPI_PigeonIMU extends PigeonIMU implements Sendable, AutoCloseable {
040
041    private double[] _xyz_dps; //instance this so we don't alloc when getRate is called.
042
043    private SimDevice m_simPigeon;
044    private SimDouble m_simFusedHeading;
045    private SimDouble m_simRawHeading;
046    private DeviceType m_type;
047
048    // callbacks to register
049        private SimValueCallback onValueChangedCallback = new OnValueChangedCallback();
050        private Runnable onPeriodicCallback = new OnPeriodicCallback();
051
052        // returned registered callbacks
053        private ArrayList<CallbackStore> simValueChangedCallbacks = new ArrayList<CallbackStore>();
054        private SimPeriodicBeforeCallback simPeriodicCallback;
055
056    /**
057     * Constructor for Pigeon IMU.
058         * @param deviceNumber device ID of Pigeon IMU
059     */
060    public WPI_PigeonIMU(int deviceNumber) {
061        super(deviceNumber);
062        m_type = DeviceType.PigeonIMU;
063                SendableRegistry.addLW(this, "Pigeon IMU ", deviceNumber);
064        init();
065    }
066
067    /**
068     * Construtor for WPI_PigeonIMU.
069     *
070     * @param talon The Talon SRX ribbon-cabled to Pigeon.
071     */
072    public WPI_PigeonIMU(TalonSRX talon) {
073        super(talon);
074        m_type = DeviceType.RibbonPigeonIMU;
075        SendableRegistry.addLW(this, "Pigeon IMU ", talon.getDeviceID());
076        init();
077    }
078
079    private void init(){
080        _xyz_dps = new double[3];
081
082        m_simPigeon = SimDevice.create("CANGyro:Pigeon IMU", getDeviceID());
083        if(m_simPigeon != null) {
084            simPeriodicCallback = HAL.registerSimPeriodicBeforeCallback(onPeriodicCallback);
085
086            m_simFusedHeading = m_simPigeon.createDouble("fusedHeading", Direction.kOutput, 0);
087
088            m_simRawHeading = m_simPigeon.createDouble("rawHeadingInput", Direction.kInput, 0);
089
090            SimDeviceSim sim = new SimDeviceSim("CANGyro:Pigeon IMU");
091            simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simRawHeading, onValueChangedCallback, true));
092        }
093    }
094
095    // ----- Auto-Closable, from Gyro ----- //
096    @Override
097    public void close(){
098        SendableRegistry.remove(this);
099        if(m_simPigeon != null) {
100            m_simPigeon.close();
101            m_simPigeon = null;
102        }
103        super.DestroyObject(); //Pigeon device, replace with super.close() once implemented
104    }
105
106
107    // ----- Callbacks for Sim ----- //
108    private class OnValueChangedCallback implements SimValueCallback {
109                @Override
110                public void callback(String name, int handle, int direction, HALValue value) {
111                        String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle));
112                        String physType = deviceName + ":" + name;
113                        PlatformJNI.JNI_SimSetPhysicsInput(m_type.value, getDeviceID(),
114                                                                               physType, WPI_CallbackHelper.getRawValue(value));
115                }
116        }
117
118        private class OnPeriodicCallback implements Runnable {
119                @Override
120                public void run() {
121                        double value = 0;
122                        int err = 0;
123
124                        int deviceID = getDeviceID();
125
126                        value = PlatformJNI.JNI_SimGetPhysicsValue(m_type.value, deviceID, "FusedHeading");
127                        err = PlatformJNI.JNI_SimGetLastError(m_type.value, deviceID);
128                        if (err == 0) {
129                                m_simFusedHeading.set(value);
130                        }
131                        value = PlatformJNI.JNI_SimGetPhysicsValue(m_type.value, deviceID, "HeadingRaw");
132                        err = PlatformJNI.JNI_SimGetLastError(m_type.value, deviceID);
133                        if (err == 0) {
134                                m_simRawHeading.set(value);
135                        }
136                }
137        }
138
139
140    // ----- WPILib Gyro Interface ----- //
141    //WPILib no longer has a Gyro interface, but these methods are standard in FRC.
142
143    /**
144     * Resets the Pigeon IMU to a heading of zero.
145     * <p>
146     * This can be used if there is significant drift in the gyro,
147     * and it needs to be recalibrated after it has been running.
148     */
149    public void reset(){
150        setFusedHeading(0);
151    }
152
153    /**
154     * Returns the heading of the robot in degrees.
155     * <p>
156     * The angle increases as the Pigeon IMU turns clockwise when looked
157     * at from the top. This follows the NED axis convention.
158     * <p>
159     * The angle is continuous; that is, it will continue from 360 to
160     * 361 degrees. This allows for algorithms that wouldn't want to
161     * see a discontinuity in the gyro output as it sweeps past from
162     * 360 to 0 on the second time around.
163     *
164     * @return The current heading of the robot in degrees
165     */
166    public double getAngle(){
167        //Negate since getAngle requires cw+ and Pigeon is ccw+
168        return -getFusedHeading();
169    }
170
171    /**
172     * Returns the rate of rotation of the Pigeon IMU.
173     * <p>
174     * The rate is positive as the Pigeon IMU turns clockwise when looked
175     * at from the top.
176     *
177     * @return The current rate in degrees per second
178     */
179    public double getRate(){
180        getRawGyro(_xyz_dps);
181        //Expected to return cw+
182        return -_xyz_dps[2];
183    }
184
185    /**
186     * Returns the heading of the robot as a {@link Rotation2d}.
187     * <p>
188     * The angle increases as the Pigeon IMU turns counterclockwise when
189     * looked at from the top. This follows the NWU axis convention.
190     * <p>
191     * The angle is continuous; that is, it will continue from 360 to
192     * 361 degrees. This allows for algorithms that wouldn't want to
193     * see a discontinuity in the gyro output as it sweeps past from
194     * 360 to 0 on the second time around.
195     *
196     * @return The current heading of the robot as a {@link Rotation2d}
197     */
198    public Rotation2d getRotation2d() {
199        //Rotation2d and Pigeon are both ccw+
200        return Rotation2d.fromDegrees(getFusedHeading());
201    }
202
203    // ----- Sendable ----- //
204    @Override
205    public void initSendable(SendableBuilder builder) {
206        builder.setSmartDashboardType("Gyro");
207        builder.addDoubleProperty("Value", this::getFusedHeading, null);
208    }
209}