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;
010import java.util.concurrent.locks.Lock;
011import java.util.concurrent.locks.ReentrantLock;
012
013import com.ctre.phoenixpro.StatusCode;
014import com.ctre.phoenixpro.StatusSignalValue;
015import com.ctre.phoenixpro.configs.MotorOutputConfigs;
016import com.ctre.phoenixpro.controls.ControlRequest;
017import com.ctre.phoenixpro.controls.DutyCycleOut;
018import com.ctre.phoenixpro.controls.NeutralOut;
019import com.ctre.phoenixpro.controls.VoltageOut;
020import com.ctre.phoenixpro.hardware.core.CoreTalonFX;
021import com.ctre.phoenixpro.jni.PlatformJNI;
022import com.ctre.phoenixpro.signals.InvertedValue;
023import com.ctre.phoenixpro.sim.DeviceType;
024import com.ctre.phoenixpro.wpiutils.AutoFeedEnable;
025import com.ctre.phoenixpro.wpiutils.CallbackHelper;
026import com.ctre.phoenixpro.wpiutils.MotorSafetyImplem;
027
028import edu.wpi.first.wpilibj.motorcontrol.MotorController;
029import edu.wpi.first.wpilibj.simulation.CallbackStore;
030import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
031import edu.wpi.first.hal.HAL;
032import edu.wpi.first.hal.HALValue;
033import edu.wpi.first.hal.SimBoolean;
034import edu.wpi.first.hal.SimDevice;
035import edu.wpi.first.hal.SimDouble;
036import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback;
037import edu.wpi.first.hal.SimDevice.Direction;
038import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
039import edu.wpi.first.hal.simulation.SimValueCallback;
040import edu.wpi.first.util.sendable.Sendable;
041import edu.wpi.first.util.sendable.SendableBuilder;
042import edu.wpi.first.util.sendable.SendableRegistry;
043
044public class TalonFX extends CoreTalonFX implements MotorController, Sendable, AutoCloseable {
045    /**
046     * The StatusSignalValue getters are copies so that calls
047     * to the WPI interface do not update any references
048     */
049    private final StatusSignalValue<Double> m_dutyCycle = getDutyCycle().clone();
050
051    private final DutyCycleOut m_setterControl = new DutyCycleOut(0);
052    private final NeutralOut m_neutralControl = new NeutralOut();
053    private final VoltageOut m_voltageControl = new VoltageOut(0);
054
055    private final MotorOutputConfigs m_configs = new MotorOutputConfigs();
056
057    private String m_description;
058
059    private static final DeviceType kSimDeviceType = DeviceType.PRO_TalonFXType;
060
061    private SimDevice m_simMotor;
062    private SimDouble m_simSupplyVoltage;
063    private SimDouble m_simDutyCycle;
064    private SimDouble m_simMotorVoltage;
065    private SimDouble m_simTorqueCurrent;
066    private SimDouble m_simSupplyCurrent;
067
068    private SimDevice m_simForwardLimit;
069    private SimBoolean m_simForwardLimitValue;
070
071    private SimDevice m_simReverseLimit;
072    private SimBoolean m_simReverseLimitValue;
073
074    private SimDevice m_simRotor;
075    private SimDouble m_simRotorPos;
076    private SimDouble m_simRotorRawPos;
077    private SimDouble m_simRotorVel;
078
079    // callbacks to register
080    private final SimValueCallback m_onValueChangedCallback = new OnValueChangedCallback();
081    private final Runnable m_onPeriodicCallback = new OnPeriodicCallback();
082
083    // returned registered callbacks
084    private final ArrayList<CallbackStore> m_simValueChangedCallbacks = new ArrayList<CallbackStore>();
085    private SimPeriodicBeforeCallback m_simPeriodicBeforeCallback = null;
086
087    /**
088     * The default motor safety timeout IF calling application
089     * enables the feature.
090     */
091    public static final double kDefaultSafetyExpiration = 0.1;
092
093    /**
094     * Late-constructed motor safety, which ensures feature is off unless calling
095     * applications explicitly enables it.
096     */
097    private MotorSafetyImplem m_motorSafety = null;
098    private final Lock m_lockMotorSaf = new ReentrantLock();
099    private double m_motSafeExpiration = kDefaultSafetyExpiration;
100
101    /**
102     * Constructs a new Talon FX motor controller object.
103     * <p>
104     * Constructs the device using the default CAN bus for the system:
105     * <ul>
106     *   <li>"rio" on roboRIO
107     *   <li>"can0" on Linux
108     *   <li>"*" on Windows
109     * </ul>
110     *
111     * @param deviceId ID of the device, as configured in Phoenix Tuner.
112     */
113    public TalonFX(int deviceId) {
114        this(deviceId, "");
115    }
116
117    /**
118     * Constructs a new Talon FX motor controller object.
119     *
120     * @param deviceId ID of the device, as configured in Phoenix Tuner.
121     * @param canbus   Name of the CAN bus this device is on. Possible CAN bus
122     *                 strings are:
123     *                 <ul>
124     *                   <li>"rio" for the native roboRIO CAN bus
125     *                   <li>CANivore name or serial number
126     *                   <li>SocketCAN interface (non-FRC Linux only)
127     *                   <li>"*" for any CANivore seen by the program
128     *                   <li>empty string (default) to select the default for the
129     *                       system:
130     *                   <ul>
131     *                     <li>"rio" on roboRIO
132     *                     <li>"can0" on Linux
133     *                     <li>"*" on Windows
134     *                   </ul>
135     *                 </ul>
136     */
137    public TalonFX(int deviceId, String canbus) {
138        super(deviceId, canbus);
139
140        m_description = "Talon FX (Pro) " + deviceId;
141        SendableRegistry.addLW(this, "Talon FX (Pro) ", deviceId);
142
143        m_simMotor = SimDevice.create("CANMotor:Talon FX (Pro)", deviceId);
144
145        final String base = "Talon FX (Pro)[" + deviceId + "]/";
146        m_simRotor = SimDevice.create("CANEncoder:" + base + "Rotor Sensor");
147        m_simForwardLimit = SimDevice.create("CANDIO:" + base + "Fwd Limit");
148        m_simReverseLimit = SimDevice.create("CANDIO:" + base + "Rev Limit");
149
150        if (m_simMotor != null) {
151            /* First make sure our simulated device gets enabled */
152            AutoFeedEnable.getInstance();
153            m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(m_onPeriodicCallback);
154
155            m_simSupplyVoltage = m_simMotor.createDouble("supplyVoltage", Direction.kInput, 12.0);
156
157            m_simDutyCycle = m_simMotor.createDouble("dutyCycle", Direction.kOutput, 0);
158            m_simMotorVoltage = m_simMotor.createDouble("motorVoltage", Direction.kOutput, 0);
159            m_simTorqueCurrent = m_simMotor.createDouble("torqueCurrent", Direction.kOutput, 0);
160            m_simSupplyCurrent = m_simMotor.createDouble("supplyCurrent", Direction.kOutput, 0);
161
162            final SimDeviceSim sim = new SimDeviceSim("CANMotor:Talon FX (Pro)");
163            m_simValueChangedCallbacks
164                    .add(sim.registerValueChangedCallback(m_simSupplyVoltage, m_onValueChangedCallback, true));
165        }
166        if (m_simRotor != null) {
167            m_simRotorPos = m_simRotor.createDouble("position", Direction.kOutput, 0);
168
169            m_simRotorRawPos = m_simRotor.createDouble("rawPositionInput", Direction.kInput, 0);
170            m_simRotorVel = m_simRotor.createDouble("velocity", Direction.kInput, 0);
171
172            final SimDeviceSim sim = new SimDeviceSim("CANEncoder:" + base + "Rotor Sensor");
173            m_simValueChangedCallbacks
174                    .add(sim.registerValueChangedCallback(m_simRotorRawPos, m_onValueChangedCallback, true));
175            m_simValueChangedCallbacks
176                    .add(sim.registerValueChangedCallback(m_simRotorVel, m_onValueChangedCallback, true));
177        }
178        if (m_simForwardLimit != null) {
179            m_simForwardLimit.createBoolean("init", Direction.kOutput, true);
180            m_simForwardLimit.createBoolean("input", Direction.kOutput, true);
181
182            m_simForwardLimitValue = m_simForwardLimit.createBoolean("value", Direction.kBidir, false);
183
184            final SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "Fwd Limit");
185            m_simValueChangedCallbacks
186                    .add(sim.registerValueChangedCallback(m_simForwardLimitValue, m_onValueChangedCallback, true));
187        }
188        if (m_simReverseLimit != null) {
189            m_simReverseLimit.createBoolean("init", Direction.kOutput, true);
190            m_simReverseLimit.createBoolean("input", Direction.kOutput, true);
191
192            m_simReverseLimitValue = m_simReverseLimit.createBoolean("value", Direction.kBidir, false);
193
194            final SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "Rev Limit");
195            m_simValueChangedCallbacks
196                    .add(sim.registerValueChangedCallback(m_simReverseLimitValue, m_onValueChangedCallback, true));
197        }
198    }
199
200    // ----- Callbacks for Sim ----- //
201    private class OnValueChangedCallback implements SimValueCallback {
202        @Override
203        public void callback(String name, int handle, int direction, HALValue value) {
204            String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle));
205            String physType = deviceName + ":" + name;
206            PlatformJNI.JNI_SimSetPhysicsInput(kSimDeviceType.value, getDeviceID(),
207                    physType, CallbackHelper.getRawValue(value));
208        }
209    }
210
211    private class OnPeriodicCallback implements Runnable {
212        @Override
213        public void run() {
214            double value = 0;
215            int err = 0;
216
217            final int deviceID = getDeviceID();
218
219            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage");
220            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
221            if (err == 0) {
222                m_simSupplyVoltage.set(value);
223            }
224            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "DutyCycle");
225            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
226            if (err == 0) {
227                m_simDutyCycle.set(value);
228            }
229            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "MotorVoltage");
230            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
231            if (err == 0) {
232                m_simMotorVoltage.set(value);
233            }
234            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "TorqueCurrent");
235            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
236            if (err == 0) {
237                m_simTorqueCurrent.set(value);
238            }
239            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyCurrent");
240            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
241            if (err == 0) {
242                m_simSupplyCurrent.set(value);
243            }
244            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RotorPosition");
245            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
246            if (err == 0) {
247                m_simRotorPos.set(value);
248            }
249            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawRotorPosition");
250            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
251            if (err == 0) {
252                m_simRotorRawPos.set(value);
253            }
254            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RotorVelocity");
255            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
256            if (err == 0) {
257                m_simRotorVel.set(value);
258            }
259            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "ForwardLimit");
260            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
261            if (err == 0) {
262                m_simForwardLimitValue.set((int) value != 0);
263            }
264            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "ReverseLimit");
265            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
266            if (err == 0) {
267                m_simReverseLimitValue.set((int) value != 0);
268            }
269        }
270    }
271
272    // ------- AutoCloseable ----- //
273    @Override
274    public void close() {
275        SendableRegistry.remove(this);
276        if (m_simPeriodicBeforeCallback != null) {
277            m_simPeriodicBeforeCallback.close();
278            m_simPeriodicBeforeCallback = null;
279        }
280        if (m_simMotor != null) {
281            m_simMotor.close();
282            m_simMotor = null;
283        }
284        if (m_simRotor != null) {
285            m_simRotor.close();
286            m_simRotor = null;
287        }
288        if (m_simForwardLimit != null) {
289            m_simForwardLimit.close();
290            m_simForwardLimit = null;
291        }
292        if (m_simReverseLimit != null) {
293            m_simReverseLimit.close();
294            m_simReverseLimit = null;
295        }
296
297        for (var callback : m_simValueChangedCallbacks) {
298            callback.close();
299        }
300        m_simValueChangedCallbacks.clear();
301    }
302
303    // ------ set/get routines for WPILIB interfaces ------//
304    /**
305     * Common interface for setting the speed of a motor controller.
306     *
307     * @param speed The speed to set. Value should be between -1.0 and 1.0.
308     */
309    @Override
310    public void set(double speed) {
311        feed();
312        setControl(m_setterControl.withOutput(speed));
313    }
314
315    /**
316     * Common interface for seting the direct voltage output of a motor controller.
317     *
318     * @param volts The voltage to output.
319     */
320    @Override
321    public void setVoltage(double volts) {
322        feed();
323        setControl(m_voltageControl.withOutput(volts));
324    }
325
326    /**
327     * Common interface for getting the current set speed of a motor controller.
328     *
329     * @return The current set speed. Value is between -1.0 and 1.0.
330     */
331    @Override
332    public double get() {
333        return m_dutyCycle.refresh().getValue();
334    }
335
336    // ---------Intercept CTRE calls for motor safety ---------//
337    @Override
338    protected StatusCode setControlPrivate(ControlRequest request) {
339        /* intercept the control setter and feed motor-safety */
340        feed();
341        return super.setControlPrivate(request);
342    }
343
344    // ----------------------- turn-motor-off routines-------------------//
345    /**
346     * Common interface for disabling a motor controller.
347     */
348    @Override
349    public void disable() {
350        setControl(m_neutralControl);
351    }
352
353    /**
354     * Common interface to stop motor movement until set is called again.
355     */
356    @Override
357    public void stopMotor() {
358        disable();
359    }
360
361    // ----------------------- Invert routines -------------------//
362    /**
363     * Common interface for inverting direction of a motor controller.
364     *
365     * @param isInverted The state of inversion, true is inverted.
366     */
367    @Override
368    public void setInverted(boolean isInverted) {
369        /* First read the configs so they're up-to-date */
370        getConfigurator().refresh(m_configs);
371        /* Then set the invert config to the appropriate value */
372        m_configs.Inverted = isInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;
373        getConfigurator().apply(m_configs);
374    }
375
376    /**
377     * Common interface for returning the inversion state of a motor controller.
378     *
379     * @return The state of the inversion, true is inverted.
380     */
381    @Override
382    public boolean getInverted() {
383        /* Read the configs so they're up-to-date */
384        getConfigurator().refresh(m_configs);
385        /* Return true if we're Clockwise_Positive */
386        return m_configs.Inverted == InvertedValue.Clockwise_Positive;
387    }
388
389    // ---- Sendable -------//
390    @Override
391    public void initSendable(SendableBuilder builder) {
392        builder.setSmartDashboardType("Motor Controller");
393        builder.setActuator(true);
394        builder.setSafeState(this::stopMotor);
395        builder.addDoubleProperty("Value", this::get, this::set);
396    }
397
398    /**
399     * @return Description of motor controller
400     */
401    public String getDescription() {
402        return m_description;
403    }
404
405    /* ----- Motor Safety ----- */
406    /** caller must lock appropriately */
407    private MotorSafetyImplem GetMotorSafety() {
408        if (m_motorSafety == null) {
409            /* newly created MS object */
410            m_motorSafety = new MotorSafetyImplem(this, getDescription());
411            /* apply the expiration timeout */
412            m_motorSafety.setExpiration(m_motSafeExpiration);
413        }
414        return m_motorSafety;
415    }
416
417    /**
418     * Feed the motor safety object.
419     * <p>
420     * Resets the timer on this object that is used to do the timeouts.
421     */
422    public void feed() {
423        m_lockMotorSaf.lock();
424        try {
425            if (m_motorSafety == null) {
426                /* do nothing, MS features were never enabled */
427            } else {
428                GetMotorSafety().feed();
429            }
430        } finally {
431            m_lockMotorSaf.unlock();
432        }
433    }
434
435    /**
436     * Set the expiration time for the corresponding motor safety object.
437     *
438     * @param expirationTime The timeout value in seconds.
439     */
440    public void setExpiration(double expirationTime) {
441        m_lockMotorSaf.lock();
442        try {
443            /* save the value for if/when we do create the MS object */
444            m_motSafeExpiration = expirationTime;
445            /* apply it only if MS object exists */
446            if (m_motorSafety == null) {
447                /* do nothing, MS features were never enabled */
448            } else {
449                /* this call will trigger creating a registered MS object */
450                GetMotorSafety().setExpiration(m_motSafeExpiration);
451            }
452        } finally {
453            m_lockMotorSaf.unlock();
454        }
455    }
456
457    /**
458     * Retrieve the timeout value for the corresponding motor safety object.
459     *
460     * @return the timeout value in seconds.
461     */
462    public double getExpiration() {
463        m_lockMotorSaf.lock();
464        try {
465            /* return the intended expiration time */
466            return m_motSafeExpiration;
467        } finally {
468            m_lockMotorSaf.unlock();
469        }
470    }
471
472    /**
473     * Determine of the motor is still operating or has timed out.
474     *
475     * @return a true value if the motor is still operating normally and hasn't
476     *         timed out.
477     */
478    public boolean isAlive() {
479        m_lockMotorSaf.lock();
480        try {
481            if (m_motorSafety == null) {
482                /* MC is alive - MS features were never enabled to neutral the MC. */
483                return true;
484            } else {
485                return GetMotorSafety().isAlive();
486            }
487        } finally {
488            m_lockMotorSaf.unlock();
489        }
490    }
491
492    /**
493     * Enable/disable motor safety for this device.
494     * <p>
495     * Turn on and off the motor safety option for this PWM object.
496     *
497     * @param enabled True if motor safety is enforced for this object
498     */
499    public void setSafetyEnabled(boolean enabled) {
500        m_lockMotorSaf.lock();
501        try {
502            if (m_motorSafety == null && !enabled) {
503                /*
504                 * Caller wants to disable MS,
505                 * but MS features were nevere enabled,
506                 * so it doesn't need to be disabled.
507                 */
508            } else {
509                /* MS will be created if it does not exist */
510                GetMotorSafety().setSafetyEnabled(enabled);
511            }
512        } finally {
513            m_lockMotorSaf.unlock();
514        }
515    }
516
517    /**
518     * Return the state of the motor safety enabled flag.
519     * <p>
520     * Return if the motor safety is currently enabled for this device.
521     *
522     * @return True if motor safety is enforced for this device
523     */
524    public boolean isSafetyEnabled() {
525        m_lockMotorSaf.lock();
526        try {
527            if (m_motorSafety == null) {
528                /* MS features were never enabled. */
529                return false;
530            } else {
531                return GetMotorSafety().isSafetyEnabled();
532            }
533        } finally {
534            m_lockMotorSaf.unlock();
535        }
536    }
537}