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.StatusSignal;
012import com.ctre.phoenix6.hardware.core.CoreCANcoder;
013import com.ctre.phoenix6.jni.PlatformJNI;
014import com.ctre.phoenix6.sim.DeviceType;
015import com.ctre.phoenix6.wpiutils.AutoFeedEnable;
016import com.ctre.phoenix6.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
032/**
033 * WPILib-integrated version of {@link CoreCANcoder}
034 */
035public class CANcoder extends CoreCANcoder implements Sendable, AutoCloseable {
036    /**
037     * The StatusSignal getters are copies so that calls
038     * to the WPI interface do not update any references
039     */
040    private final StatusSignal<Double> m_positionGetter = getPosition().clone();
041
042    private static final DeviceType kSimDeviceType = DeviceType.PRO_CANcoderType;
043
044    private SimDevice m_simCANCoder;
045    private SimDouble m_simSupplyVoltage;
046    private SimDouble m_simPosition;
047    private SimDouble m_simRawPosition;
048    private SimDouble m_simVelocity;
049
050    // callbacks to register
051    private final SimValueCallback m_onValueChangedCallback = new OnValueChangedCallback();
052    private final Runnable m_onPeriodicCallback = new OnPeriodicCallback();
053
054    // returned registered callbacks
055    private final ArrayList<CallbackStore> m_simValueChangedCallbacks = new ArrayList<CallbackStore>();
056    private SimPeriodicBeforeCallback m_simPeriodicBeforeCallback = null;
057
058    /**
059     * Constructs a new CANcoder object.
060     * <p>
061     * Constructs the device using the default CAN bus for the system:
062     * <ul>
063     *   <li>"rio" on roboRIO
064     *   <li>"can0" on Linux
065     *   <li>"*" on Windows
066     * </ul>
067     *
068     * @param deviceId ID of the device, as configured in Phoenix Tuner.
069     */
070    public CANcoder(int deviceId) {
071        this(deviceId, "");
072    }
073
074    /**
075     * Constructs a new CANcoder object.
076     *
077     * @param deviceId ID of the device, as configured in Phoenix Tuner.
078     * @param canbus   Name of the CAN bus this device is on. Possible CAN bus
079     *                 strings are:
080     *                 <ul>
081     *                   <li>"rio" for the native roboRIO CAN bus
082     *                   <li>CANivore name or serial number
083     *                   <li>SocketCAN interface (non-FRC Linux only)
084     *                   <li>"*" for any CANivore seen by the program
085     *                   <li>empty string (default) to select the default for the
086     *                       system:
087     *                   <ul>
088     *                     <li>"rio" on roboRIO
089     *                     <li>"can0" on Linux
090     *                     <li>"*" on Windows
091     *                   </ul>
092     *                 </ul>
093     */
094    public CANcoder(int deviceId, String canbus) {
095        super(deviceId, canbus);
096        SendableRegistry.addLW(this, "CANCoder (v6) ", deviceId);
097
098        m_simCANCoder = SimDevice.create("CANEncoder:CANCoder (v6)", deviceId);
099        if (m_simCANCoder != null) {
100            AutoFeedEnable.getInstance().start();
101            m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(m_onPeriodicCallback);
102
103            m_simSupplyVoltage = m_simCANCoder.createDouble("supplyVoltage", Direction.kInput, 12.0);
104
105            m_simPosition = m_simCANCoder.createDouble("position", Direction.kOutput, 0);
106
107            m_simRawPosition = m_simCANCoder.createDouble("rawPositionInput", Direction.kInput, 0);
108            m_simVelocity = m_simCANCoder.createDouble("velocity", Direction.kInput, 0);
109
110            final SimDeviceSim sim = new SimDeviceSim("CANEncoder:CANCoder (v6)");
111            m_simValueChangedCallbacks
112                    .add(sim.registerValueChangedCallback(m_simSupplyVoltage, m_onValueChangedCallback, true));
113            m_simValueChangedCallbacks
114                    .add(sim.registerValueChangedCallback(m_simRawPosition, m_onValueChangedCallback, true));
115            m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simVelocity, m_onValueChangedCallback, true));
116        }
117    }
118
119    // ----- Auto-Closable ----- //
120    @Override
121    public void close() {
122        SendableRegistry.remove(this);
123        if (m_simPeriodicBeforeCallback != null) {
124            m_simPeriodicBeforeCallback.close();
125            m_simPeriodicBeforeCallback = null;
126        }
127        if (m_simCANCoder != null) {
128            m_simCANCoder.close();
129            m_simCANCoder = null;
130        }
131
132        for (var callback : m_simValueChangedCallbacks) {
133            callback.close();
134        }
135        m_simValueChangedCallbacks.clear();
136
137        AutoFeedEnable.getInstance().stop();
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, "Position");
165            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
166            if (err == 0) {
167                m_simPosition.set(value);
168            }
169            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawPosition");
170            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
171            if (err == 0) {
172                m_simRawPosition.set(value);
173            }
174            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Velocity");
175            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
176            if (err == 0) {
177                m_simVelocity.set(value);
178            }
179        }
180    }
181
182    // ----- Sendable ----- //
183    @Override
184    public void initSendable(SendableBuilder builder) {
185        builder.setSmartDashboardType("CANCoder");
186        builder.addDoubleProperty("Position", m_positionGetter.asSupplier()::get, this::setPosition);
187    }
188}