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.CoreCANcoder;
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.SimDouble;
027import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
028import edu.wpi.first.units.measure.Angle;
029import edu.wpi.first.util.sendable.Sendable;
030import edu.wpi.first.util.sendable.SendableBuilder;
031import edu.wpi.first.util.sendable.SendableRegistry;
032import edu.wpi.first.wpilibj.RobotBase;
033import edu.wpi.first.wpilibj.simulation.CallbackStore;
034import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
035
036/**
037 * WPILib-integrated version of {@link CoreCANcoder}.
038 */
039public class CANcoder extends CoreCANcoder implements Sendable, AutoCloseable {
040    /**
041     * The StatusSignal getters are copies so that calls
042     * to the WPI interface do not update any references
043     */
044    @SuppressWarnings("this-escape")
045    private final StatusSignal<Angle> m_positionGetter = getPosition(false).clone();
046
047    private static final DeviceType kSimDeviceType = DeviceType.P6_CANcoderType;
048
049    private SimDevice m_simCANcoder;
050    private SimDouble m_simSupplyVoltage;
051    private SimDouble m_simPosition;
052    private SimDouble m_simRawPosition;
053    private SimDouble m_simVelocity;
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 CANcoder object.
061     * <p>
062     * Constructs the device using the default CAN bus for the system
063     * (see {@link CANBus#CANBus()}).
064     *
065     * @param deviceId ID of the device, as configured in Phoenix Tuner
066     */
067    public CANcoder(int deviceId) {
068        this(deviceId, new CANBus());
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     * @deprecated Constructing devices with a CAN bus string is deprecated for removal
092     * in the 2027 season. Construct devices using a {@link CANBus} instance instead.
093     */
094    @Deprecated(since = "2026", forRemoval = true)
095    public CANcoder(int deviceId, String canbus) {
096        this(deviceId, new CANBus(canbus));
097    }
098
099    /**
100     * Constructs a new CANcoder 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    @SuppressWarnings("this-escape")
106    public CANcoder(int deviceId, CANBus canbus) {
107        super(deviceId, canbus);
108
109        if (RobotBase.isSimulation()) {
110            /* run in both swsim and hwsim */
111            AutoFeedEnable.getInstance().start();
112        }
113        if (Utils.isReplay()) {
114            ReplayAutoEnable.getInstance().start();
115        }
116        if (deviceId == -1) return;
117
118        SendableRegistry.addLW(this, "CANcoder (v6) ", deviceId);
119
120        m_simCANcoder = SimDevice.create("CANEncoder:CANcoder (v6)", deviceId);
121        if (m_simCANcoder != null) {
122            m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(this::onPeriodic);
123
124            m_simSupplyVoltage = m_simCANcoder.createDouble("supplyVoltage", Direction.kInput, 12.0);
125
126            m_simPosition = m_simCANcoder.createDouble("position", Direction.kOutput, 0);
127
128            m_simRawPosition = m_simCANcoder.createDouble("rawPositionInput", Direction.kInput, 0);
129            m_simVelocity = m_simCANcoder.createDouble("velocity", Direction.kInput, 0);
130
131            final SimDeviceSim sim = new SimDeviceSim("CANEncoder:CANcoder (v6)");
132            m_simValueChangedCallbacks.add(
133                sim.registerValueChangedCallback(m_simSupplyVoltage, this::onValueChanged, true)
134            );
135            m_simValueChangedCallbacks.add(
136                sim.registerValueChangedCallback(m_simRawPosition, this::onValueChanged, true)
137            );
138            m_simValueChangedCallbacks.add(
139                sim.registerValueChangedCallback(m_simVelocity, this::onValueChanged, true)
140            );
141        }
142    }
143
144    /**
145     * Constructs a stubbed-out CANcoder, where all status signals, controls, configs,
146     * etc. perform no action and immediately return OK. This can be used to silence
147     * error messages for devices that have been completely removed from the robot.
148     *
149     * @return Stubbed-out CANcoder
150     */
151    public static CANcoder none() {
152        return new CANcoder(-1, new CANBus());
153    }
154
155    // ----- Auto-Closable ----- //
156    @Override
157    public void close() {
158        SendableRegistry.remove(this);
159        if (m_simPeriodicBeforeCallback != null) {
160            m_simPeriodicBeforeCallback.close();
161            m_simPeriodicBeforeCallback = null;
162        }
163        if (m_simCANcoder != null) {
164            m_simCANcoder.close();
165            m_simCANcoder = null;
166        }
167
168        for (var callback : m_simValueChangedCallbacks) {
169            callback.close();
170        }
171        m_simValueChangedCallbacks.clear();
172
173        AutoFeedEnable.getInstance().stop();
174        ReplayAutoEnable.getInstance().stop();
175    }
176
177    // ----- Callbacks for Sim ----- //
178    private void onValueChanged(String name, int handle, int direction, HALValue value) {
179        String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle));
180        String physType = deviceName + ":" + name;
181        PlatformJNI.JNI_SimSetPhysicsInput(
182            kSimDeviceType.value, getDeviceID(),
183            physType, CallbackHelper.getRawValue(value)
184        );
185    }
186
187    private void onPeriodic() {
188        double value = 0;
189        int err = 0;
190
191        final int deviceID = getDeviceID();
192
193        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage");
194        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
195        if (err == 0) {
196            m_simSupplyVoltage.set(value);
197        }
198        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Position");
199        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
200        if (err == 0) {
201            m_simPosition.set(value);
202        }
203        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawPosition");
204        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
205        if (err == 0) {
206            m_simRawPosition.set(value);
207        }
208        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Velocity");
209        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
210        if (err == 0) {
211            m_simVelocity.set(value);
212        }
213    }
214
215    // ----- Sendable ----- //
216    @Override
217    public void initSendable(SendableBuilder builder) {
218        builder.setSmartDashboardType("CANcoder");
219        builder.addDoubleProperty("Position", () -> m_positionGetter.refresh().getValueAsDouble(), this::setPosition);
220    }
221}