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.CoreCANrange;
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.SimBoolean;
025import edu.wpi.first.hal.SimDevice;
026import edu.wpi.first.hal.SimDevice.Direction;
027import edu.wpi.first.hal.SimDouble;
028import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
029import edu.wpi.first.units.measure.Distance;
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 CoreCANrange}
039 */
040public class CANrange extends CoreCANrange 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 final StatusSignal<Distance> m_distanceGetter = getDistance(false).clone();
046    private final StatusSignal<Boolean> m_isDetectedGetter = getIsDetected(false).clone();
047
048    private static final DeviceType kSimDeviceType = DeviceType.P6_CANrangeType;
049
050    private SimDevice m_simCANrange;
051    private SimDouble m_simSupplyVoltage;
052    private SimDouble m_simDistance;
053    private SimBoolean m_simIsDetected;
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 CANrange object.
061     * <p>
062     * Constructs the device using the default CAN bus for the system:
063     * <ul>
064     *   <li>"rio" on roboRIO
065     *   <li>"can0" on Linux
066     *   <li>"*" on Windows
067     * </ul>
068     *
069     * @param deviceId ID of the device, as configured in Phoenix Tuner.
070     */
071    public CANrange(int deviceId) {
072        this(deviceId, "");
073    }
074
075    /**
076     * Constructs a new CANrange object.
077     *
078     * @param deviceId ID of the device, as configured in Phoenix Tuner.
079     * @param canbus   The CAN bus this device is on.
080     */
081    public CANrange(int deviceId, CANBus canbus) {
082        this(deviceId, canbus.getName());
083    }
084
085    /**
086     * Constructs a new CANrange object.
087     *
088     * @param deviceId ID of the device, as configured in Phoenix Tuner.
089     * @param canbus   Name of the CAN bus this device is on. Possible CAN bus
090     *                 strings are:
091     *                 <ul>
092     *                   <li>"rio" for the native roboRIO CAN bus
093     *                   <li>CANivore name or serial number
094     *                   <li>SocketCAN interface (non-FRC Linux only)
095     *                   <li>"*" for any CANivore seen by the program
096     *                   <li>empty string (default) to select the default for the
097     *                       system:
098     *                   <ul>
099     *                     <li>"rio" on roboRIO
100     *                     <li>"can0" on Linux
101     *                     <li>"*" on Windows
102     *                   </ul>
103     *                 </ul>
104     */
105    public CANrange(int deviceId, String canbus) {
106        super(deviceId, canbus);
107        SendableRegistry.addLW(this, "CANrange (v6) ", deviceId);
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
117        m_simCANrange = SimDevice.create("CAN:CANrange (v6)", deviceId);
118        if (m_simCANrange != null) {
119            m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(this::onPeriodic);
120
121            m_simSupplyVoltage = m_simCANrange.createDouble("supplyVoltage", Direction.kInput, 12.0);
122            m_simDistance = m_simCANrange.createDouble("distance", Direction.kInput, 0);
123
124            m_simIsDetected = m_simCANrange.createBoolean("isDetected", Direction.kOutput, false);
125
126            final SimDeviceSim sim = new SimDeviceSim("CAN:CANrange (v6)");
127            m_simValueChangedCallbacks
128                    .add(sim.registerValueChangedCallback(m_simSupplyVoltage, this::onValueChanged, true));
129            m_simValueChangedCallbacks
130                    .add(sim.registerValueChangedCallback(m_simDistance, this::onValueChanged, true));
131        }
132    }
133
134    // ----- Auto-Closable ----- //
135    @Override
136    public void close() {
137        SendableRegistry.remove(this);
138        if (m_simPeriodicBeforeCallback != null) {
139            m_simPeriodicBeforeCallback.close();
140            m_simPeriodicBeforeCallback = null;
141        }
142        if (m_simCANrange != null) {
143            m_simCANrange.close();
144            m_simCANrange = null;
145        }
146
147        for (var callback : m_simValueChangedCallbacks) {
148            callback.close();
149        }
150        m_simValueChangedCallbacks.clear();
151
152        AutoFeedEnable.getInstance().stop();
153    }
154
155    // ----- Callbacks for Sim ----- //
156    private void onValueChanged(String name, int handle, int direction, HALValue value) {
157        String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle));
158        String physType = deviceName + ":" + name;
159        PlatformJNI.JNI_SimSetPhysicsInput(
160            kSimDeviceType.value, getDeviceID(),
161            physType, CallbackHelper.getRawValue(value)
162        );
163    }
164
165    private void onPeriodic() {
166        double value = 0;
167        int err = 0;
168
169        final int deviceID = getDeviceID();
170
171        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage");
172        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
173        if (err == 0) {
174            m_simSupplyVoltage.set(value);
175        }
176        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Distance");
177        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
178        if (err == 0) {
179            m_simDistance.set(value);
180        }
181        value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "IsDetected");
182        err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
183        if (err == 0) {
184            m_simIsDetected.set(value != 0.0);
185        }
186    }
187
188    // ----- Sendable ----- //
189    @Override
190    public void initSendable(SendableBuilder builder) {
191        builder.setSmartDashboardType("CANrange");
192        builder.addDoubleProperty("Distance", () -> m_distanceGetter.refresh().getValueAsDouble(), null);
193        builder.addBooleanProperty("Is Detected", () -> m_isDetectedGetter.refresh().getValue(), null);
194    }
195}