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    private final StatusSignal<Angle> m_positionGetter = getPosition(false).clone();
045
046    private static final DeviceType kSimDeviceType = DeviceType.P6_CANcoderType;
047
048    private SimDevice m_simCANcoder;
049    private SimDouble m_simSupplyVoltage;
050    private SimDouble m_simPosition;
051    private SimDouble m_simRawPosition;
052    private SimDouble m_simVelocity;
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   The CAN bus this device is on.
079     */
080    public CANcoder(int deviceId, CANBus canbus) {
081        this(deviceId, canbus.getName());
082    }
083
084    /**
085     * Constructs a new CANcoder object.
086     *
087     * @param deviceId ID of the device, as configured in Phoenix Tuner.
088     * @param canbus   Name of the CAN bus this device is on. Possible CAN bus
089     *                 strings are:
090     *                 <ul>
091     *                   <li>"rio" for the native roboRIO CAN bus
092     *                   <li>CANivore name or serial number
093     *                   <li>SocketCAN interface (non-FRC Linux only)
094     *                   <li>"*" for any CANivore seen by the program
095     *                   <li>empty string (default) to select the default for the
096     *                       system:
097     *                   <ul>
098     *                     <li>"rio" on roboRIO
099     *                     <li>"can0" on Linux
100     *                     <li>"*" on Windows
101     *                   </ul>
102     *                 </ul>
103     */
104    public CANcoder(int deviceId, String canbus) {
105        super(deviceId, canbus);
106        SendableRegistry.addLW(this, "CANcoder (v6) ", deviceId);
107
108        if (RobotBase.isSimulation()) {
109            /* run in both swsim and hwsim */
110            AutoFeedEnable.getInstance().start();
111        }
112        if (Utils.isReplay()) {
113            ReplayAutoEnable.getInstance().start();
114        }
115
116        m_simCANcoder = SimDevice.create("CANEncoder:CANcoder (v6)", deviceId);
117        if (m_simCANcoder != null) {
118            m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(this::onPeriodic);
119
120            m_simSupplyVoltage = m_simCANcoder.createDouble("supplyVoltage", Direction.kInput, 12.0);
121
122            m_simPosition = m_simCANcoder.createDouble("position", Direction.kOutput, 0);
123
124            m_simRawPosition = m_simCANcoder.createDouble("rawPositionInput", Direction.kInput, 0);
125            m_simVelocity = m_simCANcoder.createDouble("velocity", Direction.kInput, 0);
126
127            final SimDeviceSim sim = new SimDeviceSim("CANEncoder:CANcoder (v6)");
128            m_simValueChangedCallbacks
129                    .add(sim.registerValueChangedCallback(m_simSupplyVoltage, this::onValueChanged, true));
130            m_simValueChangedCallbacks
131                    .add(sim.registerValueChangedCallback(m_simRawPosition, this::onValueChanged, true));
132            m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simVelocity, this::onValueChanged, true));
133        }
134    }
135
136    // ----- Auto-Closable ----- //
137    @Override
138    public void close() {
139        SendableRegistry.remove(this);
140        if (m_simPeriodicBeforeCallback != null) {
141            m_simPeriodicBeforeCallback.close();
142            m_simPeriodicBeforeCallback = null;
143        }
144        if (m_simCANcoder != null) {
145            m_simCANcoder.close();
146            m_simCANcoder = null;
147        }
148
149        for (var callback : m_simValueChangedCallbacks) {
150            callback.close();
151        }
152        m_simValueChangedCallbacks.clear();
153
154        AutoFeedEnable.getInstance().stop();
155        ReplayAutoEnable.getInstance().stop();
156    }
157
158    // ----- Callbacks for Sim ----- //
159    private void onValueChanged(String name, int handle, int direction, HALValue value) {
160        String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle));
161        String physType = deviceName + ":" + name;
162        PlatformJNI.JNI_SimSetPhysicsInput(
163            kSimDeviceType.value, getDeviceID(),
164            physType, CallbackHelper.getRawValue(value)
165        );
166    }
167
168    private void onPeriodic() {
169        double value = 0;
170        int err = 0;
171
172        final int deviceID = getDeviceID();
173
174        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage");
175        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
176        if (err == 0) {
177            m_simSupplyVoltage.set(value);
178        }
179        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Position");
180        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
181        if (err == 0) {
182            m_simPosition.set(value);
183        }
184        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawPosition");
185        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
186        if (err == 0) {
187            m_simRawPosition.set(value);
188        }
189        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Velocity");
190        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
191        if (err == 0) {
192            m_simVelocity.set(value);
193        }
194    }
195
196    // ----- Sendable ----- //
197    @Override
198    public void initSendable(SendableBuilder builder) {
199        builder.setSmartDashboardType("CANcoder");
200        builder.addDoubleProperty("Position", () -> m_positionGetter.refresh().getValueAsDouble(), this::setPosition);
201    }
202}