001/**
002 * WPI Compliant motor controller class.
003 * WPILIB's object model requires many interfaces to be implemented to use
004 * the various features.
005 * This includes...
006 * - Software PID loops running in the robot controller
007 * - LiveWindow/Test mode features
008 * - Motor Safety (auto-turn off of motor if Set stops getting called)
009 * - Single Parameter set that assumes a simple motor controller.
010 */
011package com.ctre.phoenix.motorcontrol.can;
012
013import com.ctre.phoenix.motorcontrol.ControlMode;
014import com.ctre.phoenix.motorcontrol.DemandType;
015import com.ctre.phoenix.motorcontrol.WPI_AutoFeedEnable;
016import com.ctre.phoenix.motorcontrol.WPI_MotorSafetyImplem;
017import com.ctre.phoenix.platform.DeviceType;
018import com.ctre.phoenix.platform.PlatformJNI;
019import com.ctre.phoenix.Logger;
020import com.ctre.phoenix.WPI_CallbackHelper;
021import com.ctre.phoenix.ErrorCode;
022
023import java.util.ArrayList;
024
025import edu.wpi.first.wpilibj.RobotController;
026import edu.wpi.first.wpilibj.motorcontrol.MotorController;
027import edu.wpi.first.wpilibj.simulation.CallbackStore;
028import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
029import edu.wpi.first.util.sendable.SendableRegistry;
030import edu.wpi.first.wpilibj.DriverStation;
031import edu.wpi.first.util.sendable.Sendable;
032import edu.wpi.first.util.sendable.SendableBuilder;
033import edu.wpi.first.hal.FRCNetComm.tResourceType;
034import edu.wpi.first.hal.SimDevice.Direction;
035import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
036import edu.wpi.first.hal.simulation.SimValueCallback;
037import edu.wpi.first.hal.simulation.SimulatorJNI;
038import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback;
039import edu.wpi.first.hal.HAL;
040import edu.wpi.first.hal.HALValue;
041import edu.wpi.first.hal.SimDevice;
042import edu.wpi.first.hal.SimDouble;
043import edu.wpi.first.hal.SimBoolean;
044
045/**
046 * CTRE Talon FX Motor Controller when used on CAN Bus.
047 *
048 * @deprecated This device's Phoenix 5 API is deprecated for removal in the
049 * 2025 season. Users should update to Phoenix 6 firmware and migrate to the
050 * Phoenix 6 API. A migration guide is available at
051 * https://v6.docs.ctr-electronics.com/en/stable/docs/migration/migration-guide/index.html.
052 * <p>
053 * If the Phoenix 5 API must be used for this device, the device must have 22.X
054 * firmware. This firmware is available in Tuner X after selecting Phoenix 5 in
055 * the firmware year dropdown.
056 */
057@Deprecated(since = "2024", forRemoval = true)
058public class WPI_TalonFX extends TalonFX implements MotorController, Sendable, AutoCloseable {
059
060        private String _description;
061        private double _speed;
062
063        private SimDevice m_simMotor;
064        private SimDouble m_simPercOut;
065        private SimDouble m_simMotorOutputLeadVoltage;
066        private SimDouble m_simSupplyCurrent;
067        private SimDouble m_simMotorCurrent;
068        private SimDouble m_simVbat;
069
070        private SimDevice m_simIntegSens;
071        private SimDouble m_simIntegSensPos;
072        private SimDouble m_simIntegSensAbsPos;
073        private SimDouble m_simIntegSensRawPos;
074        private SimDouble m_simIntegSensVel;
075
076        private SimDevice m_simFwdLim;
077        private SimBoolean m_simFwdLimInit;
078        private SimBoolean m_simFwdLimInput;
079        private SimBoolean m_simFwdLimValue;
080
081        private SimDevice m_simRevLim;
082        private SimBoolean m_simRevLimInit;
083        private SimBoolean m_simRevLimInput;
084        private SimBoolean m_simRevLimValue;
085
086        // callbacks to register
087        private SimValueCallback onValueChangedCallback = new OnValueChangedCallback();
088        private Runnable onPeriodicCallback = new OnPeriodicCallback();
089
090        // returned registered callbacks
091        private ArrayList<CallbackStore> simValueChangedCallbacks = new ArrayList<CallbackStore>();
092        private SimPeriodicBeforeCallback simPeriodicCallback;
093
094        /**
095         * The default motor safety timeout IF calling application
096         * enables the feature.
097         */
098        public static final double kDefaultSafetyExpiration = 0.1;
099
100        /**
101         * Late-constructed motor safety, which ensures feature is off unless calling
102         * applications explicitly enables it.
103         */
104        private WPI_MotorSafetyImplem _motorSafety = null;
105        private final Object _lockMotorSaf = new Object();
106        private double _motSafeExpiration = kDefaultSafetyExpiration;
107
108        /**
109         * Constructor for motor controller
110         * @param deviceNumber device ID of motor controller
111         * @param canbus Name of the CANbus; can be a CANivore device name or serial number.
112         *               Pass in nothing or "rio" to use the roboRIO.
113         */
114        public WPI_TalonFX(int deviceNumber, String canbus) {
115                super(deviceNumber, canbus);
116                _description = "Talon FX " + deviceNumber;
117
118                SendableRegistry.addLW(this, "Talon FX ", deviceNumber);
119
120                m_simMotor = SimDevice.create("CANMotor:Talon FX", deviceNumber);
121                if(m_simMotor != null){
122                        WPI_AutoFeedEnable.getInstance();
123                        simPeriodicCallback = HAL.registerSimPeriodicBeforeCallback(onPeriodicCallback);
124
125                        m_simPercOut = m_simMotor.createDouble("percentOutput", Direction.kOutput, 0);
126                        m_simMotorOutputLeadVoltage = m_simMotor.createDouble("motorOutputLeadVoltage", Direction.kOutput, 0);
127
128                        m_simSupplyCurrent = m_simMotor.createDouble("supplyCurrent", Direction.kInput, 0);
129                        m_simMotorCurrent = m_simMotor.createDouble("motorCurrent", Direction.kInput, 0);
130                        m_simVbat = m_simMotor.createDouble("busVoltage", Direction.kInput, 12.0);
131
132                        SimDeviceSim sim = new SimDeviceSim("CANMotor:Talon FX");
133                        simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simSupplyCurrent, onValueChangedCallback, true));
134                        simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simMotorCurrent, onValueChangedCallback, true));
135                        simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simVbat, onValueChangedCallback, true));
136                }
137
138                String base = "Talon FX[" + deviceNumber + "]/";
139                m_simIntegSens = SimDevice.create("CANEncoder:" + base + "Integrated Sensor");
140                if(m_simIntegSens != null){
141                        m_simIntegSensPos = m_simIntegSens.createDouble("position", Direction.kOutput, 0);
142                        m_simIntegSensAbsPos = m_simIntegSens.createDouble("absolutePosition", Direction.kOutput, 0);
143
144                        m_simIntegSensRawPos = m_simIntegSens.createDouble("rawPositionInput", Direction.kInput, 0);
145                        m_simIntegSensVel = m_simIntegSens.createDouble("velocity", Direction.kInput, 0);
146
147                        SimDeviceSim sim = new SimDeviceSim("CANEncoder:" + base + "Integrated Sensor");
148                        simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simIntegSensRawPos, onValueChangedCallback, true));
149                        simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simIntegSensVel, onValueChangedCallback, true));
150                }
151
152                m_simFwdLim = SimDevice.create("CANDIO:" + base + "Fwd Limit");
153                if(m_simFwdLim != null){
154                        m_simFwdLimInit = m_simFwdLim.createBoolean("init", Direction.kOutput, true);
155                        m_simFwdLimInput = m_simFwdLim.createBoolean("input", Direction.kOutput, true);
156
157                        m_simFwdLimValue = m_simFwdLim.createBoolean("value", Direction.kBidir, false);
158
159                        SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "Fwd Limit");
160                        simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simFwdLimValue, onValueChangedCallback, true));
161                }
162
163                m_simRevLim = SimDevice.create("CANDIO:" + base + "Rev Limit");
164                if(m_simRevLim != null){
165                        m_simRevLimInit = m_simRevLim.createBoolean("init", Direction.kOutput, true);
166                        m_simRevLimInput = m_simRevLim.createBoolean("input", Direction.kOutput, true);
167
168                        m_simRevLimValue = m_simRevLim.createBoolean("value", Direction.kBidir, false);
169
170                        SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "Rev Limit");
171                        simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simRevLimValue, onValueChangedCallback, true));
172                }
173        }
174
175        /**
176         * Constructor for motor controller
177         * @param deviceNumber device ID of motor controller
178         */
179        public WPI_TalonFX(int deviceNumber) {
180                this(deviceNumber, "");
181        }
182
183    // ----- Auto-Closable ----- //
184    @Override
185    public void close(){
186        SendableRegistry.remove(this);
187        if(m_simMotor != null) {
188            m_simMotor.close();
189            m_simMotor = null;
190        }
191        if(m_simIntegSens != null) {
192            m_simIntegSens.close();
193            m_simIntegSens = null;
194        }
195        if(m_simFwdLim != null) {
196            m_simFwdLim.close();
197            m_simFwdLim = null;
198        }
199        if(m_simRevLim != null) {
200            m_simRevLim.close();
201            m_simRevLim = null;
202        }
203        super.DestroyObject(); //Destroy the device
204    }
205
206        // ----- Callbacks for Sim ----- //
207        private class OnValueChangedCallback implements SimValueCallback {
208                @Override
209                public void callback(String name, int handle, int direction, HALValue value) {
210                        String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle));
211                        String physType = deviceName + ":" + name;
212                        PlatformJNI.JNI_SimSetPhysicsInput(DeviceType.TalonFX.value, getDeviceID(),
213                                                                               physType, WPI_CallbackHelper.getRawValue(value));
214                }
215        }
216
217        private class OnPeriodicCallback implements Runnable {
218                @Override
219                public void run() {
220                        double value = 0;
221                        int err = 0;
222
223                        int deviceID = getDeviceID();
224
225                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "PercentOutput");
226                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID);
227                        if (err == 0) {
228                                m_simPercOut.set(value);
229                        }
230                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "MotorOutputLeadVoltage");
231                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID);
232                        if (err == 0) {
233                                m_simMotorOutputLeadVoltage.set(value);
234                        }
235                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "BusVoltage");
236                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID);
237                        if (err == 0) {
238                                m_simVbat.set(value);
239                        }
240                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "CurrentSupply");
241                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID);
242                        if (err == 0) {
243                                m_simSupplyCurrent.set(value);
244                        }
245                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "CurrentStator");
246                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID);
247                        if (err == 0) {
248                                m_simMotorCurrent.set(value);
249                        }
250                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "IntegSensPos");
251                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID);
252                        if (err == 0) {
253                                m_simIntegSensPos.set(value);
254                        }
255                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "IntegSensAbsPos");
256                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID);
257                        if (err == 0) {
258                                m_simIntegSensAbsPos.set(value);
259                        }
260                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "IntegSensRawPos");
261                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID);
262                        if (err == 0) {
263                                m_simIntegSensRawPos.set(value);
264                        }
265                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "IntegSensVel");
266                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID);
267                        if (err == 0) {
268                                m_simIntegSensVel.set(value);
269                        }
270                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "LimitFwd");
271                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID);
272                        if (err == 0) {
273                                m_simFwdLimValue.set((int)value != 0);
274                        }
275                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "LimitRev");
276                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID);
277                        if (err == 0) {
278                                m_simRevLimValue.set((int)value != 0);
279                        }
280                }
281        }
282
283        // ------ set/get routines for WPILIB interfaces ------//
284        /**
285         * Common interface for setting the speed of a simple speed controller.
286         *
287         * @param speed The speed to set.  Value should be between -1.0 and 1.0.
288         *                                                                      Value is also saved for Get().
289         */
290        @Override
291        public void set(double speed) {
292                _speed = speed;
293                set(ControlMode.PercentOutput, _speed);
294                feed();
295        }
296
297        /**
298         * Common interface for getting the current set speed of a speed controller.
299         *
300         * @return The current set speed. Value is between -1.0 and 1.0.
301         */
302        @Override
303        public double get() {
304                return _speed;
305        }
306
307        // ---------Intercept CTRE calls for motor safety ---------//
308        /**
309         * Sets the appropriate output on the talon, depending on the mode.
310         * @param mode The output mode to apply.
311         * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
312         * In Current mode, output value is in amperes.
313         * In Velocity mode, output value is in position change / 100ms.
314         * In Position mode, output value is in encoder ticks or an analog value,
315         *   depending on the sensor.
316         * In Follower mode, the output value is the integer device ID of the talon to
317         * duplicate.
318         *
319         * @param value The setpoint value, as described above.
320         *
321         *
322         *      Standard Driving Example:
323         *      _talonLeft.set(ControlMode.PercentOutput, leftJoy);
324         *      _talonRght.set(ControlMode.PercentOutput, rghtJoy);
325         */
326        public void set(ControlMode mode, double value) {
327                /* intercept the advanced Set and feed motor-safety */
328                super.set(mode, value);
329                feed();
330        }
331
332        /**
333         * @param mode Sets the appropriate output on the talon, depending on the mode.
334         * @param demand0 The output value to apply.
335         *      such as advanced feed forward and/or auxiliary close-looping in firmware.
336         * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
337         * In Current mode, output value is in amperes.
338         * In Velocity mode, output value is in position change / 100ms.
339         * In Position mode, output value is in encoder ticks or an analog value,
340         *   depending on the sensor. See
341         * In Follower mode, the output value is the integer device ID of the talon to
342         * duplicate.
343         *
344         * @param demand1Type The demand type for demand1.
345         * Neutral: Ignore demand1 and apply no change to the demand0 output.
346         * AuxPID: Use demand1 to set the target for the auxiliary PID 1.
347         * ArbitraryFeedForward: Use demand1 as an arbitrary additive value to the
348         *       demand0 output.  In PercentOutput the demand0 output is the motor output,
349         *   and in closed-loop modes the demand0 output is the output of PID0.
350         * @param demand1 Supplmental output value.  Units match the set mode.
351         *
352         *
353         *  Arcade Drive Example:
354         *              _talonLeft.set(ControlMode.PercentOutput, joyForward, DemandType.ArbitraryFeedForward, +joyTurn);
355         *              _talonRght.set(ControlMode.PercentOutput, joyForward, DemandType.ArbitraryFeedForward, -joyTurn);
356         *
357         *      Drive Straight Example:
358         *      Note: Selected Sensor Configuration is necessary for both PID0 and PID1.
359         *              _talonLeft.follow(_talonRght, FollwerType.AuxOutput1);
360         *              _talonRght.set(ControlMode.PercentOutput, joyForward, DemandType.AuxPID, desiredRobotHeading);
361         *
362         *      Drive Straight to a Distance Example:
363         *      Note: Other configurations (sensor selection, PID gains, etc.) need to be set.
364         *              _talonLeft.follow(_talonRght, FollwerType.AuxOutput1);
365         *              _talonRght.set(ControlMode.MotionMagic, targetDistance, DemandType.AuxPID, desiredRobotHeading);
366         */
367        public void set(ControlMode mode, double demand0, DemandType demand1Type, double demand1){
368                /* intercept the advanced Set and feed motor-safety */
369                super.set(mode, demand0, demand1Type, demand1);
370                feed();
371        }
372
373        /**
374         * Sets the voltage output of the SpeedController.  Compensates for the current bus
375         * voltage to ensure that the desired voltage is output even if the battery voltage is below
376         * 12V - highly useful when the voltage outputs are "meaningful" (e.g. they come from a
377         * feedforward calculation).
378         *
379         * <p>NOTE: This function *must* be called regularly in order for voltage compensation to work
380         * properly - unlike the ordinary set function, it is not "set it and forget it."
381         *
382         * @param outputVolts The voltage to output.
383         */
384        @Override
385        public void setVoltage(double outputVolts) {
386                if(super.isVoltageCompensationEnabled())
387                {
388                        com.ctre.phoenix.Logger.log(ErrorCode.DoubleVoltageCompensatingWPI, _description + ": setVoltage ");
389                }
390                set(outputVolts / RobotController.getBatteryVoltage());
391        }
392
393        // ----------------------- Invert routines -------------------//
394        /**
395         * Common interface for inverting direction of a speed controller.
396         *
397         * @param isInverted The state of inversion, true is inverted.
398         */
399        @Override
400        public void setInverted(boolean isInverted) {
401                super.setInverted(isInverted);
402        }
403
404        /**
405         * Common interface for returning the inversion state of a speed controller.
406         *
407         * @return The state of inversion, true is inverted.
408         */
409        @Override
410        public boolean getInverted() {
411                return super.getInverted();
412        }
413
414        // ----------------------- turn-motor-off routines-------------------//
415        /**
416         * Common interface for disabling a motor.
417         */
418        @Override
419        public void disable() {
420                neutralOutput();
421        }
422
423        /**
424         * Common interface to stop the motor until Set is called again.
425         */
426        @Override
427        public void stopMotor() {
428                neutralOutput();
429        }
430
431        // ---- Sendable -------//
432
433        /**
434         * Initialize sendable
435         * @param builder Base sendable to build on
436         */
437        @Override
438        public void initSendable(SendableBuilder builder) {
439                builder.setSmartDashboardType("Motor Controller");
440                builder.setActuator(true);
441                builder.setSafeState(this::stopMotor);
442                builder.addDoubleProperty("Value", this::get, this::set);
443        }
444
445        /**
446         * @return description of controller
447         */
448        public String getDescription() {
449                return _description;
450        }
451
452        /* ----- Motor Safety ----- */
453        /** caller must lock appropriately */
454        private WPI_MotorSafetyImplem GetMotorSafety() {
455                if (_motorSafety == null) {
456                        /* newly created MS object */
457                        _motorSafety = new WPI_MotorSafetyImplem(this, getDescription());
458                        /* apply the expiration timeout */
459                        _motorSafety.setExpiration(_motSafeExpiration);
460                }
461                return _motorSafety;
462        }
463        /**
464         * Feed the motor safety object.
465         *
466         * <p>Resets the timer on this object that is used to do the timeouts.
467         */
468        public void feed() {
469                synchronized (_lockMotorSaf) {
470                        if (_motorSafety == null) {
471                                /* do nothing, MS features were never enabled */
472                        } else {
473                                GetMotorSafety().feed();
474                        }
475                }
476        }
477
478        /**
479         * Set the expiration time for the corresponding motor safety object.
480         *
481         * @param expirationTime The timeout value in seconds.
482         */
483        public void setExpiration(double expirationTime) {
484                synchronized (_lockMotorSaf) {
485                        /* save the value for if/when we do create the MS object */
486                        _motSafeExpiration = expirationTime;
487                        /* apply it only if MS object exists */
488                        if (_motorSafety == null) {
489                                /* do nothing, MS features were never enabled */
490                        } else {
491                                /* this call will trigger creating a registered MS object */
492                                GetMotorSafety().setExpiration(_motSafeExpiration);
493                        }
494                }
495        }
496
497        /**
498         * Retrieve the timeout value for the corresponding motor safety object.
499         *
500         * @return the timeout value in seconds.
501         */
502        public double getExpiration() {
503                synchronized (_lockMotorSaf) {
504                        /* return the intended expiration time */
505                        return _motSafeExpiration;
506                }
507        }
508
509        /**
510         * Determine of the motor is still operating or has timed out.
511         *
512         * @return a true value if the motor is still operating normally and hasn't timed out.
513         */
514        public boolean isAlive() {
515                synchronized (_lockMotorSaf) {
516                        if (_motorSafety == null) {
517                                /* MC is alive - MS features were never enabled to neutral the MC. */
518                                return true;
519                        } else {
520                                return GetMotorSafety().isAlive();
521                        }
522                }
523        }
524
525        /**
526         * Enable/disable motor safety for this device.
527         *
528         * <p>Turn on and off the motor safety option for this PWM object.
529         *
530         * @param enabled True if motor safety is enforced for this object
531         */
532        public void setSafetyEnabled(boolean enabled) {
533                synchronized (_lockMotorSaf) {
534                        if (_motorSafety == null && !enabled) {
535                                /* Caller wants to disable MS,
536                                        but MS features were nevere enabled,
537                                        so it doesn't need to be disabled. */
538                        } else {
539                                /* MS will be created if it does not exist */
540                                GetMotorSafety().setSafetyEnabled(enabled);
541                        }
542                }
543        }
544
545        /**
546         * Return the state of the motor safety enabled flag.
547         *
548         * <p>Return if the motor safety is currently enabled for this device.
549         *
550         * @return True if motor safety is enforced for this device
551         */
552        public boolean isSafetyEnabled() {
553                synchronized (_lockMotorSaf) {
554                        if (_motorSafety == null) {
555                                /* MS features were never enabled. */
556                                return false;
557                        } else {
558                                return GetMotorSafety().isSafetyEnabled();
559                        }
560                }
561        }
562
563        private void timeoutFunc() {
564                if (DriverStation.isDisabled() || DriverStation.isTest()) {
565                return;
566                }
567
568                DriverStation.reportError(getDescription() + "... Output not updated often enough.", false);
569
570                stopMotor();
571        }
572}