Source code

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.mechanisms;
008
009import java.util.Optional;
010import java.util.concurrent.atomic.AtomicBoolean;
011import java.util.function.BooleanSupplier;
012
013import com.ctre.phoenix6.StatusCode;
014import com.ctre.phoenix6.configs.DifferentialSensorsConfigs;
015import com.ctre.phoenix6.controls.DifferentialFollower;
016import com.ctre.phoenix6.controls.NeutralOut;
017import com.ctre.phoenix6.controls.CoastOut;
018import com.ctre.phoenix6.controls.StaticBrake;
019import com.ctre.phoenix6.hardware.CANcoder;
020import com.ctre.phoenix6.hardware.Pigeon2;
021import com.ctre.phoenix6.hardware.TalonFX;
022import com.ctre.phoenix6.signals.DifferentialSensorSourceValue;
023
024import com.ctre.phoenix6.controls.DifferentialDutyCycle;
025import com.ctre.phoenix6.controls.DifferentialMotionMagicDutyCycle;
026import com.ctre.phoenix6.controls.DifferentialMotionMagicVoltage;
027import com.ctre.phoenix6.controls.DifferentialPositionDutyCycle;
028import com.ctre.phoenix6.controls.DifferentialPositionVoltage;
029import com.ctre.phoenix6.controls.DifferentialVelocityDutyCycle;
030import com.ctre.phoenix6.controls.DifferentialVelocityVoltage;
031import com.ctre.phoenix6.controls.DifferentialVoltage;
032
033/**
034 * Manages control of a simple two-axis differential mechanism.
035 *
036 * This mechanism provides limited differential functionality. Pro users
037 * on a CAN FD bus can use the {@link DifferentialMechanism} class instead
038 * for full functionality.
039 */
040public class SimpleDifferentialMechanism {
041    /**
042     * Sensor sources for a differential Pigeon 2.
043     */
044    public enum DifferentialPigeon2Source {
045        Yaw,
046        Pitch,
047        Roll
048    }
049
050    /**
051     * Possible reasons for the mechanism to disable.
052     */
053    public enum DisabledReason {
054        /**
055         * No reason given.
056         */
057        None,
058        /**
059         * A remote sensor is not present on CAN Bus.
060         */
061        MissingRemoteSensor,
062        /**
063         * The remote Talon FX used for differential
064         * control is not present on CAN Bus.
065         */
066        MissingDifferentialFX,
067        /**
068         * A remote sensor position has overflowed. Because of the nature
069         * of remote sensors, it is possible for a remote sensor position
070         * to overflow beyond what is supported by the status signal frame.
071         * However, this is rare and cannot occur over the course of an FRC
072         * match under normal use.
073         */
074        RemoteSensorPosOverflow,
075        /**
076         * A device or remote sensor has reset.
077         */
078        DeviceHasReset,
079    }
080
081    /**
082     * Possible reasons for the mechanism to require
083     * user action to resume control.
084     */
085    public enum RequiresUserReason {
086        /**
087         * No reason given.
088         */
089        None,
090        /**
091         * A remote sensor position has overflowed. Because of the nature
092         * of remote sensors, it is possible for a remote sensor position
093         * to overflow beyond what is supported by the status signal frame.
094         * However, this is rare and cannot occur over the course of an FRC
095         * match under normal use.
096         */
097        RemoteSensorPosOverflow,
098        /**
099         * A device or remote sensor has reset.
100         */
101        DeviceHasReset,
102    }
103
104    /**
105     * The default number of retries for config applies.
106     */
107    public static final int kDefaultConfigRetries = 5;
108
109    private final TalonFX _diffAddFX;
110    private final TalonFX _diffSubFX;
111    private final Optional<Pigeon2> _pigeon2;
112    private final DifferentialPigeon2Source _pigeonSource;
113    private final Optional<CANcoder> _cancoder;
114
115    private final DifferentialFollower _diffFollow;
116
117    private final NeutralOut _neutral = new NeutralOut();
118    private final CoastOut _coast = new CoastOut();
119    private final StaticBrake _brake = new StaticBrake();
120
121    private final BooleanSupplier _diffAddFXResetChecker;
122    private final BooleanSupplier _diffSubFXResetChecker;
123    private final Optional<BooleanSupplier> _pigeon2ResetChecker;
124    private final Optional<BooleanSupplier> _cancoderResetChecker;
125
126    private boolean _hasAppliedConfigs = false;
127
128    private final AtomicBoolean _mechanismDisabled = new AtomicBoolean(false);
129    private final AtomicBoolean _requiresUserAction = new AtomicBoolean(false);
130
131    private DisabledReason _disabledReason = DisabledReason.None;
132    private RequiresUserReason _requiresUserReason = RequiresUserReason.None;
133
134    /**
135     * Creates a new simple differential mechanism using the given two {@link TalonFX} devices.
136     * The mechanism will use the average of the two Talon FX sensors on the primary axis,
137     * and the difference between the two Talon FX sensors on the differential axis.
138     * <p>
139     * This mechanism provides limited differential functionality. Pro users on a CAN FD
140     * bus can use the {@link DifferentialMechanism} class instead for full functionality.
141     *
142     * @param differentialAddFX The Talon FX that will have the differential output added to its regular output.
143     * @param differentialSubFX The Talon FX that will have the differential output subtracted from its regular output.
144     * @param motorDirectionsAlign Whether the differential motors' directions are aligned.
145     */
146    public SimpleDifferentialMechanism(TalonFX differentialAddFX, TalonFX differentialSubFX, boolean motorDirectionsAlign)
147    {
148        _diffAddFX = differentialAddFX;
149        _diffSubFX = differentialSubFX;
150        _pigeon2 = Optional.empty();
151        _pigeonSource = null;
152        _cancoder = Optional.empty();
153        _diffFollow = new DifferentialFollower(_diffAddFX.getDeviceID(), !motorDirectionsAlign);
154        _diffAddFXResetChecker = _diffAddFX.getResetOccurredChecker();
155        _diffSubFXResetChecker = _diffSubFX.getResetOccurredChecker();
156        _pigeon2ResetChecker = Optional.empty();
157        _cancoderResetChecker = Optional.empty();
158    }
159
160    /**
161     * Creates a new simple differential mechanism using the given two {@link TalonFX} devices and
162     * a {@link Pigeon2}. The mechanism will use the average of the two Talon FX sensors on the
163     * primary axis, and the selected Pigeon 2 sensor source on the differential axis.
164     * <p>
165     * This mechanism provides limited differential functionality. Pro users on a CAN FD
166     * bus can use the {@link DifferentialMechanism} class instead for full functionality.
167     *
168     * @param differentialAddFX The Talon FX that will have the differential output added to its regular output.
169     * @param differentialSubFX The Talon FX that will have the differential output subtracted from its regular output.
170     * @param motorDirectionsAlign Whether the differential motors' directions are aligned.
171     * @param pigeon2 The Pigeon 2 to use for the differential axis.
172     * @param pigeonSource The sensor source to use for the Pigeon 2 (Yaw, Pitch, or Roll).
173     */
174    public SimpleDifferentialMechanism(TalonFX differentialAddFX, TalonFX differentialSubFX, boolean motorDirectionsAlign, Pigeon2 pigeon2, DifferentialPigeon2Source pigeonSource)
175    {
176        _diffAddFX = differentialAddFX;
177        _diffSubFX = differentialSubFX;
178        _pigeon2 = Optional.of(pigeon2);
179        _pigeonSource = pigeonSource;
180        _cancoder = Optional.empty();
181        _diffFollow = new DifferentialFollower(_diffAddFX.getDeviceID(), !motorDirectionsAlign);
182        _diffAddFXResetChecker = _diffAddFX.getResetOccurredChecker();
183        _diffSubFXResetChecker = _diffSubFX.getResetOccurredChecker();
184        _pigeon2ResetChecker = Optional.of(_pigeon2.get().getResetOccurredChecker());
185        _cancoderResetChecker = Optional.empty();
186    }
187
188    /**
189     * Creates a new simple differential mechanism using the given two {@link TalonFX} devices and
190     * a {@link CANcoder}. The mechanism will use the average of the two Talon FX sensors on the
191     * primary axis, and the CANcoder position/velocity on the differential axis.
192     * <p>
193     * This mechanism provides limited differential functionality. Pro users on a CAN FD
194     * bus can use the {@link DifferentialMechanism} class instead for full functionality.
195     *
196     * @param differentialAddFX The Talon FX that will have the differential output added to its regular output.
197     * @param differentialSubFX The Talon FX that will have the differential output subtracted from its regular output.
198     * @param motorDirectionsAlign Whether the differential motors' directions are aligned.
199     * @param cancoder The CANcoder to use for the differential axis.
200     */
201    public SimpleDifferentialMechanism(TalonFX differentialAddFX, TalonFX differentialSubFX, boolean motorDirectionsAlign, CANcoder cancoder)
202    {
203        _diffAddFX = differentialAddFX;
204        _diffSubFX = differentialSubFX;
205        _pigeon2 = Optional.empty();
206        _pigeonSource = null;
207        _cancoder = Optional.of(cancoder);
208        _diffFollow = new DifferentialFollower(_diffAddFX.getDeviceID(), !motorDirectionsAlign);
209        _diffAddFXResetChecker = _diffAddFX.getResetOccurredChecker();
210        _diffSubFXResetChecker = _diffSubFX.getResetOccurredChecker();
211        _pigeon2ResetChecker = Optional.empty();
212        _cancoderResetChecker = Optional.of(_cancoder.get().getResetOccurredChecker());
213    }
214
215    /**
216     * Get the Talon FX that is differential leader. The differential
217     * leader calculates the output for the differential follower. The
218     * differential leader is also useful for fault detection, and it
219     * reports status signals for the differential controller.
220     *
221     * @return Differential leader Talon FX
222     */
223    public TalonFX getDifferentialLeader()
224    {
225        return _diffAddFX;
226    }
227
228    /**
229     * Get the Talon FX that is differential follower. The differential
230     * follower's position and velocity are used by the differential leader
231     * for the differential controller.
232     *
233     * @return Differential follower Talon FX
234     */
235    public TalonFX getDifferentialFollower()
236    {
237        return _diffSubFX;
238    }
239
240    /**
241     * Apply the mechanism configs to the devices. This should be
242     * called after applying all other configs to the devices.
243     * <p>
244     * If the user does not call this function by the time SetControl
245     * is called, SetControl will apply the configs once.
246     * <p>
247     * This function defaults to retrying up to {@link #kDefaultConfigRetries}.
248     *
249     * @return Status Code of the config applies.
250     */
251    public StatusCode applyConfigs() {
252        return applyConfigs(kDefaultConfigRetries);
253    }
254
255    /**
256     * Apply the mechanism configs to the devices. This should be
257     * called after applying all other configs to the devices.
258     * <p>
259     * If the user does not call this function by the time SetControl
260     * is called, SetControl will apply the configs once.
261     *
262     * @param numRetries Number of retries when applying the configs
263     * @return Status Code of the config applies.
264     */
265    public StatusCode applyConfigs(int numRetries)
266    {
267        StatusCode retval = StatusCode.OK;
268
269        if (numRetries < 1) numRetries = 1;
270
271        /*
272         * The onboard differential controller adds the differential output to its own output
273         * and subtracts the differential output for differential followers, so use _diffAddFX
274         * as the primary controller.
275         */
276        final var diff_cfg = new DifferentialSensorsConfigs();
277
278        /* disable differential control on _diffSubFX */
279        {
280            StatusCode _diffSubFX_retval = StatusCode.OK;
281            for (int i = 0; i < numRetries; ++i) {
282                _diffSubFX_retval = _diffSubFX.getConfigurator().apply(diff_cfg);
283                if (_diffSubFX_retval.isOK()) break;
284            }
285            if (retval.isOK()) {
286                retval = _diffSubFX_retval;
287            }
288        }
289
290        /* set up differential control on _diffAddFX */
291        diff_cfg.DifferentialTalonFXSensorID = _diffSubFX.getDeviceID();
292        if (_pigeon2.isPresent()) {
293            /* use the Pigeon 2 for differential control */
294            switch (_pigeonSource) {
295                case Yaw:
296                default:
297                    diff_cfg.DifferentialSensorSource = DifferentialSensorSourceValue.RemotePigeon2_Yaw;
298                    break;
299                case Pitch:
300                    diff_cfg.DifferentialSensorSource = DifferentialSensorSourceValue.RemotePigeon2_Pitch;
301                    break;
302                case Roll:
303                    diff_cfg.DifferentialSensorSource = DifferentialSensorSourceValue.RemotePigeon2_Roll;
304                    break;
305            }
306            diff_cfg.DifferentialRemoteSensorID = _pigeon2.get().getDeviceID();
307        } else if (_cancoder.isPresent()) {
308            /* use the CANcoder for differential control */
309            diff_cfg.DifferentialSensorSource = DifferentialSensorSourceValue.RemoteCANcoder;
310            diff_cfg.DifferentialRemoteSensorID = _cancoder.get().getDeviceID();
311        } else {
312            /* use the difference between the two Talon FXs for differential control */
313            diff_cfg.DifferentialSensorSource = DifferentialSensorSourceValue.RemoteTalonFX_Diff;
314        }
315
316        {
317            StatusCode _diffAddFX_retval = StatusCode.OK;
318            for (int i = 0; i < numRetries; ++i) {
319                _diffAddFX_retval = _diffAddFX.getConfigurator().apply(diff_cfg);
320                if (_diffAddFX_retval.isOK()) break;
321            }
322            if (retval.isOK()) {
323                retval = _diffAddFX_retval;
324            }
325        }
326
327        if (retval.isOK()) {
328            _hasAppliedConfigs = true;
329        }
330
331        return retval;
332    }
333
334    /**
335     * Call this method periodically to keep the mechanism state updated.
336     */
337    public void periodic()
338    {
339        StatusCode retval = StatusCode.OK;
340
341        /* handle remote sensor position overflow fault */
342        if (_diffAddFX.getFault_RemoteSensorPosOverflow().getValue()) {
343            /* fault the mechanism until the user clears it manually */
344            _requiresUserReason = RequiresUserReason.RemoteSensorPosOverflow;
345            _requiresUserAction.setRelease(true);
346
347            _disabledReason = DisabledReason.RemoteSensorPosOverflow;
348            retval = StatusCode.MechanismFaulted;
349        }
350
351        /* handle missing remote sensor fault */
352        if (_diffAddFX.getFault_RemoteSensorDataInvalid().getValue() ||
353            _diffSubFX.getFault_RemoteSensorDataInvalid().getValue()
354        ) {
355            /* temporarily fault the mechanism while the fault is active */
356            _disabledReason = DisabledReason.MissingRemoteSensor;
357            retval = StatusCode.MechanismFaulted;
358        }
359        /* handle missing differential Talon FX fault */
360        if (_diffAddFX.getFault_MissingDifferentialFX().getValue()) {
361            /* temporarily fault the mechanism while the fault is active */
362            _disabledReason = DisabledReason.MissingDifferentialFX;
363            retval = StatusCode.MechanismFaulted;
364        }
365
366        /* handle if any of the devices have power cycled */
367        final boolean diffAddFX_hasReset = _diffAddFXResetChecker.getAsBoolean();
368        final boolean diffSubFX_hasReset = _diffSubFXResetChecker.getAsBoolean();
369        final boolean pigeon2_hasReset = _pigeon2ResetChecker.isPresent() && _pigeon2ResetChecker.get().getAsBoolean();
370        final boolean cancoder_hasReset = _cancoderResetChecker.isPresent() && _cancoderResetChecker.get().getAsBoolean();
371        final boolean diffAddFX_remsens_hasReset = _diffAddFX.getStickyFault_RemoteSensorReset().getValue();
372        final boolean diffSubFX_remsens_hasReset = _diffSubFX.getStickyFault_RemoteSensorReset().getValue();
373
374        if (diffAddFX_hasReset || diffSubFX_hasReset ||
375            pigeon2_hasReset || cancoder_hasReset ||
376            diffAddFX_remsens_hasReset || diffSubFX_remsens_hasReset
377        ) {
378            /* fault the mechanism until the user clears it manually */
379            _requiresUserReason = RequiresUserReason.DeviceHasReset;
380            _requiresUserAction.setRelease(true);
381
382            _disabledReason = DisabledReason.DeviceHasReset;
383            retval = StatusCode.MechanismFaulted;
384        }
385
386        if (retval.isOK() && _requiresUserAction.getOpaque()) {
387            /* keep the mechanism faulted until user clears the fault */
388            retval = StatusCode.MechanismFaulted;
389        }
390
391        if (!retval.isOK()) {
392            /* disable the mechanism */
393            _mechanismDisabled.setRelease(true);
394        } else {
395            /* re-enable the mechanism */
396            _disabledReason = DisabledReason.None;
397            _mechanismDisabled.setRelease(false);
398        }
399    }
400
401    /**
402     * Get whether the mechanism is currently disabled due to an issue.
403     *
404     * @return true if the mechanism is temporarily disabled
405     */
406    public boolean isDisabled()
407    {
408        return _mechanismDisabled.getAcquire();
409    }
410
411    /**
412     * Get whether the mechanism is currently disabled and requires
413     * user action to re-enable mechanism control.
414     *
415     * @return true if the mechanism is disabled and the user must manually
416     *         perform an action
417     */
418    public boolean requiresUserAction()
419    {
420        return _requiresUserAction.getAcquire();
421    }
422
423    /**
424     * Gets the state of the mechanism.
425     *
426     * @return MechanismState representing the state of the mechanism
427     */
428    public MechanismState getMechanismState()
429    {
430        if (requiresUserAction()) {
431            return MechanismState.RequiresUserAction;
432        } else if (isDisabled()) {
433            return MechanismState.Disabled;
434        } else {
435            return MechanismState.OK;
436        }
437    }
438
439    /**
440     * Indicate to the mechanism that the user has performed the required
441     * action to resume mechanism control.
442     */
443    public void clearUserRequirement()
444    {
445        if (_diffAddFX.getStickyFault_RemoteSensorReset().getValue()) {
446            _diffAddFX.clearStickyFault_RemoteSensorReset();
447        }
448        if (_diffSubFX.getStickyFault_RemoteSensorReset().getValue()) {
449            _diffSubFX.clearStickyFault_RemoteSensorReset();
450        }
451        _requiresUserReason = RequiresUserReason.None;
452        _requiresUserAction.setRelease(false);
453    }
454
455    /**
456     * @return The reason for the mechanism being disabled
457     */
458    public DisabledReason getDisabledReason()
459    {
460        return _disabledReason;
461    }
462    /**
463     * @return The reason for the mechanism requiring user
464     *         action to resume control
465     */
466    public RequiresUserReason getRequiresUserReason()
467    {
468        return _requiresUserReason;
469    }
470
471    private StatusCode beforeControl()
472    {
473        StatusCode retval = StatusCode.OK;
474        if (!_hasAppliedConfigs) {
475            retval = applyConfigs();
476        }
477
478        if (retval.isOK() && _mechanismDisabled.getOpaque()) {
479            /* disable the mechanism */
480            retval = StatusCode.MechanismFaulted;
481        }
482
483        if (!retval.isOK()) {
484            /* neutral the output */
485            setNeutralOut();
486        }
487        return retval;
488    }
489
490    /**
491     * Request neutral output of mechanism. The applied brake type
492     * is determined by the NeutralMode configuration of each device.
493     * <p>
494     * Since the NeutralMode configuration of devices may not align, users
495     * may prefer to use the {@link #setCoastOut()} or {@link #setStaticBrake()} method.
496     *
497     * @return Status Code of the request.
498     */
499    public StatusCode setNeutralOut()
500    {
501        var retval = StatusCode.OK;
502        {
503            final var _diffAddFX_retval = _diffAddFX.setControl(_neutral);
504            if (retval.isOK()) {
505                retval = _diffAddFX_retval;
506            }
507        }
508        {
509            final var _diffSubFX_retval = _diffSubFX.setControl(_neutral);
510            if (retval.isOK()) {
511                retval = _diffSubFX_retval;
512            }
513        }
514        return retval;
515    }
516
517    /**
518     * Request coast neutral output of mechanism. The bridge is
519     * disabled and the rotor is allowed to coast.
520     *
521     * @return Status Code of the request.
522     */
523    public StatusCode setCoastOut()
524    {
525        var retval = StatusCode.OK;
526        {
527            final var _diffAddFX_retval = _diffAddFX.setControl(_coast);
528            if (retval.isOK()) {
529                retval = _diffAddFX_retval;
530            }
531        }
532        {
533            final var _diffSubFX_retval = _diffSubFX.setControl(_coast);
534            if (retval.isOK()) {
535                retval = _diffSubFX_retval;
536            }
537        }
538        return retval;
539    }
540
541    /**
542     * Applies full neutral-brake on the mechanism by shorting
543     * motor leads together.
544     *
545     * @return Status Code of the request.
546     */
547    public StatusCode setStaticBrake()
548    {
549        var retval = StatusCode.OK;
550        {
551            final var _diffAddFX_retval = _diffAddFX.setControl(_brake);
552            if (retval.isOK()) {
553                retval = _diffAddFX_retval;
554            }
555        }
556        {
557            final var _diffSubFX_retval = _diffSubFX.setControl(_brake);
558            if (retval.isOK()) {
559                retval = _diffSubFX_retval;
560            }
561        }
562        return retval;
563    }
564
565    
566    
567    /**
568     * Sets the control request for this mechanism.
569     *
570     * @param _diffAddFXRequest    Request a specified motor duty cycle with a
571     *                             differential position closed-loop.
572     * @return Status Code of the request.
573     */
574    public StatusCode setControl(DifferentialDutyCycle _diffAddFXRequest)
575    {
576        var retval = StatusCode.OK;
577    
578        retval = beforeControl();
579        if (!retval.isOK()) {
580            return retval;
581        }
582        
583        {    
584            final var _diffAddFX_reqPtr = _diffAddFXRequest;
585            
586            final var _diffAddFX_retval = _diffAddFX.setControl(_diffAddFX_reqPtr);
587            if (retval.isOK()) {
588                retval = _diffAddFX_retval;
589            }
590        }
591        
592        {
593            final var _diffSubFX_retval = _diffSubFX.setControl(_diffFollow);
594            if (retval.isOK()) {
595                retval = _diffSubFX_retval;
596            }
597        }
598        
599        return retval;
600    }
601    
602    /**
603     * Sets the control request for this mechanism.
604     *
605     * @param _diffAddFXRequest    Request a specified voltage with a differential
606     *                             position closed-loop.
607     * @return Status Code of the request.
608     */
609    public StatusCode setControl(DifferentialVoltage _diffAddFXRequest)
610    {
611        var retval = StatusCode.OK;
612    
613        retval = beforeControl();
614        if (!retval.isOK()) {
615            return retval;
616        }
617        
618        {    
619            final var _diffAddFX_reqPtr = _diffAddFXRequest;
620            
621            final var _diffAddFX_retval = _diffAddFX.setControl(_diffAddFX_reqPtr);
622            if (retval.isOK()) {
623                retval = _diffAddFX_retval;
624            }
625        }
626        
627        {
628            final var _diffSubFX_retval = _diffSubFX.setControl(_diffFollow);
629            if (retval.isOK()) {
630                retval = _diffSubFX_retval;
631            }
632        }
633        
634        return retval;
635    }
636    
637    /**
638     * Sets the control request for this mechanism.
639     *
640     * @param _diffAddFXRequest    Request PID to target position with a
641     *                             differential position setpoint.
642     * @return Status Code of the request.
643     */
644    public StatusCode setControl(DifferentialPositionDutyCycle _diffAddFXRequest)
645    {
646        var retval = StatusCode.OK;
647    
648        retval = beforeControl();
649        if (!retval.isOK()) {
650            return retval;
651        }
652        
653        {    
654            final var _diffAddFX_reqPtr = _diffAddFXRequest;
655            
656            final var _diffAddFX_retval = _diffAddFX.setControl(_diffAddFX_reqPtr);
657            if (retval.isOK()) {
658                retval = _diffAddFX_retval;
659            }
660        }
661        
662        {
663            final var _diffSubFX_retval = _diffSubFX.setControl(_diffFollow);
664            if (retval.isOK()) {
665                retval = _diffSubFX_retval;
666            }
667        }
668        
669        return retval;
670    }
671    
672    /**
673     * Sets the control request for this mechanism.
674     *
675     * @param _diffAddFXRequest    Request PID to target position with a
676     *                             differential position setpoint
677     * @return Status Code of the request.
678     */
679    public StatusCode setControl(DifferentialPositionVoltage _diffAddFXRequest)
680    {
681        var retval = StatusCode.OK;
682    
683        retval = beforeControl();
684        if (!retval.isOK()) {
685            return retval;
686        }
687        
688        {    
689            final var _diffAddFX_reqPtr = _diffAddFXRequest;
690            
691            final var _diffAddFX_retval = _diffAddFX.setControl(_diffAddFX_reqPtr);
692            if (retval.isOK()) {
693                retval = _diffAddFX_retval;
694            }
695        }
696        
697        {
698            final var _diffSubFX_retval = _diffSubFX.setControl(_diffFollow);
699            if (retval.isOK()) {
700                retval = _diffSubFX_retval;
701            }
702        }
703        
704        return retval;
705    }
706    
707    /**
708     * Sets the control request for this mechanism.
709     *
710     * @param _diffAddFXRequest    Request PID to target velocity with a
711     *                             differential position setpoint.
712     * @return Status Code of the request.
713     */
714    public StatusCode setControl(DifferentialVelocityDutyCycle _diffAddFXRequest)
715    {
716        var retval = StatusCode.OK;
717    
718        retval = beforeControl();
719        if (!retval.isOK()) {
720            return retval;
721        }
722        
723        {    
724            final var _diffAddFX_reqPtr = _diffAddFXRequest;
725            
726            final var _diffAddFX_retval = _diffAddFX.setControl(_diffAddFX_reqPtr);
727            if (retval.isOK()) {
728                retval = _diffAddFX_retval;
729            }
730        }
731        
732        {
733            final var _diffSubFX_retval = _diffSubFX.setControl(_diffFollow);
734            if (retval.isOK()) {
735                retval = _diffSubFX_retval;
736            }
737        }
738        
739        return retval;
740    }
741    
742    /**
743     * Sets the control request for this mechanism.
744     *
745     * @param _diffAddFXRequest    Request PID to target velocity with a
746     *                             differential position setpoint.
747     * @return Status Code of the request.
748     */
749    public StatusCode setControl(DifferentialVelocityVoltage _diffAddFXRequest)
750    {
751        var retval = StatusCode.OK;
752    
753        retval = beforeControl();
754        if (!retval.isOK()) {
755            return retval;
756        }
757        
758        {    
759            final var _diffAddFX_reqPtr = _diffAddFXRequest;
760            
761            final var _diffAddFX_retval = _diffAddFX.setControl(_diffAddFX_reqPtr);
762            if (retval.isOK()) {
763                retval = _diffAddFX_retval;
764            }
765        }
766        
767        {
768            final var _diffSubFX_retval = _diffSubFX.setControl(_diffFollow);
769            if (retval.isOK()) {
770                retval = _diffSubFX_retval;
771            }
772        }
773        
774        return retval;
775    }
776    
777    /**
778     * Sets the control request for this mechanism.
779     *
780     * @param _diffAddFXRequest    Requests Motion Magic® to target a final position
781     *                             using a motion profile, and PID to a differential
782     *                             position setpoint.
783     * @return Status Code of the request.
784     */
785    public StatusCode setControl(DifferentialMotionMagicDutyCycle _diffAddFXRequest)
786    {
787        var retval = StatusCode.OK;
788    
789        retval = beforeControl();
790        if (!retval.isOK()) {
791            return retval;
792        }
793        
794        {    
795            final var _diffAddFX_reqPtr = _diffAddFXRequest;
796            
797            final var _diffAddFX_retval = _diffAddFX.setControl(_diffAddFX_reqPtr);
798            if (retval.isOK()) {
799                retval = _diffAddFX_retval;
800            }
801        }
802        
803        {
804            final var _diffSubFX_retval = _diffSubFX.setControl(_diffFollow);
805            if (retval.isOK()) {
806                retval = _diffSubFX_retval;
807            }
808        }
809        
810        return retval;
811    }
812    
813    /**
814     * Sets the control request for this mechanism.
815     *
816     * @param _diffAddFXRequest    Requests Motion Magic® to target a final position
817     *                             using a motion profile, and PID to a differential
818     *                             position setpoint.
819     * @return Status Code of the request.
820     */
821    public StatusCode setControl(DifferentialMotionMagicVoltage _diffAddFXRequest)
822    {
823        var retval = StatusCode.OK;
824    
825        retval = beforeControl();
826        if (!retval.isOK()) {
827            return retval;
828        }
829        
830        {    
831            final var _diffAddFX_reqPtr = _diffAddFXRequest;
832            
833            final var _diffAddFX_retval = _diffAddFX.setControl(_diffAddFX_reqPtr);
834            if (retval.isOK()) {
835                retval = _diffAddFX_retval;
836            }
837        }
838        
839        {
840            final var _diffSubFX_retval = _diffSubFX.setControl(_diffFollow);
841            if (retval.isOK()) {
842                retval = _diffSubFX_retval;
843            }
844        }
845        
846        return retval;
847    }
848    
849}