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.CoreCANcoder;
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.util.sendable.Sendable;
027import edu.wpi.first.util.sendable.SendableBuilder;
028import edu.wpi.first.util.sendable.SendableRegistry;
029import edu.wpi.first.wpilibj.simulation.CallbackStore;
030import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
031
032public class CANcoder extends CoreCANcoder implements Sendable, AutoCloseable {
033    /**
034     * The StatusSignalValue getters are copies so that calls
035     * to the WPI interface do not update any references
036     */
037    private final StatusSignalValue<Double> m_positionGetter = getPosition().clone();
038
039    private static final DeviceType kSimDeviceType = DeviceType.PRO_CANcoderType;
040
041    private SimDevice m_simCANCoder;
042    private SimDouble m_simSupplyVoltage;
043    private SimDouble m_simPosition;
044    private SimDouble m_simRawPosition;
045    private SimDouble m_simVelocity;
046
047    // callbacks to register
048    private final SimValueCallback m_onValueChangedCallback = new OnValueChangedCallback();
049    private final Runnable m_onPeriodicCallback = new OnPeriodicCallback();
050
051    // returned registered callbacks
052    private final ArrayList<CallbackStore> m_simValueChangedCallbacks = new ArrayList<CallbackStore>();
053    private SimPeriodicBeforeCallback m_simPeriodicBeforeCallback = null;
054
055    /**
056     * Constructs a new CANcoder object.
057     * <p>
058     * Constructs the device using the default CAN bus for the system:
059     * <ul>
060     *   <li>"rio" on roboRIO
061     *   <li>"can0" on Linux
062     *   <li>"*" on Windows
063     * </ul>
064     *
065     * @param deviceId ID of the device, as configured in Phoenix Tuner.
066     */
067    public CANcoder(int deviceId) {
068        this(deviceId, "");
069    }
070
071    /**
072     * Constructs a new CANcoder object.
073     *
074     * @param deviceId ID of the device, as configured in Phoenix Tuner.
075     * @param canbus   Name of the CAN bus this device is on. Possible CAN bus
076     *                 strings are:
077     *                 <ul>
078     *                   <li>"rio" for the native roboRIO CAN bus
079     *                   <li>CANivore name or serial number
080     *                   <li>SocketCAN interface (non-FRC Linux only)
081     *                   <li>"*" for any CANivore seen by the program
082     *                   <li>empty string (default) to select the default for the
083     *                       system:
084     *                   <ul>
085     *                     <li>"rio" on roboRIO
086     *                     <li>"can0" on Linux
087     *                     <li>"*" on Windows
088     *                   </ul>
089     *                 </ul>
090     */
091    public CANcoder(int deviceId, String canbus) {
092        super(deviceId, canbus);
093        SendableRegistry.addLW(this, "CANCoder (Pro) ", deviceId);
094
095        m_simCANCoder = SimDevice.create("CANEncoder:CANCoder (Pro)", deviceId);
096        if (m_simCANCoder != null) {
097            AutoFeedEnable.getInstance();
098            m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(m_onPeriodicCallback);
099
100            m_simSupplyVoltage = m_simCANCoder.createDouble("supplyVoltage", Direction.kInput, 12.0);
101
102            m_simPosition = m_simCANCoder.createDouble("position", Direction.kOutput, 0);
103
104            m_simRawPosition = m_simCANCoder.createDouble("rawPositionInput", Direction.kInput, 0);
105            m_simVelocity = m_simCANCoder.createDouble("velocity", Direction.kInput, 0);
106
107            final SimDeviceSim sim = new SimDeviceSim("CANEncoder:CANCoder (Pro)");
108            m_simValueChangedCallbacks
109                    .add(sim.registerValueChangedCallback(m_simSupplyVoltage, m_onValueChangedCallback, true));
110            m_simValueChangedCallbacks
111                    .add(sim.registerValueChangedCallback(m_simRawPosition, m_onValueChangedCallback, true));
112            m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simVelocity, m_onValueChangedCallback, true));
113        }
114    }
115
116    // ----- Auto-Closable ----- //
117    @Override
118    public void close() {
119        SendableRegistry.remove(this);
120        if (m_simPeriodicBeforeCallback != null) {
121            m_simPeriodicBeforeCallback.close();
122            m_simPeriodicBeforeCallback = null;
123        }
124        if (m_simCANCoder != null) {
125            m_simCANCoder.close();
126            m_simCANCoder = null;
127        }
128
129        for (var callback : m_simValueChangedCallbacks) {
130            callback.close();
131        }
132        m_simValueChangedCallbacks.clear();
133    }
134
135    // ----- Callbacks for Sim ----- //
136    private class OnValueChangedCallback implements SimValueCallback {
137        @Override
138        public void callback(String name, int handle, int direction, HALValue value) {
139            String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle));
140            String physType = deviceName + ":" + name;
141            PlatformJNI.JNI_SimSetPhysicsInput(kSimDeviceType.value, getDeviceID(),
142                    physType, CallbackHelper.getRawValue(value));
143        }
144    }
145
146    private class OnPeriodicCallback implements Runnable {
147        @Override
148        public void run() {
149            double value = 0;
150            int err = 0;
151
152            final int deviceID = getDeviceID();
153
154            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage");
155            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
156            if (err == 0) {
157                m_simSupplyVoltage.set(value);
158            }
159            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Position");
160            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
161            if (err == 0) {
162                m_simPosition.set(value);
163            }
164            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawPosition");
165            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
166            if (err == 0) {
167                m_simRawPosition.set(value);
168            }
169            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Velocity");
170            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
171            if (err == 0) {
172                m_simVelocity.set(value);
173            }
174        }
175    }
176
177    // ----- Sendable ----- //
178    @Override
179    public void initSendable(SendableBuilder builder) {
180        builder.setSmartDashboardType("CANCoder");
181        builder.addDoubleProperty("Position", m_positionGetter.asSupplier()::get, this::setPosition);
182    }
183}