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.CANBus;
012import com.ctre.phoenix6.StatusSignal;
013import com.ctre.phoenix6.Utils;
014import com.ctre.phoenix6.hardware.core.CoreCANdi;
015import com.ctre.phoenix6.jni.PlatformJNI;
016import com.ctre.phoenix6.sim.DeviceType;
017import com.ctre.phoenix6.wpiutils.AutoFeedEnable;
018import com.ctre.phoenix6.wpiutils.CallbackHelper;
019import com.ctre.phoenix6.wpiutils.ReplayAutoEnable;
020
021import edu.wpi.first.hal.HAL;
022import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback;
023import edu.wpi.first.hal.HALValue;
024import edu.wpi.first.hal.SimDevice;
025import edu.wpi.first.hal.SimDevice.Direction;
026import edu.wpi.first.hal.SimBoolean;
027import edu.wpi.first.hal.SimDouble;
028import edu.wpi.first.hal.SimEnum;
029import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
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.RobotBase;
034import edu.wpi.first.wpilibj.simulation.CallbackStore;
035import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
036
037/**
038 * WPILib-integrated version of {@link CoreCANdi}
039 */
040public class CANdi extends CoreCANdi implements Sendable, AutoCloseable {
041    /**
042     * The StatusSignal getters are copies so that calls
043     * to the WPI interface do not update any references
044     */
045    private static final DeviceType kSimDeviceType = DeviceType.P6_CANdiType;
046
047    private SimDevice m_simCANdi;
048    private SimDouble m_simSupplyVoltage;
049    private SimDouble m_simRequestedOutputCurrent;
050    private SimDouble m_simOutputCurrent;
051
052    private SimDevice m_simPwm1;
053    private SimDouble m_simPwm1Position;
054    private SimBoolean m_simPwm1Connected;
055    private SimDouble m_simPwm1Velocity;
056    private SimDouble m_simPwm1RiseRise;
057    private SimDouble m_simPwm1RiseFall;
058
059    private SimDevice m_simPwm2;
060    private SimDouble m_simPwm2Position;
061    private SimBoolean m_simPwm2Connected;
062    private SimDouble m_simPwm2Velocity;
063    private SimDouble m_simPwm2RiseRise;
064    private SimDouble m_simPwm2RiseFall;
065
066    private SimDevice m_simQuadrature;
067    private SimDouble m_simQuadPos;
068    private SimDouble m_simQuadRawPos;
069    private SimDouble m_simQuadVel;
070
071    private SimDevice m_simS1DIO;
072    private SimBoolean m_simS1Closed;
073    private SimEnum m_simS1State;
074
075    private SimDevice m_simS2DIO;
076    private SimBoolean m_simS2Closed;
077    private SimEnum m_simS2State;
078
079    // returned registered callbacks
080    private final ArrayList<CallbackStore> m_simValueChangedCallbacks = new ArrayList<CallbackStore>();
081    private SimPeriodicBeforeCallback m_simPeriodicBeforeCallback = null;
082
083    /**
084     * Constructs a new CANdi object.
085     * <p>
086     * Constructs the device using the default CAN bus for the system:
087     * <ul>
088     *   <li>"rio" on roboRIO
089     *   <li>"can0" on Linux
090     *   <li>"*" on Windows
091     * </ul>
092     *
093     * @param deviceId ID of the device, as configured in Phoenix Tuner.
094     */
095    public CANdi(int deviceId) {
096        this(deviceId, "");
097    }
098
099    /**
100     * Constructs a new CANdi object.
101     *
102     * @param deviceId ID of the device, as configured in Phoenix Tuner.
103     * @param canbus   The CAN bus this device is on.
104     */
105    public CANdi(int deviceId, CANBus canbus) {
106        this(deviceId, canbus.getName());
107    }
108
109    /**
110     * Constructs a new CANdi object.
111     *
112     * @param deviceId ID of the device, as configured in Phoenix Tuner.
113     * @param canbus   Name of the CAN bus this device is on. Possible CAN bus
114     *                 strings are:
115     *                 <ul>
116     *                   <li>"rio" for the native roboRIO CAN bus
117     *                   <li>CANivore name or serial number
118     *                   <li>SocketCAN interface (non-FRC Linux only)
119     *                   <li>"*" for any CANivore seen by the program
120     *                   <li>empty string (default) to select the default for the
121     *                       system:
122     *                   <ul>
123     *                     <li>"rio" on roboRIO
124     *                     <li>"can0" on Linux
125     *                     <li>"*" on Windows
126     *                   </ul>
127     *                 </ul>
128     */
129    public CANdi(int deviceId, String canbus) {
130        super(deviceId, canbus);
131        SendableRegistry.addLW(this, "CANdi (v6) ", deviceId);
132
133        if (RobotBase.isSimulation()) {
134            /* run in both swsim and hwsim */
135            AutoFeedEnable.getInstance().start();
136        }
137        if (Utils.isReplay()) {
138            ReplayAutoEnable.getInstance().start();
139        }
140
141        m_simCANdi = SimDevice.create("CAN:CANdi (v6)", deviceId);
142
143        final String base = "CANdi (v6)[" + deviceId + "]/";
144        m_simPwm1 = SimDevice.create("CANDutyCycle:" + base + "PWM1");
145        m_simPwm2 = SimDevice.create("CANDutyCycle:" + base + "PWM2");
146        m_simQuadrature = SimDevice.create("CANEncoder:" + base + "Quadrature");
147        m_simS1DIO = SimDevice.create("CANDIO:" + base + "S1");
148        m_simS2DIO = SimDevice.create("CANDIO:" + base + "S2");
149
150        final String[] StateEnums = new String[]{"Floating", "Low", "High"};
151
152        if (m_simCANdi != null) {
153            m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(this::onPeriodic);
154
155            m_simSupplyVoltage = m_simCANdi.createDouble("supplyVoltage", Direction.kInput, 12.0);
156            m_simRequestedOutputCurrent = m_simCANdi.createDouble("requestedOutputCurrent", Direction.kInput, 0);
157            m_simOutputCurrent = m_simCANdi.createDouble("outputCurrent", Direction.kOutput, 0);
158
159            final SimDeviceSim sim = new SimDeviceSim("CAN:CANdi (v6)");
160            m_simValueChangedCallbacks
161                    .add(sim.registerValueChangedCallback(m_simSupplyVoltage, this::onValueChanged, true));
162            m_simValueChangedCallbacks
163                    .add(sim.registerValueChangedCallback(m_simRequestedOutputCurrent, this::onValueChanged, true));
164        }
165        if (m_simPwm1 != null) {
166            m_simPwm1Position = m_simPwm1.createDouble("position", Direction.kInput, 0);
167            m_simPwm1Connected = m_simPwm1.createBoolean("connected", Direction.kInput, true);
168            m_simPwm1Velocity = m_simPwm1.createDouble("velocity", Direction.kInput, 0);
169            m_simPwm1RiseRise = m_simPwm1.createDouble("riseToRise", Direction.kInput, 0.004);
170            m_simPwm1RiseFall = m_simPwm1.createDouble("riseToFall", Direction.kInput, 0);
171
172            final SimDeviceSim sim = new SimDeviceSim("CANDutyCycle:" + base + "PWM1");
173            m_simValueChangedCallbacks
174                    .add(sim.registerValueChangedCallback(m_simPwm1Position, this::onValueChanged, true));
175            m_simValueChangedCallbacks
176                    .add(sim.registerValueChangedCallback(m_simPwm1Connected, this::onValueChanged, true));
177            m_simValueChangedCallbacks
178                    .add(sim.registerValueChangedCallback(m_simPwm1Velocity, this::onValueChanged, true));
179            m_simValueChangedCallbacks
180                    .add(sim.registerValueChangedCallback(m_simPwm1RiseRise, this::onValueChanged, true));
181            m_simValueChangedCallbacks
182                    .add(sim.registerValueChangedCallback(m_simPwm1RiseFall, this::onValueChanged, true));
183        }
184        if (m_simPwm2 != null) {
185            m_simPwm2Position = m_simPwm2.createDouble("position", Direction.kInput, 0);
186            m_simPwm2Connected = m_simPwm2.createBoolean("connected", Direction.kInput, true);
187            m_simPwm2Velocity = m_simPwm2.createDouble("velocity", Direction.kInput, 0);
188            m_simPwm2RiseRise = m_simPwm2.createDouble("riseToRise", Direction.kInput, 0.004);
189            m_simPwm2RiseFall = m_simPwm2.createDouble("riseToFall", Direction.kInput, 0);
190
191            final SimDeviceSim sim = new SimDeviceSim("CANDutyCycle:" + base + "PWM2");
192            m_simValueChangedCallbacks
193                    .add(sim.registerValueChangedCallback(m_simPwm2Position, this::onValueChanged, true));
194            m_simValueChangedCallbacks
195                    .add(sim.registerValueChangedCallback(m_simPwm2Connected, this::onValueChanged, true));
196            m_simValueChangedCallbacks
197                    .add(sim.registerValueChangedCallback(m_simPwm2Velocity, this::onValueChanged, true));
198            m_simValueChangedCallbacks
199                    .add(sim.registerValueChangedCallback(m_simPwm2RiseRise, this::onValueChanged, true));
200            m_simValueChangedCallbacks
201                    .add(sim.registerValueChangedCallback(m_simPwm2RiseFall, this::onValueChanged, true));
202        }
203        if (m_simQuadrature != null) {
204            m_simQuadrature.createBoolean("init", Direction.kOutput, true);
205
206            m_simQuadPos = m_simQuadrature.createDouble("position", Direction.kOutput, 0);
207
208            m_simQuadRawPos = m_simQuadrature.createDouble("rawPositionInput", Direction.kInput, 0);
209            m_simQuadVel = m_simQuadrature.createDouble("velocity", Direction.kInput, 0);
210
211            final SimDeviceSim sim = new SimDeviceSim("CANEncoder:" + base + "Quadrature");
212            m_simValueChangedCallbacks
213                    .add(sim.registerValueChangedCallback(m_simQuadRawPos, this::onValueChanged, true));
214            m_simValueChangedCallbacks
215                    .add(sim.registerValueChangedCallback(m_simQuadVel, this::onValueChanged, true));
216        }
217        if (m_simS1DIO != null) {
218            m_simS1DIO.createBoolean("init", Direction.kOutput, true);
219            m_simS1DIO.createBoolean("input", Direction.kOutput, true);
220            m_simS1Closed = m_simS1DIO.createBoolean("value", Direction.kOutput, false);
221            m_simS1State = m_simS1DIO.createEnum("state", Direction.kInput, StateEnums, 0);
222
223            final SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "S1");
224            m_simValueChangedCallbacks
225                    .add(sim.registerValueChangedCallback(m_simS1State, this::onValueChanged, true));
226        }
227        if (m_simS2DIO != null) {
228            m_simS2DIO.createBoolean("init", Direction.kOutput, true);
229            m_simS2DIO.createBoolean("input", Direction.kOutput, true);
230            m_simS2Closed = m_simS2DIO.createBoolean("value", Direction.kOutput, false);
231            m_simS2State = m_simS2DIO.createEnum("state", Direction.kInput, StateEnums, 0);
232
233            final SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "S2");
234            m_simValueChangedCallbacks
235                    .add(sim.registerValueChangedCallback(m_simS2State, this::onValueChanged, true));
236        }
237    }
238
239    // ----- Auto-Closable ----- //
240    @Override
241    public void close() {
242        SendableRegistry.remove(this);
243        if (m_simPeriodicBeforeCallback != null) {
244            m_simPeriodicBeforeCallback.close();
245            m_simPeriodicBeforeCallback = null;
246        }
247        if (m_simCANdi != null) {
248            m_simCANdi.close();
249            m_simCANdi = null;
250        }
251        if (m_simPwm1 != null) {
252            m_simPwm1.close();
253            m_simPwm1 = null;
254        }
255        if (m_simPwm2 != null) {
256            m_simPwm2.close();
257            m_simPwm2 = null;
258        }
259        if (m_simQuadrature != null) {
260            m_simQuadrature.close();
261            m_simQuadrature = null;
262        }
263        if (m_simS1DIO != null) {
264            m_simS1DIO.close();
265            m_simS1DIO = null;
266        }
267        if (m_simS2DIO != null) {
268            m_simS2DIO.close();
269            m_simS2DIO = null;
270        }
271
272        for (var callback : m_simValueChangedCallbacks) {
273            callback.close();
274        }
275        m_simValueChangedCallbacks.clear();
276
277        AutoFeedEnable.getInstance().stop();
278    }
279
280    // ----- Callbacks for Sim ----- //
281    private void onValueChanged(String name, int handle, int direction, HALValue value) {
282        String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle));
283        String physType = deviceName + ":" + name;
284        PlatformJNI.JNI_SimSetPhysicsInput(
285            kSimDeviceType.value, getDeviceID(),
286            physType, CallbackHelper.getRawValue(value)
287        );
288    }
289
290    private void onPeriodic() {
291        double value = 0;
292        int err = 0;
293
294        final int deviceID = getDeviceID();
295
296        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage");
297        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
298        if (err == 0) {
299            m_simSupplyVoltage.set(value);
300        }
301        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RequestedOutputCurrent");
302        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
303        if (err == 0) {
304            m_simRequestedOutputCurrent.set(value);
305        }
306        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "OutputCurrent");
307        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
308        if (err == 0) {
309            m_simOutputCurrent.set(value);
310        }
311
312        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM1Position");
313        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
314        if (err == 0) {
315            m_simPwm1Position.set(value);
316        }
317        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM1Connected");
318        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
319        if (err == 0) {
320            m_simPwm1Connected.set((int)value != 0);
321        }
322        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM1Velocity");
323        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
324        if (err == 0) {
325            m_simPwm1Velocity.set(value);
326        }
327        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM1RiseToRise");
328        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
329        if (err == 0) {
330            m_simPwm1RiseRise.set(value);
331        }
332        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM1RiseToFall");
333        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
334        if (err == 0) {
335            m_simPwm1RiseFall.set(value);
336        }
337        
338        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM2Position");
339        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
340        if (err == 0) {
341            m_simPwm2Position.set(value);
342        }
343        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM2Connected");
344        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
345        if (err == 0) {
346            m_simPwm2Connected.set((int)value != 0);
347        }
348        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM2Velocity");
349        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
350        if (err == 0) {
351            m_simPwm2Velocity.set(value);
352        }
353        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM2RiseToRise");
354        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
355        if (err == 0) {
356            m_simPwm2RiseRise.set(value);
357        }
358        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM2RiseToFall");
359        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
360        if (err == 0) {
361            m_simPwm2RiseFall.set(value);
362        }
363
364        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "QuadraturePosition");
365        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
366        if (err == 0) {
367            m_simQuadPos.set(value);
368        }
369        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawQuadraturePosition");
370        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
371        if (err == 0) {
372            m_simQuadRawPos.set(value);
373        }
374        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "QuadratureVelocity");
375        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
376        if (err == 0) {
377            m_simQuadVel.set(value);
378        }
379
380        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "S1State");
381        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
382        if (err == 0) {
383            m_simS1State.set((int)value);
384        }
385        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "S1Closed");
386        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
387        if (err == 0) {
388            m_simS1Closed.set((int)value != 0);
389        }
390
391        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "S2State");
392        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
393        if (err == 0) {
394            m_simS2State.set((int)value);
395        }
396        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "S2Closed");
397        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
398        if (err == 0) {
399            m_simS2Closed.set((int)value != 0);
400        }
401    }
402
403    // ----- Sendable ----- //
404    @Override
405    public void initSendable(SendableBuilder builder) {
406        builder.setSmartDashboardType("CANdi");
407    }
408}