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.OptionalInt;
011import java.util.concurrent.atomic.AtomicBoolean;
012import java.util.function.BooleanSupplier;
013
014import edu.wpi.first.units.measure.*;
015import edu.wpi.first.wpilibj.DriverStation;
016
017import static edu.wpi.first.units.Units.*;
018
019import com.ctre.phoenix6.BaseStatusSignal;
020import com.ctre.phoenix6.CANBus;
021import com.ctre.phoenix6.StatusCode;
022import com.ctre.phoenix6.StatusSignal;
023import com.ctre.phoenix6.configs.DifferentialSensorsConfigs;
024import com.ctre.phoenix6.configs.MotorOutputConfigs;
025import com.ctre.phoenix6.configs.TalonFXConfiguration;
026import com.ctre.phoenix6.configs.TalonFXSConfiguration;
027import com.ctre.phoenix6.controls.DifferentialFollower;
028import com.ctre.phoenix6.controls.NeutralOut;
029import com.ctre.phoenix6.controls.CoastOut;
030import com.ctre.phoenix6.controls.StaticBrake;
031import com.ctre.phoenix6.hardware.CANcoder;
032import com.ctre.phoenix6.hardware.CANdi;
033import com.ctre.phoenix6.hardware.Pigeon2;
034import com.ctre.phoenix6.hardware.TalonFX;
035import com.ctre.phoenix6.hardware.TalonFXS;
036import com.ctre.phoenix6.hardware.traits.CommonTalon;
037import com.ctre.phoenix6.mechanisms.DifferentialMotorConstants.*;
038import com.ctre.phoenix6.signals.DifferentialSensorSourceValue;
039import com.ctre.phoenix6.signals.InvertedValue;
040import com.ctre.phoenix6.signals.MotorAlignmentValue;
041import com.ctre.phoenix6.signals.NeutralModeValue;
042
043import com.ctre.phoenix6.controls.ControlRequest;
044import com.ctre.phoenix6.controls.DutyCycleOut;
045import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
046import com.ctre.phoenix6.controls.MotionMagicExpoDutyCycle;
047import com.ctre.phoenix6.controls.MotionMagicExpoTorqueCurrentFOC;
048import com.ctre.phoenix6.controls.MotionMagicExpoVoltage;
049import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
050import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle;
051import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC;
052import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
053import com.ctre.phoenix6.controls.MotionMagicVoltage;
054import com.ctre.phoenix6.controls.PositionDutyCycle;
055import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
056import com.ctre.phoenix6.controls.PositionVoltage;
057import com.ctre.phoenix6.controls.TorqueCurrentFOC;
058import com.ctre.phoenix6.controls.VelocityDutyCycle;
059import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
060import com.ctre.phoenix6.controls.VelocityVoltage;
061import com.ctre.phoenix6.controls.VoltageOut;
062import com.ctre.phoenix6.controls.compound.Diff_DutyCycleOut_Open;
063import com.ctre.phoenix6.controls.compound.Diff_DutyCycleOut_Position;
064import com.ctre.phoenix6.controls.compound.Diff_DutyCycleOut_Velocity;
065import com.ctre.phoenix6.controls.compound.Diff_MotionMagicDutyCycle_Open;
066import com.ctre.phoenix6.controls.compound.Diff_MotionMagicDutyCycle_Position;
067import com.ctre.phoenix6.controls.compound.Diff_MotionMagicDutyCycle_Velocity;
068import com.ctre.phoenix6.controls.compound.Diff_MotionMagicExpoDutyCycle_Open;
069import com.ctre.phoenix6.controls.compound.Diff_MotionMagicExpoDutyCycle_Position;
070import com.ctre.phoenix6.controls.compound.Diff_MotionMagicExpoDutyCycle_Velocity;
071import com.ctre.phoenix6.controls.compound.Diff_MotionMagicExpoTorqueCurrentFOC_Open;
072import com.ctre.phoenix6.controls.compound.Diff_MotionMagicExpoTorqueCurrentFOC_Position;
073import com.ctre.phoenix6.controls.compound.Diff_MotionMagicExpoTorqueCurrentFOC_Velocity;
074import com.ctre.phoenix6.controls.compound.Diff_MotionMagicExpoVoltage_Open;
075import com.ctre.phoenix6.controls.compound.Diff_MotionMagicExpoVoltage_Position;
076import com.ctre.phoenix6.controls.compound.Diff_MotionMagicExpoVoltage_Velocity;
077import com.ctre.phoenix6.controls.compound.Diff_MotionMagicTorqueCurrentFOC_Open;
078import com.ctre.phoenix6.controls.compound.Diff_MotionMagicTorqueCurrentFOC_Position;
079import com.ctre.phoenix6.controls.compound.Diff_MotionMagicTorqueCurrentFOC_Velocity;
080import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVelocityDutyCycle_Open;
081import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVelocityDutyCycle_Position;
082import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVelocityDutyCycle_Velocity;
083import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVelocityTorqueCurrentFOC_Open;
084import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVelocityTorqueCurrentFOC_Position;
085import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVelocityTorqueCurrentFOC_Velocity;
086import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVelocityVoltage_Open;
087import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVelocityVoltage_Position;
088import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVelocityVoltage_Velocity;
089import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVoltage_Open;
090import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVoltage_Position;
091import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVoltage_Velocity;
092import com.ctre.phoenix6.controls.compound.Diff_PositionDutyCycle_Open;
093import com.ctre.phoenix6.controls.compound.Diff_PositionDutyCycle_Position;
094import com.ctre.phoenix6.controls.compound.Diff_PositionDutyCycle_Velocity;
095import com.ctre.phoenix6.controls.compound.Diff_PositionTorqueCurrentFOC_Open;
096import com.ctre.phoenix6.controls.compound.Diff_PositionTorqueCurrentFOC_Position;
097import com.ctre.phoenix6.controls.compound.Diff_PositionTorqueCurrentFOC_Velocity;
098import com.ctre.phoenix6.controls.compound.Diff_PositionVoltage_Open;
099import com.ctre.phoenix6.controls.compound.Diff_PositionVoltage_Position;
100import com.ctre.phoenix6.controls.compound.Diff_PositionVoltage_Velocity;
101import com.ctre.phoenix6.controls.compound.Diff_TorqueCurrentFOC_Open;
102import com.ctre.phoenix6.controls.compound.Diff_TorqueCurrentFOC_Position;
103import com.ctre.phoenix6.controls.compound.Diff_TorqueCurrentFOC_Velocity;
104import com.ctre.phoenix6.controls.compound.Diff_VelocityDutyCycle_Open;
105import com.ctre.phoenix6.controls.compound.Diff_VelocityDutyCycle_Position;
106import com.ctre.phoenix6.controls.compound.Diff_VelocityDutyCycle_Velocity;
107import com.ctre.phoenix6.controls.compound.Diff_VelocityTorqueCurrentFOC_Open;
108import com.ctre.phoenix6.controls.compound.Diff_VelocityTorqueCurrentFOC_Position;
109import com.ctre.phoenix6.controls.compound.Diff_VelocityTorqueCurrentFOC_Velocity;
110import com.ctre.phoenix6.controls.compound.Diff_VelocityVoltage_Open;
111import com.ctre.phoenix6.controls.compound.Diff_VelocityVoltage_Position;
112import com.ctre.phoenix6.controls.compound.Diff_VelocityVoltage_Velocity;
113import com.ctre.phoenix6.controls.compound.Diff_VoltageOut_Open;
114import com.ctre.phoenix6.controls.compound.Diff_VoltageOut_Position;
115import com.ctre.phoenix6.controls.compound.Diff_VoltageOut_Velocity;
116
117/**
118 * Manages control of a two-axis differential mechanism.
119 * <p>
120 * This mechanism requires the devices to be Pro licensed and
121 * connected to a CAN FD bus. Unlicensed users and users on a CAN
122 * 2.0 bus can use the {@link SimpleDifferentialMechanism} instead
123 * with limited functionality.
124 * <p>
125 * A differential mechanism has two axes of motion, where the position
126 * along each axis is determined by two motors in separate gearboxes:
127 * <ul>
128 *   <li>Driving both motors in a common direction causes the mechanism
129 *       to move forward/reverse, up/down, etc.
130 *   <ul>
131 *     <li>This is the Average axis: position is determined by the
132 *         average of the two motors' positions.
133 *   </ul>
134 *   <li>Driving the motors in opposing directions causes the mechanism
135 *       to twist or rotate left/right.
136 *   <ul>
137 *     <li>This is the Difference axis: rotation is determined by half
138 *         the difference of the two motors' positions.
139 *   </ul>
140 * </ul>
141 */
142public class DifferentialMechanism<MotorT extends CommonTalon> {
143    /**
144     * Functional interface for device constructors.
145     */
146    @FunctionalInterface
147    public static interface DeviceConstructor<DeviceT> {
148        DeviceT create(int id, CANBus canbus);
149    }
150
151    /**
152     * Possible reasons for the mechanism to disable.
153     */
154    public enum DisabledReasonValue {
155        /**
156         * No reason given.
157         */
158        None,
159        /**
160         * A remote sensor is not present on CAN Bus.
161         */
162        MissingRemoteSensor,
163        /**
164         * The remote Talon FX used for differential
165         * control is not present on CAN Bus.
166         */
167        MissingDifferentialFX,
168        /**
169         * A remote sensor position has overflowed. Because of the nature
170         * of remote sensors, it is possible for a remote sensor position
171         * to overflow beyond what is supported by the status signal frame.
172         * However, this is rare and cannot occur over the course of an FRC
173         * match under normal use.
174         */
175        RemoteSensorPosOverflow,
176        /**
177         * A device or remote sensor has reset.
178         */
179        DeviceHasReset,
180    }
181
182    /**
183     * Possible reasons for the mechanism to require
184     * user action to resume control.
185     */
186    public enum RequiresUserReasonValue {
187        /**
188         * No reason given.
189         */
190        None,
191        /**
192         * A remote sensor position has overflowed. Because of the nature
193         * of remote sensors, it is possible for a remote sensor position
194         * to overflow beyond what is supported by the status signal frame.
195         * However, this is rare and cannot occur over the course of an FRC
196         * match under normal use.
197         */
198        RemoteSensorPosOverflow,
199        /**
200         * A device or remote sensor has reset.
201         */
202        DeviceHasReset,
203    }
204
205    /** Number of times to attempt config applies. */
206    private static final int kNumConfigAttempts = 2;
207
208    private final MotorT _diffLeaderFX;
209    private final MotorT _diffFollowerFX;
210
211    private final double _sensorToDiffRatio;
212
213    private final BooleanSupplier _diffLeaderFXResetChecker;
214    private final BooleanSupplier _diffFollowerFXResetChecker;
215    private final Optional<BooleanSupplier> _diffSensorResetChecker;
216
217    private final DifferentialFollower _diffFollow;
218
219    private final NeutralOut _neutral = new NeutralOut();
220    private final CoastOut _coast = new CoastOut();
221    private final StaticBrake _brake = new StaticBrake();
222
223    private final AtomicBoolean _mechanismDisabled = new AtomicBoolean(false);
224    private final AtomicBoolean _requiresUserAction = new AtomicBoolean(false);
225
226    private DisabledReasonValue _disabledReason = DisabledReasonValue.None;
227    private RequiresUserReasonValue _requiresUserReason = RequiresUserReasonValue.None;
228
229    /**
230     * Creates a new differential mechanism using two {@link CommonTalon} devices.
231     * The mechanism will use the average of the two Talon FX sensors on the primary axis,
232     * and half of the difference between the two Talon FX sensors on the differential axis.
233     * <p>
234     * This mechanism requires the devices to be Pro licensed and connected to a CAN FD bus.
235     * Unlicensed users and users on a CAN 2.0 bus can use the {@link SimpleDifferentialMechanism}
236     * instead with limited functionality.
237     *
238     * @param motorConstructor Constructor for the motors, such as {@code TalonFX::new}
239     * @param constants Constants used to construct the mechanism
240     */
241    public DifferentialMechanism(DeviceConstructor<MotorT> motorConstructor, DifferentialMotorConstants<?> constants) {
242        final var canbus = new CANBus(constants.CANBusName);
243        _diffLeaderFX = motorConstructor.create(constants.LeaderId, canbus);
244        _diffFollowerFX = motorConstructor.create(constants.FollowerId, canbus);
245        _sensorToDiffRatio = constants.SensorToDifferentialRatio;
246        _diffLeaderFXResetChecker = _diffLeaderFX.getResetOccurredChecker();
247        _diffFollowerFXResetChecker = _diffFollowerFX.getResetOccurredChecker();
248        _diffFollow = new DifferentialFollower(_diffLeaderFX.getDeviceID(), constants.Alignment);
249        _diffSensorResetChecker = Optional.empty();
250
251        applyConfigs(constants, DifferentialSensorSourceValue.RemoteTalonFX_HalfDiff, OptionalInt.empty());
252        /* Set update frequency of relevant signals */
253        BaseStatusSignal.setUpdateFrequencyForAll(
254            constants.ClosedLoopRate,
255            _diffLeaderFX.getDifferentialOutput(false),
256            _diffFollowerFX.getPosition(false),
257            _diffFollowerFX.getVelocity(false)
258        );
259    }
260
261    /**
262     * Creates a new differential mechanism using two {@link CommonTalon} devices and
263     * a {@link Pigeon2}. The mechanism will use the average of the two Talon FX sensors on the
264     * primary axis, and the selected Pigeon 2 sensor source on the differential axis.
265     * <p>
266     * This mechanism requires the devices to be Pro licensed and connected to a CAN FD bus.
267     * Unlicensed users and users on a CAN 2.0 bus can use the {@link SimpleDifferentialMechanism}
268     * instead with limited functionality.
269     *
270     * @param motorConstructor Constructor for the motors, such as {@code TalonFX::new}
271     * @param constants Constants used to construct the mechanism
272     * @param pigeon2 The Pigeon 2 to use for the differential axis
273     * @param pigeonSource The sensor source to use for the Pigeon 2 (Yaw, Pitch, or Roll)
274     */
275    public DifferentialMechanism(DeviceConstructor<MotorT> motorConstructor, DifferentialMotorConstants<?> constants, Pigeon2 pigeon2, DifferentialPigeon2Source pigeonSource) {
276        final var canbus = new CANBus(constants.CANBusName);
277        _diffLeaderFX = motorConstructor.create(constants.LeaderId, canbus);
278        _diffFollowerFX = motorConstructor.create(constants.FollowerId, canbus);
279        _sensorToDiffRatio = constants.SensorToDifferentialRatio;
280        _diffLeaderFXResetChecker = _diffLeaderFX.getResetOccurredChecker();
281        _diffFollowerFXResetChecker = _diffFollowerFX.getResetOccurredChecker();
282        _diffSensorResetChecker = Optional.of(pigeon2.getResetOccurredChecker());
283        _diffFollow = new DifferentialFollower(_diffLeaderFX.getDeviceID(), constants.Alignment);
284
285        DifferentialSensorSourceValue diffSensorSource;
286        switch (pigeonSource) {
287            case Yaw:
288            default:
289                diffSensorSource = DifferentialSensorSourceValue.RemotePigeon2Yaw;
290                break;
291            case Pitch:
292                diffSensorSource = DifferentialSensorSourceValue.RemotePigeon2Pitch;
293                break;
294            case Roll:
295                diffSensorSource = DifferentialSensorSourceValue.RemotePigeon2Roll;
296                break;
297        }
298        applyConfigs(constants, diffSensorSource, OptionalInt.of(pigeon2.getDeviceID()));
299
300        /* Set update frequency of relevant signals */
301        BaseStatusSignal.setUpdateFrequencyForAll(
302            constants.ClosedLoopRate,
303            _diffLeaderFX.getDifferentialOutput(false),
304            _diffFollowerFX.getPosition(false),
305            _diffFollowerFX.getVelocity(false)
306        );
307        switch (pigeonSource) {
308            case Yaw:
309                pigeon2.getYaw(false).setUpdateFrequency(constants.ClosedLoopRate);
310                break;
311            case Pitch:
312                pigeon2.getPitch(false).setUpdateFrequency(constants.ClosedLoopRate);
313                break;
314            case Roll:
315                pigeon2.getRoll(false).setUpdateFrequency(constants.ClosedLoopRate);
316                break;
317        }
318    }
319
320    /**
321     * Creates a new differential mechanism using two {@link CommonTalon} devices and
322     * a {@link CANcoder}. The mechanism will use the average of the two Talon FX sensors on the
323     * primary axis, and the CANcoder position/velocity on the differential axis.
324     * <p>
325     * This mechanism requires the devices to be Pro licensed and connected to a CAN FD bus.
326     * Unlicensed users and users on a CAN 2.0 bus can use the {@link SimpleDifferentialMechanism}
327     * instead with limited functionality.
328     *
329     * @param motorConstructor Constructor for the motors, such as {@code TalonFX::new}
330     * @param constants Constants used to construct the mechanism
331     * @param cancoder The CANcoder to use for the differential axis
332     */
333    public DifferentialMechanism(DeviceConstructor<MotorT> motorConstructor, DifferentialMotorConstants<?> constants, CANcoder cancoder) {
334        final var canbus = new CANBus(constants.CANBusName);
335        _diffLeaderFX = motorConstructor.create(constants.LeaderId, canbus);
336        _diffFollowerFX = motorConstructor.create(constants.FollowerId, canbus);
337        _sensorToDiffRatio = constants.SensorToDifferentialRatio;
338        _diffLeaderFXResetChecker = _diffLeaderFX.getResetOccurredChecker();
339        _diffFollowerFXResetChecker = _diffFollowerFX.getResetOccurredChecker();
340        _diffSensorResetChecker = Optional.of(cancoder.getResetOccurredChecker());
341        _diffFollow = new DifferentialFollower(_diffLeaderFX.getDeviceID(), constants.Alignment);
342
343        applyConfigs(constants, DifferentialSensorSourceValue.RemoteCANcoder, OptionalInt.of(cancoder.getDeviceID()));
344        /* Set update frequency of relevant signals */
345        BaseStatusSignal.setUpdateFrequencyForAll(
346            constants.ClosedLoopRate,
347            _diffLeaderFX.getDifferentialOutput(false),
348            _diffFollowerFX.getPosition(false),
349            _diffFollowerFX.getVelocity(false),
350            cancoder.getPosition(false),
351            cancoder.getVelocity(false)
352        );
353    }
354
355    /**
356     * Creates a new differential mechanism using two {@link CommonTalon} devices and a
357     * {@link CANdi}. The mechanism will use the average of the two Talon FX sensors on the primary
358     * axis, and the selected CTR Electronics' CANdi™ branded sensor source on the differential axis.
359     * <p>
360     * This mechanism requires the devices to be Pro licensed and connected to a CAN FD bus.
361     * Unlicensed users and users on a CAN 2.0 bus can use the {@link SimpleDifferentialMechanism}
362     * instead with limited functionality.
363     *
364     * @param motorConstructor Constructor for the motors, such as {@code TalonFX::new}
365     * @param constants Constants used to construct the mechanism
366     * @param candi The CTR Electronics' CANdi™ branded device to use for the differential axis
367     * @param candiSource The sensor source to use for the CTR Electronics' CANdi™ branded device
368     */
369    public DifferentialMechanism(DeviceConstructor<MotorT> motorConstructor, DifferentialMotorConstants<?> constants, CANdi candi, DifferentialCANdiSource candiSource) {
370        final var canbus = new CANBus(constants.CANBusName);
371        _diffLeaderFX = motorConstructor.create(constants.LeaderId, canbus);
372        _diffFollowerFX = motorConstructor.create(constants.FollowerId, canbus);
373        _sensorToDiffRatio = constants.SensorToDifferentialRatio;
374        _diffLeaderFXResetChecker = _diffLeaderFX.getResetOccurredChecker();
375        _diffFollowerFXResetChecker = _diffFollowerFX.getResetOccurredChecker();
376        _diffSensorResetChecker = Optional.of(candi.getResetOccurredChecker());
377        _diffFollow = new DifferentialFollower(_diffLeaderFX.getDeviceID(), constants.Alignment);
378
379        DifferentialSensorSourceValue diffSensorSource;
380        switch (candiSource) {
381            case PWM1:
382            default:
383                diffSensorSource = DifferentialSensorSourceValue.RemoteCANdiPWM1;
384                break;
385            case PWM2:
386                diffSensorSource = DifferentialSensorSourceValue.RemoteCANdiPWM2;
387                break;
388            case Quadrature:
389                diffSensorSource = DifferentialSensorSourceValue.RemoteCANdiQuadrature;
390                break;
391        }
392        applyConfigs(constants, diffSensorSource, OptionalInt.of(candi.getDeviceID()));
393
394        /* Set update frequency of relevant signals */
395        BaseStatusSignal.setUpdateFrequencyForAll(
396            constants.ClosedLoopRate,
397            _diffLeaderFX.getDifferentialOutput(false),
398            _diffFollowerFX.getPosition(false),
399            _diffFollowerFX.getVelocity(false)
400        );
401        switch (candiSource) {
402            case PWM1:
403                BaseStatusSignal.setUpdateFrequencyForAll(
404                    constants.ClosedLoopRate,
405                    candi.getPWM1Position(false),
406                    candi.getPWM1Velocity(false)
407                );
408                break;
409            case PWM2:
410                BaseStatusSignal.setUpdateFrequencyForAll(
411                    constants.ClosedLoopRate,
412                    candi.getPWM2Position(false),
413                    candi.getPWM2Velocity(false)
414                );
415                break;
416            case Quadrature:
417                BaseStatusSignal.setUpdateFrequencyForAll(
418                    constants.ClosedLoopRate,
419                    candi.getQuadraturePosition(false),
420                    candi.getQuadratureVelocity(false)
421                );
422                break;
423        }
424    }
425
426    private void applyConfigs(DifferentialMotorConstants<?> constants, DifferentialSensorSourceValue diffSensorSource, OptionalInt diffSensorID) {
427        /*
428         * The onboard differential controller adds the differential output to its own output
429         * and subtracts the differential output for differential followers, so use _diffLeaderFX
430         * as the primary controller.
431         */
432
433        InvertedValue leaderInvert;
434
435        /* set up differential control on _diffLeaderFX */
436        {
437            var diff_cfg = new DifferentialSensorsConfigs();
438
439            diff_cfg.DifferentialTalonFXSensorID = _diffFollowerFX.getDeviceID();
440            diff_cfg.DifferentialSensorSource = diffSensorSource;
441            diffSensorID.ifPresent(id -> diff_cfg.DifferentialRemoteSensorID = id);
442            diff_cfg.SensorToDifferentialRatio = constants.SensorToDifferentialRatio;
443
444            StatusCode response = StatusCode.OK;
445
446            if (
447                _diffLeaderFX instanceof TalonFX diffLeaderFX &&
448                (constants.LeaderInitialConfigs == null || constants.LeaderInitialConfigs instanceof TalonFXConfiguration)
449            ) {
450                var leaderConfigs = (TalonFXConfiguration)constants.LeaderInitialConfigs;
451                if (leaderConfigs == null) {
452                    leaderConfigs = new TalonFXConfiguration();
453                }
454                leaderInvert = leaderConfigs.MotorOutput.Inverted;
455
456                leaderConfigs.DifferentialSensors = diff_cfg;
457                for (int i = 0; i < kNumConfigAttempts; ++i) {
458                    response = diffLeaderFX.getConfigurator().apply(leaderConfigs);
459                    if (response.isOK()) break;
460                }
461            } else if (
462                _diffLeaderFX instanceof TalonFXS diffLeaderFXS &&
463                (constants.LeaderInitialConfigs == null || constants.LeaderInitialConfigs instanceof TalonFXSConfiguration)
464            ) {
465                var leaderConfigs = (TalonFXSConfiguration)constants.LeaderInitialConfigs;
466                if (leaderConfigs == null) {
467                    leaderConfigs = new TalonFXSConfiguration();
468                }
469                leaderInvert = leaderConfigs.MotorOutput.Inverted;
470
471                leaderConfigs.DifferentialSensors = diff_cfg;
472                for (int i = 0; i < kNumConfigAttempts; ++i) {
473                    response = diffLeaderFXS.getConfigurator().apply(leaderConfigs);
474                    if (response.isOK()) break;
475                }
476            } else {
477                DriverStation.reportError(
478                    "[phoenix] DifferentialMechanism motor type does not match the DifferentialMotorConstants initial configs type. Ensure that the DifferentialMechanism and DifferentialMotorConstants instances are both constructed with the same motor type (TalonFX/TalonFXConfiguration, TalonFXS/TalonFXSConfiguration, etc.).",
479                    true
480                );
481                return;
482            }
483
484            if (!response.isOK()) {
485                System.out.println(
486                    "Talon ID " + _diffLeaderFX.getDeviceID() + " failed config with error " + response.toString()
487                );
488            }
489        }
490
491        /* disable differential control on _diffFollowerFX */
492        {
493            StatusCode response = StatusCode.OK;
494
495            if (_diffFollowerFX instanceof TalonFX diffFollowerFX) {
496                var followerConfigs = (TalonFXConfiguration)constants.FollowerInitialConfigs;
497                if (followerConfigs == null) {
498                    followerConfigs = new TalonFXConfiguration();
499                }
500                followerConfigs.DifferentialSensors = new DifferentialSensorsConfigs();
501                followerConfigs.MotorOutput.Inverted = constants.Alignment == MotorAlignmentValue.Aligned
502                    ? leaderInvert
503                    : leaderInvert == InvertedValue.CounterClockwise_Positive
504                        ? InvertedValue.Clockwise_Positive
505                        : InvertedValue.CounterClockwise_Positive;
506
507                if (constants.FollowerUsesCommonLeaderConfigs) {
508                    /* copy over common config groups from the leader */
509                    var leaderConfigs = (TalonFXConfiguration)constants.LeaderInitialConfigs;
510                    if (leaderConfigs == null) {
511                        leaderConfigs = new TalonFXConfiguration();
512                    }
513
514                    followerConfigs.Audio = leaderConfigs.Audio;
515                    followerConfigs.CurrentLimits = leaderConfigs.CurrentLimits;
516                    followerConfigs.MotorOutput = leaderConfigs.MotorOutput.clone()
517                        .withInverted(followerConfigs.MotorOutput.Inverted);
518                    followerConfigs.Voltage = leaderConfigs.Voltage;
519                    followerConfigs.TorqueCurrent = leaderConfigs.TorqueCurrent;
520                }
521
522                for (int i = 0; i < kNumConfigAttempts; ++i) {
523                    response = diffFollowerFX.getConfigurator().apply(followerConfigs);
524                    if (response.isOK()) break;
525                }
526            } else if (_diffFollowerFX instanceof TalonFXS diffFollowerFXS) {
527                var followerConfigs = (TalonFXSConfiguration)constants.FollowerInitialConfigs;
528                if (followerConfigs == null) {
529                    followerConfigs = new TalonFXSConfiguration();
530                }
531                followerConfigs.DifferentialSensors = new DifferentialSensorsConfigs();
532                followerConfigs.MotorOutput.Inverted = constants.Alignment == MotorAlignmentValue.Aligned
533                    ? leaderInvert
534                    : leaderInvert == InvertedValue.CounterClockwise_Positive
535                        ? InvertedValue.Clockwise_Positive
536                        : InvertedValue.CounterClockwise_Positive;
537
538                if (constants.FollowerUsesCommonLeaderConfigs) {
539                    /* copy over common config groups from the leader */
540                    var leaderConfigs = (TalonFXSConfiguration)constants.LeaderInitialConfigs;
541                    if (leaderConfigs == null) {
542                        leaderConfigs = new TalonFXSConfiguration();
543                    }
544
545                    followerConfigs.Audio = leaderConfigs.Audio;
546                    followerConfigs.CurrentLimits = leaderConfigs.CurrentLimits;
547                    followerConfigs.MotorOutput = leaderConfigs.MotorOutput.clone()
548                        .withInverted(followerConfigs.MotorOutput.Inverted);
549                    followerConfigs.Voltage = leaderConfigs.Voltage;
550                }
551
552                for (int i = 0; i < kNumConfigAttempts; ++i) {
553                    response = diffFollowerFXS.getConfigurator().apply(followerConfigs);
554                    if (response.isOK()) break;
555                }
556            } else {
557                DriverStation.reportError(
558                    "[phoenix] DifferentialMechanism motor type does not match the DifferentialMotorConstants initial configs type. Ensure that the DifferentialMechanism and DifferentialMotorConstants instances are both constructed with the same motor type (TalonFX/TalonFXConfiguration, TalonFXS/TalonFXSConfiguration, etc.).",
559                    true
560                );
561                return;
562            }
563
564            if (!response.isOK()) {
565                System.out.println(
566                    "Talon ID " + _diffFollowerFX.getDeviceID() + " failed config with error " + response.toString()
567                );
568            }
569        }
570    }
571
572    private StatusCode configNeutralModeImpl(MotorT motor, NeutralModeValue neutralMode, double timeoutSeconds) {
573        StatusCode status = StatusCode.OK;
574
575        var motorOutCfg = new MotorOutputConfigs();
576        if (motor instanceof TalonFX motorFX) {
577            /* First read the configs so they're up-to-date */
578            status = motorFX.getConfigurator().refresh(motorOutCfg, timeoutSeconds);
579            if (status.isOK()) {
580                /* Then set the neutral mode config to the appropriate value */
581                motorOutCfg.NeutralMode = neutralMode;
582                status = motorFX.getConfigurator().apply(motorOutCfg, timeoutSeconds);
583            }
584        } else if (motor instanceof TalonFXS motorFXS) {
585            /* First read the configs so they're up-to-date */
586            status = motorFXS.getConfigurator().refresh(motorOutCfg, timeoutSeconds);
587            if (status.isOK()) {
588                /* Then set the neutral mode config to the appropriate value */
589                motorOutCfg.NeutralMode = neutralMode;
590                status = motorFXS.getConfigurator().apply(motorOutCfg, timeoutSeconds);
591            }
592        }
593
594        if (!status.isOK()) {
595            System.out.println(
596                "Talon ID " + motor.getDeviceID() + " failed config with error " + status.toString()
597            );
598        }
599        return status;
600    }
601
602    /**
603     * Configures the neutral mode to use for both motors in the mechanism.
604     * <p>
605     * This will wait up to 0.100 seconds (100ms) by default.
606     *
607     * @param neutralMode The state of the motor controller bridge when output is neutral or disabled
608     * @return Status code of the first failed config call, or OK if all succeeded
609     */
610    public final StatusCode configNeutralMode(NeutralModeValue neutralMode) {
611        return configNeutralMode(neutralMode, 0.100);
612    }
613
614    /**
615     * Configures the neutral mode to use for both motors in the mechanism.
616     *
617     * @param neutralMode The state of the motor controller bridge when output is neutral or disabled
618     * @param timeoutSeconds Maximum amount of time to wait when performing each configuration
619     * @return Status code of the first failed config call, or OK if all succeeded
620     */
621    public final StatusCode configNeutralMode(NeutralModeValue neutralMode, double timeoutSeconds) {
622        StatusCode retval = StatusCode.OK;
623        {
624            final var status = configNeutralModeImpl(_diffLeaderFX, neutralMode, timeoutSeconds);
625            if (retval.isOK()) retval = status;
626        }
627        {
628            final var status = configNeutralModeImpl(_diffFollowerFX, neutralMode, timeoutSeconds);
629            if (retval.isOK()) retval = status;
630        }
631        return retval;
632    }
633
634    /**
635     * Call this method periodically to automatically protect against dangerous
636     * fault conditions and keep {@link #getMechanismState()} updated.
637     */
638    public final void periodic() {
639        StatusCode retval = StatusCode.OK;
640
641        /* handle remote sensor position overflow fault */
642        if (_diffLeaderFX.getFault_RemoteSensorPosOverflow().getValue()) {
643            /* fault the mechanism until the user clears it manually */
644            _requiresUserReason = RequiresUserReasonValue.RemoteSensorPosOverflow;
645            _requiresUserAction.setRelease(true);
646
647            _disabledReason = DisabledReasonValue.RemoteSensorPosOverflow;
648            retval = StatusCode.MechanismFaulted;
649        }
650
651        /* handle missing remote sensor fault */
652        if (_diffLeaderFX.getFault_RemoteSensorDataInvalid().getValue() ||
653            _diffFollowerFX.getFault_RemoteSensorDataInvalid().getValue()
654        ) {
655            /* temporarily fault the mechanism while the fault is active */
656            _disabledReason = DisabledReasonValue.MissingRemoteSensor;
657            retval = StatusCode.MechanismFaulted;
658        }
659        /* handle missing differential Talon FX fault */
660        if (_diffLeaderFX.getFault_MissingDifferentialFX().getValue()) {
661            /* temporarily fault the mechanism while the fault is active */
662            _disabledReason = DisabledReasonValue.MissingDifferentialFX;
663            retval = StatusCode.MechanismFaulted;
664        }
665
666        /* handle if any of the devices have power cycled */
667        final boolean diffLeaderFX_hasReset = _diffLeaderFXResetChecker.getAsBoolean();
668        final boolean diffFollowerFX_hasReset = _diffFollowerFXResetChecker.getAsBoolean();
669        final boolean diffSensor_hasReset = _diffSensorResetChecker.isPresent() && _diffSensorResetChecker.get().getAsBoolean();
670        final boolean diffLeaderFX_remsens_hasReset = _diffLeaderFX.getStickyFault_RemoteSensorReset().getValue();
671        final boolean diffFollowerFX_remsens_hasReset = _diffFollowerFX.getStickyFault_RemoteSensorReset().getValue();
672
673        if (diffLeaderFX_hasReset || diffFollowerFX_hasReset || diffSensor_hasReset ||
674            diffLeaderFX_remsens_hasReset || diffFollowerFX_remsens_hasReset
675        ) {
676            /* fault the mechanism until the user clears it manually */
677            _requiresUserReason = RequiresUserReasonValue.DeviceHasReset;
678            _requiresUserAction.setRelease(true);
679
680            _disabledReason = DisabledReasonValue.DeviceHasReset;
681            retval = StatusCode.MechanismFaulted;
682        }
683
684        if (retval.isOK() && _requiresUserAction.getOpaque()) {
685            /* keep the mechanism faulted until user clears the fault */
686            retval = StatusCode.MechanismFaulted;
687        }
688
689        if (!retval.isOK()) {
690            /* disable the mechanism */
691            _mechanismDisabled.setRelease(true);
692        } else {
693            /* re-enable the mechanism */
694            _disabledReason = DisabledReasonValue.None;
695            _mechanismDisabled.setRelease(false);
696        }
697    }
698
699    /**
700     * Get whether the mechanism is currently disabled due to an issue.
701     *
702     * @return true if the mechanism is temporarily disabled
703     */
704    public final boolean isDisabled() {
705        return _mechanismDisabled.getAcquire();
706    }
707
708    /**
709     * Get whether the mechanism is currently disabled and requires
710     * user action to re-enable mechanism control.
711     *
712     * @return true if the mechanism is disabled and the user must manually
713     *         perform an action
714     */
715    public final boolean requiresUserAction() {
716        return _requiresUserAction.getAcquire();
717    }
718
719    /**
720     * Gets the state of the mechanism.
721     *
722     * @return MechanismState representing the state of the mechanism
723     */
724    public final MechanismState getMechanismState() {
725        if (requiresUserAction()) {
726            return MechanismState.RequiresUserAction;
727        } else if (isDisabled()) {
728            return MechanismState.Disabled;
729        } else {
730            return MechanismState.OK;
731        }
732    }
733
734    /**
735     * Indicate to the mechanism that the user has performed the required
736     * action to resume mechanism control.
737     */
738    public final void clearUserRequirement() {
739        if (_diffLeaderFX.getStickyFault_RemoteSensorReset().getValue()) {
740            _diffLeaderFX.clearStickyFault_RemoteSensorReset();
741        }
742        if (_diffFollowerFX.getStickyFault_RemoteSensorReset().getValue()) {
743            _diffFollowerFX.clearStickyFault_RemoteSensorReset();
744        }
745        _requiresUserReason = RequiresUserReasonValue.None;
746        _requiresUserAction.setRelease(false);
747    }
748
749    /**
750     * @return The reason for the mechanism being disabled
751     */
752    public final DisabledReasonValue getDisabledReason() {
753        return _disabledReason;
754    }
755    /**
756     * @return The reason for the mechanism requiring user
757     *         action to resume control
758     */
759    public final RequiresUserReasonValue getRequiresUserReason() {
760        return _requiresUserReason;
761    }
762
763    /**
764     * Average component of the mechanism position.
765     * 
766     * <ul>
767     *   <li> <b>Minimum Value:</b> -16384.0
768     *   <li> <b>Maximum Value:</b> 16383.999755859375
769     *   <li> <b>Default Value:</b> 0
770     *   <li> <b>Units:</b> rotations
771     * </ul>
772     * <p>
773     * This refreshes and returns a cached StatusSignal object.
774     * 
775     * @return DifferentialAveragePosition Status Signal Object
776     */
777    public final StatusSignal<Angle> getAveragePosition() {
778        return getAveragePosition(true);
779    }
780    /**
781     * Average component of the mechanism position.
782     * 
783     * <ul>
784     *   <li> <b>Minimum Value:</b> -16384.0
785     *   <li> <b>Maximum Value:</b> 16383.999755859375
786     *   <li> <b>Default Value:</b> 0
787     *   <li> <b>Units:</b> rotations
788     * </ul>
789     * <p>
790     * This refreshes and returns a cached StatusSignal object.
791     * 
792     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
793     * @return DifferentialAveragePosition Status Signal Object
794     */
795    public final StatusSignal<Angle> getAveragePosition(boolean refresh) {
796        return _diffLeaderFX.getDifferentialAveragePosition(refresh);
797    }
798
799    /**
800     * Average component of the mechanism velocity.
801     * 
802     * <ul>
803     *   <li> <b>Minimum Value:</b> -512.0
804     *   <li> <b>Maximum Value:</b> 511.998046875
805     *   <li> <b>Default Value:</b> 0
806     *   <li> <b>Units:</b> rotations per second
807     * </ul>
808     * <p>
809     * This refreshes and returns a cached StatusSignal object.
810     * 
811     * @return DifferentialAverageVelocity Status Signal Object
812     */
813    public final StatusSignal<AngularVelocity> getAverageVelocity() {
814        return getAverageVelocity(true);
815    }
816    /**
817     * Average component of the mechanism velocity.
818     * 
819     * <ul>
820     *   <li> <b>Minimum Value:</b> -512.0
821     *   <li> <b>Maximum Value:</b> 511.998046875
822     *   <li> <b>Default Value:</b> 0
823     *   <li> <b>Units:</b> rotations per second
824     * </ul>
825     * <p>
826     * This refreshes and returns a cached StatusSignal object.
827     * 
828     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
829     * @return DifferentialAverageVelocity Status Signal Object
830     */
831    public final StatusSignal<AngularVelocity> getAverageVelocity(boolean refresh) {
832        return _diffLeaderFX.getDifferentialAverageVelocity(refresh);
833    }
834
835    /**
836     * Differential component of the mechanism position.
837     * 
838     * <ul>
839     *   <li> <b>Minimum Value:</b> -16384.0
840     *   <li> <b>Maximum Value:</b> 16383.999755859375
841     *   <li> <b>Default Value:</b> 0
842     *   <li> <b>Units:</b> rotations
843     * </ul>
844     * <p>
845     * This refreshes and returns a cached StatusSignal object.
846     * 
847     * @return DifferentialDifferencePosition Status Signal Object
848     */
849    public final StatusSignal<Angle> getDifferentialPosition() {
850        return getDifferentialPosition(true);
851    }
852    /**
853     * Differential component of the mechanism position.
854     * 
855     * <ul>
856     *   <li> <b>Minimum Value:</b> -16384.0
857     *   <li> <b>Maximum Value:</b> 16383.999755859375
858     *   <li> <b>Default Value:</b> 0
859     *   <li> <b>Units:</b> rotations
860     * </ul>
861     * <p>
862     * This refreshes and returns a cached StatusSignal object.
863     * 
864     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
865     * @return DifferentialDifferencePosition Status Signal Object
866     */
867    public final StatusSignal<Angle> getDifferentialPosition(boolean refresh) {
868        return _diffLeaderFX.getDifferentialDifferencePosition(refresh);
869    }
870
871    /**
872     * Differential component of the mechanism velocity.
873     * 
874     * <ul>
875     *   <li> <b>Minimum Value:</b> -512.0
876     *   <li> <b>Maximum Value:</b> 511.998046875
877     *   <li> <b>Default Value:</b> 0
878     *   <li> <b>Units:</b> rotations per second
879     * </ul>
880     * <p>
881     * This refreshes and returns a cached StatusSignal object.
882     * 
883     * @return DifferentialDifferenceVelocity Status Signal Object
884     */
885    public final StatusSignal<AngularVelocity> getDifferentialVelocity() {
886        return getDifferentialVelocity(true);
887    }
888    /**
889     * Differential component of the mechanism velocity.
890     * 
891     * <ul>
892     *   <li> <b>Minimum Value:</b> -512.0
893     *   <li> <b>Maximum Value:</b> 511.998046875
894     *   <li> <b>Default Value:</b> 0
895     *   <li> <b>Units:</b> rotations per second
896     * </ul>
897     * <p>
898     * This refreshes and returns a cached StatusSignal object.
899     * 
900     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
901     * @return DifferentialDifferenceVelocity Status Signal Object
902     */
903    public final StatusSignal<AngularVelocity> getDifferentialVelocity(boolean refresh) {
904        return _diffLeaderFX.getDifferentialDifferenceVelocity(refresh);
905    }
906
907    /**
908     * Value that the average closed loop is targeting.
909     * <p>
910     * This is the value that the closed loop PID controller targets.
911     * <p>
912     * This refreshes and returns a cached StatusSignal object.
913     * 
914     * @return ClosedLoopReference Status Signal object
915     */
916    public final StatusSignal<Double> getAverageClosedLoopReference() {
917        return getAverageClosedLoopReference(true);
918    }
919    /**
920     * Value that the average closed loop is targeting.
921     * <p>
922     * This is the value that the closed loop PID controller targets.
923     * <p>
924     * This refreshes and returns a cached StatusSignal object.
925     * 
926     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
927     * @return ClosedLoopReference Status Signal object
928     */
929    public final StatusSignal<Double> getAverageClosedLoopReference(boolean refresh) {
930        return _diffLeaderFX.getClosedLoopReference(refresh);
931    }
932
933    /**
934     * Derivative of the target that the average closed loop is targeting.
935     * <p>
936     * This is the change in the closed loop reference. This may be used
937     * in the feed-forward calculation, the derivative-error, or in
938     * application of the signage for kS. Typically, this represents the
939     * target velocity during Motion Magic®.
940     * <p>
941     * This refreshes and returns a cached StatusSignal object.
942     * 
943     * @return ClosedLoopReferenceSlope Status Signal object
944     */
945    public final StatusSignal<Double> getAverageClosedLoopReferenceSlope() {
946        return getAverageClosedLoopReferenceSlope(true);
947    }
948    /**
949     * Derivative of the target that the average closed loop is targeting.
950     * <p>
951     * This is the change in the closed loop reference. This may be used
952     * in the feed-forward calculation, the derivative-error, or in
953     * application of the signage for kS. Typically, this represents the
954     * target velocity during Motion Magic®.
955     * <p>
956     * This refreshes and returns a cached StatusSignal object.
957     * 
958     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
959     * @return ClosedLoopReferenceSlope Status Signal object
960     */
961    public final StatusSignal<Double> getAverageClosedLoopReferenceSlope(boolean refresh) {
962        return _diffLeaderFX.getClosedLoopReferenceSlope(refresh);
963    }
964
965    /**
966     * The difference between target average reference and current measurement.
967     * <p>
968     * This is the value that is treated as the error in the PID loop.
969     * <p>
970     * This refreshes and returns a cached StatusSignal object.
971     * 
972     * @return ClosedLoopError Status Signal object
973     */
974    public final StatusSignal<Double> getAverageClosedLoopError() {
975        return getAverageClosedLoopError(true);
976    }
977    /**
978     * The difference between target average reference and current measurement.
979     * <p>
980     * This is the value that is treated as the error in the PID loop.
981     * <p>
982     * This refreshes and returns a cached StatusSignal object.
983     * 
984     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
985     * @return ClosedLoopError Status Signal object
986     */
987    public final StatusSignal<Double> getAverageClosedLoopError(boolean refresh) {
988        return _diffLeaderFX.getClosedLoopError(refresh);
989    }
990
991    /**
992     * Value that the differential closed loop is targeting.
993     * <p>
994     * This is the value that the differential closed loop PID controller
995     * targets (on the difference axis).
996     * <p>
997     * This refreshes and returns a cached StatusSignal object.
998     * 
999     * @return DifferentialClosedLoopReference Status Signal object
1000     */
1001    public final StatusSignal<Double> getDifferentialClosedLoopReference() {
1002        return getDifferentialClosedLoopReference(true);
1003    }
1004    /**
1005     * Value that the differential closed loop is targeting.
1006     * <p>
1007     * This is the value that the differential closed loop PID controller
1008     * targets (on the difference axis).
1009     * <p>
1010     * This refreshes and returns a cached StatusSignal object.
1011     * 
1012     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
1013     * @return DifferentialClosedLoopReference Status Signal object
1014     */
1015    public final StatusSignal<Double> getDifferentialClosedLoopReference(boolean refresh) {
1016        return _diffLeaderFX.getDifferentialClosedLoopReference(refresh);
1017    }
1018
1019    /**
1020     * Derivative of the target that the differential closed loop is
1021     * targeting.
1022     * <p>
1023     * This is the change in the closed loop reference (on the difference
1024     * axis). This may be used in the feed-forward calculation, the
1025     * derivative-error, or in application of the signage for kS.
1026     * Typically, this represents the target velocity during Motion
1027     * Magic®.
1028     * <p>
1029     * This refreshes and returns a cached StatusSignal object.
1030     * 
1031     * @return DifferentialClosedLoopReferenceSlope Status Signal object
1032     */
1033    public final StatusSignal<Double> getDifferentialClosedLoopReferenceSlope() {
1034        return getDifferentialClosedLoopReferenceSlope(true);
1035    }
1036    /**
1037     * Derivative of the target that the differential closed loop is
1038     * targeting.
1039     * <p>
1040     * This is the change in the closed loop reference (on the difference
1041     * axis). This may be used in the feed-forward calculation, the
1042     * derivative-error, or in application of the signage for kS.
1043     * Typically, this represents the target velocity during Motion
1044     * Magic®.
1045     * <p>
1046     * This refreshes and returns a cached StatusSignal object.
1047     * 
1048     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
1049     * @return DifferentialClosedLoopReferenceSlope Status Signal object
1050     */
1051    public final StatusSignal<Double> getDifferentialClosedLoopReferenceSlope(boolean refresh) {
1052        return _diffLeaderFX.getDifferentialClosedLoopReferenceSlope(refresh);
1053    }
1054
1055    /**
1056     * The difference between target differential reference and current
1057     * measurement.
1058     * <p>
1059     * This is the value that is treated as the error in the differential
1060     * PID loop (on the difference axis).
1061     * <p>
1062     * This refreshes and returns a cached StatusSignal object.
1063     * 
1064     * @return DifferentialClosedLoopError Status Signal object
1065     */
1066    public final StatusSignal<Double> getDifferentialClosedLoopError() {
1067        return getDifferentialClosedLoopError(true);
1068    }
1069    /**
1070     * The difference between target differential reference and current
1071     * measurement.
1072     * <p>
1073     * This is the value that is treated as the error in the differential
1074     * PID loop (on the difference axis).
1075     * <p>
1076     * This refreshes and returns a cached StatusSignal object.
1077     * 
1078     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
1079     * @return DifferentialClosedLoopError Status Signal object
1080     */
1081    public final StatusSignal<Double> getDifferentialClosedLoopError(boolean refresh) {
1082        return _diffLeaderFX.getDifferentialClosedLoopError(refresh);
1083    }
1084
1085    /**
1086     * Sets the position of the mechanism in rotations, assuming a differential
1087     * position of 0 rotations.
1088     * <p>
1089     * This will wait up to 0.100 seconds (100ms) by default.
1090     *
1091     * @param avgPosition The average position of the mechanism, in rotations
1092     * @return StatusCode of the set command
1093     */
1094    public final StatusCode setPosition(double avgPosition) {
1095        return setPosition(avgPosition, 0.0, 0.100);
1096    }
1097    /**
1098     * Sets the position of the mechanism in rotations.
1099     * <p>
1100     * This will wait up to 0.100 seconds (100ms) by default.
1101     *
1102     * @param avgPosition The average position of the mechanism, in rotations
1103     * @param diffPosition The differential position of the mechanism, in rotations
1104     * @return StatusCode of the set command
1105     */
1106    public final StatusCode setPosition(double avgPosition, double diffPosition) {
1107        return setPosition(avgPosition, diffPosition, 0.100);
1108    }
1109    /**
1110     * Sets the position of the mechanism in rotations.
1111     *
1112     * @param avgPosition The average position of the mechanism, in rotations
1113     * @param diffPosition The differential position of the mechanism, in rotations
1114     * @param timeoutSeconds Maximum time to wait up to in seconds
1115     * @return StatusCode of the set command
1116     */
1117    public final StatusCode setPosition(double avgPosition, double diffPosition, double timeoutSeconds) {
1118        var retval = StatusCode.OK;
1119
1120        var response = _diffLeaderFX.setPosition(avgPosition + diffPosition * _sensorToDiffRatio, timeoutSeconds);
1121        if (retval.isOK()) retval = response;
1122        response = _diffFollowerFX.setPosition(avgPosition - diffPosition * _sensorToDiffRatio, timeoutSeconds);
1123        if (retval.isOK()) retval = response;
1124
1125        return retval;
1126    }
1127
1128    /**
1129     * Sets the position of the mechanism, assuming a differential
1130     * position of 0 rotations.
1131     * <p>
1132     * This will wait up to 0.100 seconds (100ms) by default.
1133     *
1134     * @param avgPosition The average position of the mechanism
1135     * @return StatusCode of the set command
1136     */
1137    public final StatusCode setPosition(Angle avgPosition) {
1138        return setPosition(avgPosition.in(Rotations));
1139    }
1140    /**
1141     * Sets the position of the mechanism.
1142     * <p>
1143     * This will wait up to 0.100 seconds (100ms) by default.
1144     *
1145     * @param avgPosition The average position of the mechanism
1146     * @param diffPosition The differential position of the mechanism
1147     * @return StatusCode of the set command
1148     */
1149    public final StatusCode setPosition(Angle avgPosition, Angle diffPosition) {
1150        return setPosition(avgPosition.in(Rotations), diffPosition.in(Rotations));
1151    }
1152    /**
1153     * Sets the position of the mechanism.
1154     *
1155     * @param avgPosition The average position of the mechanism
1156     * @param diffPosition The differential position of the mechanism
1157     * @param timeoutSeconds Maximum time to wait up to in seconds
1158     * @return StatusCode of the set command
1159     */
1160    public final StatusCode setPosition(Angle avgPosition, Angle diffPosition, double timeoutSeconds) {
1161        return setPosition(avgPosition.in(Rotations), diffPosition.in(Rotations), timeoutSeconds);
1162    }
1163
1164    /**
1165     * Get the Talon FX that is differential leader. The differential
1166     * leader calculates the output for the differential follower. The
1167     * differential leader is also useful for fault detection, and it
1168     * reports status signals for the differential controller.
1169     *
1170     * @return Differential leader Talon FX
1171     */
1172    public final MotorT getLeader() {
1173        return _diffLeaderFX;
1174    }
1175
1176    /**
1177     * Get the Talon FX that is differential follower. The differential
1178     * follower's position and velocity are used by the differential leader
1179     * for the differential controller.
1180     *
1181     * @return Differential follower Talon FX
1182     */
1183    public final MotorT getFollower() {
1184        return _diffFollowerFX;
1185    }
1186
1187    private StatusCode beforeControl() {
1188        StatusCode retval = StatusCode.OK;
1189        if (_mechanismDisabled.getOpaque()) {
1190            /* disable the mechanism */
1191            retval = StatusCode.MechanismFaulted;
1192        }
1193
1194        if (!retval.isOK()) {
1195            /* neutral the output */
1196            setNeutralOut();
1197        }
1198        return retval;
1199    }
1200
1201    /**
1202     * Request neutral output of mechanism. The applied brake type
1203     * is determined by the NeutralMode configuration of each device.
1204     * <p>
1205     * Since the NeutralMode configuration of devices may not align, users
1206     * may prefer to use the {@link #setCoastOut()} or {@link #setStaticBrake()} method.
1207     *
1208     * @return Status Code of the request.
1209     */
1210    public final StatusCode setNeutralOut() {
1211        var retval = StatusCode.OK;
1212        {
1213            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_neutral);
1214            if (retval.isOK()) {
1215                retval = _diffLeaderFX_retval;
1216            }
1217        }
1218        {
1219            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_neutral);
1220            if (retval.isOK()) {
1221                retval = _diffFollowerFX_retval;
1222            }
1223        }
1224        return retval;
1225    }
1226
1227    /**
1228     * Request coast neutral output of mechanism. The bridge is
1229     * disabled and the rotor is allowed to coast.
1230     *
1231     * @return Status Code of the request.
1232     */
1233    public final StatusCode setCoastOut() {
1234        var retval = StatusCode.OK;
1235        {
1236            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_coast);
1237            if (retval.isOK()) {
1238                retval = _diffLeaderFX_retval;
1239            }
1240        }
1241        {
1242            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_coast);
1243            if (retval.isOK()) {
1244                retval = _diffFollowerFX_retval;
1245            }
1246        }
1247        return retval;
1248    }
1249
1250    /**
1251     * Applies full neutral-brake on the mechanism by shorting
1252     * motor leads together.
1253     *
1254     * @return Status Code of the request.
1255     */
1256    public final StatusCode setStaticBrake() {
1257        var retval = StatusCode.OK;
1258        {
1259            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_brake);
1260            if (retval.isOK()) {
1261                retval = _diffLeaderFX_retval;
1262            }
1263        }
1264        {
1265            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_brake);
1266            if (retval.isOK()) {
1267                retval = _diffFollowerFX_retval;
1268            }
1269        }
1270        return retval;
1271    }
1272
1273    private ControlRequest _diffLeaderFX_req = null;
1274    
1275    /**
1276     * Sets the control request for this mechanism.
1277     *
1278     * @param AverageRequest Average DutyCycleOut request of the mechanism.
1279     * @param DifferentialRequest Differential PositionDutyCycle request of the
1280     *                            mechanism. Note: The UpdateFreqHz parameter for
1281     *                            this control request will be ignored by the
1282     *                            control frame.
1283     * @return Status code of the request
1284     */
1285    public final StatusCode setControl(DutyCycleOut AverageRequest, PositionDutyCycle DifferentialRequest) {
1286        var retval = StatusCode.OK;
1287    
1288        retval = beforeControl();
1289        if (!retval.isOK()) {
1290            return retval;
1291        }
1292        
1293        {
1294            final Diff_DutyCycleOut_Position _diffLeaderFX_reqPtr;
1295            if (_diffLeaderFX_req instanceof Diff_DutyCycleOut_Position req) {
1296                _diffLeaderFX_reqPtr = req;
1297                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1298                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1299            } else {
1300                _diffLeaderFX_reqPtr = new Diff_DutyCycleOut_Position(AverageRequest, DifferentialRequest);
1301                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1302            }
1303            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1304            
1305            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1306            if (retval.isOK()) {
1307                retval = _diffLeaderFX_retval;
1308            }
1309        }
1310        
1311        {
1312            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1313            if (retval.isOK()) {
1314                retval = _diffFollowerFX_retval;
1315            }
1316        }
1317        
1318        return retval;
1319    }
1320    
1321    /**
1322     * Sets the control request for this mechanism.
1323     *
1324     * @param AverageRequest Average PositionDutyCycle request of the mechanism.
1325     * @param DifferentialRequest Differential PositionDutyCycle request of the
1326     *                            mechanism. Note: The UpdateFreqHz parameter for
1327     *                            this control request will be ignored by the
1328     *                            control frame.
1329     * @return Status code of the request
1330     */
1331    public final StatusCode setControl(PositionDutyCycle AverageRequest, PositionDutyCycle DifferentialRequest) {
1332        var retval = StatusCode.OK;
1333    
1334        retval = beforeControl();
1335        if (!retval.isOK()) {
1336            return retval;
1337        }
1338        
1339        {
1340            final Diff_PositionDutyCycle_Position _diffLeaderFX_reqPtr;
1341            if (_diffLeaderFX_req instanceof Diff_PositionDutyCycle_Position req) {
1342                _diffLeaderFX_reqPtr = req;
1343                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1344                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1345            } else {
1346                _diffLeaderFX_reqPtr = new Diff_PositionDutyCycle_Position(AverageRequest, DifferentialRequest);
1347                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1348            }
1349            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1350            
1351            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1352            if (retval.isOK()) {
1353                retval = _diffLeaderFX_retval;
1354            }
1355        }
1356        
1357        {
1358            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1359            if (retval.isOK()) {
1360                retval = _diffFollowerFX_retval;
1361            }
1362        }
1363        
1364        return retval;
1365    }
1366    
1367    /**
1368     * Sets the control request for this mechanism.
1369     *
1370     * @param AverageRequest Average VelocityDutyCYcle request of the mechanism.
1371     * @param DifferentialRequest Differential PositionDutyCycle request of the
1372     *                            mechanism. Note: The UpdateFreqHz parameter for
1373     *                            this control request will be ignored by the
1374     *                            control frame.
1375     * @return Status code of the request
1376     */
1377    public final StatusCode setControl(VelocityDutyCycle AverageRequest, PositionDutyCycle DifferentialRequest) {
1378        var retval = StatusCode.OK;
1379    
1380        retval = beforeControl();
1381        if (!retval.isOK()) {
1382            return retval;
1383        }
1384        
1385        {
1386            final Diff_VelocityDutyCycle_Position _diffLeaderFX_reqPtr;
1387            if (_diffLeaderFX_req instanceof Diff_VelocityDutyCycle_Position req) {
1388                _diffLeaderFX_reqPtr = req;
1389                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1390                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1391            } else {
1392                _diffLeaderFX_reqPtr = new Diff_VelocityDutyCycle_Position(AverageRequest, DifferentialRequest);
1393                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1394            }
1395            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1396            
1397            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1398            if (retval.isOK()) {
1399                retval = _diffLeaderFX_retval;
1400            }
1401        }
1402        
1403        {
1404            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1405            if (retval.isOK()) {
1406                retval = _diffFollowerFX_retval;
1407            }
1408        }
1409        
1410        return retval;
1411    }
1412    
1413    /**
1414     * Sets the control request for this mechanism.
1415     *
1416     * @param AverageRequest Average MotionMagicDutyCycle request of the mechanism.
1417     * @param DifferentialRequest Differential PositionDutyCycle request of the
1418     *                            mechanism. Note: The UpdateFreqHz parameter for
1419     *                            this control request will be ignored by the
1420     *                            control frame.
1421     * @return Status code of the request
1422     */
1423    public final StatusCode setControl(MotionMagicDutyCycle AverageRequest, PositionDutyCycle DifferentialRequest) {
1424        var retval = StatusCode.OK;
1425    
1426        retval = beforeControl();
1427        if (!retval.isOK()) {
1428            return retval;
1429        }
1430        
1431        {
1432            final Diff_MotionMagicDutyCycle_Position _diffLeaderFX_reqPtr;
1433            if (_diffLeaderFX_req instanceof Diff_MotionMagicDutyCycle_Position req) {
1434                _diffLeaderFX_reqPtr = req;
1435                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1436                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1437            } else {
1438                _diffLeaderFX_reqPtr = new Diff_MotionMagicDutyCycle_Position(AverageRequest, DifferentialRequest);
1439                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1440            }
1441            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1442            
1443            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1444            if (retval.isOK()) {
1445                retval = _diffLeaderFX_retval;
1446            }
1447        }
1448        
1449        {
1450            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1451            if (retval.isOK()) {
1452                retval = _diffFollowerFX_retval;
1453            }
1454        }
1455        
1456        return retval;
1457    }
1458    
1459    /**
1460     * Sets the control request for this mechanism.
1461     *
1462     * @param AverageRequest Average MotionMagicExpoDutyCycle request of the
1463     *                       mechanism.
1464     * @param DifferentialRequest Differential PositionDutyCycle request of the
1465     *                            mechanism. Note: The UpdateFreqHz parameter for
1466     *                            this control request will be ignored by the
1467     *                            control frame.
1468     * @return Status code of the request
1469     */
1470    public final StatusCode setControl(MotionMagicExpoDutyCycle AverageRequest, PositionDutyCycle DifferentialRequest) {
1471        var retval = StatusCode.OK;
1472    
1473        retval = beforeControl();
1474        if (!retval.isOK()) {
1475            return retval;
1476        }
1477        
1478        {
1479            final Diff_MotionMagicExpoDutyCycle_Position _diffLeaderFX_reqPtr;
1480            if (_diffLeaderFX_req instanceof Diff_MotionMagicExpoDutyCycle_Position req) {
1481                _diffLeaderFX_reqPtr = req;
1482                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1483                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1484            } else {
1485                _diffLeaderFX_reqPtr = new Diff_MotionMagicExpoDutyCycle_Position(AverageRequest, DifferentialRequest);
1486                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1487            }
1488            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1489            
1490            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1491            if (retval.isOK()) {
1492                retval = _diffLeaderFX_retval;
1493            }
1494        }
1495        
1496        {
1497            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1498            if (retval.isOK()) {
1499                retval = _diffFollowerFX_retval;
1500            }
1501        }
1502        
1503        return retval;
1504    }
1505    
1506    /**
1507     * Sets the control request for this mechanism.
1508     *
1509     * @param AverageRequest Average MotionMagicVelocityDutyCycle request of the
1510     *                       mechanism.
1511     * @param DifferentialRequest Differential PositionDutyCycle request of the
1512     *                            mechanism. Note: The UpdateFreqHz parameter for
1513     *                            this control request will be ignored by the
1514     *                            control frame.
1515     * @return Status code of the request
1516     */
1517    public final StatusCode setControl(MotionMagicVelocityDutyCycle AverageRequest, PositionDutyCycle DifferentialRequest) {
1518        var retval = StatusCode.OK;
1519    
1520        retval = beforeControl();
1521        if (!retval.isOK()) {
1522            return retval;
1523        }
1524        
1525        {
1526            final Diff_MotionMagicVelocityDutyCycle_Position _diffLeaderFX_reqPtr;
1527            if (_diffLeaderFX_req instanceof Diff_MotionMagicVelocityDutyCycle_Position req) {
1528                _diffLeaderFX_reqPtr = req;
1529                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1530                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1531            } else {
1532                _diffLeaderFX_reqPtr = new Diff_MotionMagicVelocityDutyCycle_Position(AverageRequest, DifferentialRequest);
1533                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1534            }
1535            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1536            
1537            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1538            if (retval.isOK()) {
1539                retval = _diffLeaderFX_retval;
1540            }
1541        }
1542        
1543        {
1544            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1545            if (retval.isOK()) {
1546                retval = _diffFollowerFX_retval;
1547            }
1548        }
1549        
1550        return retval;
1551    }
1552    
1553    /**
1554     * Sets the control request for this mechanism.
1555     *
1556     * @param AverageRequest Average DutyCycleOut request of the mechanism.
1557     * @param DifferentialRequest Differential VelocityDutyCycle request of the
1558     *                            mechanism. Note: The UpdateFreqHz parameter for
1559     *                            this control request will be ignored by the
1560     *                            control frame.
1561     * @return Status code of the request
1562     */
1563    public final StatusCode setControl(DutyCycleOut AverageRequest, VelocityDutyCycle DifferentialRequest) {
1564        var retval = StatusCode.OK;
1565    
1566        retval = beforeControl();
1567        if (!retval.isOK()) {
1568            return retval;
1569        }
1570        
1571        {
1572            final Diff_DutyCycleOut_Velocity _diffLeaderFX_reqPtr;
1573            if (_diffLeaderFX_req instanceof Diff_DutyCycleOut_Velocity req) {
1574                _diffLeaderFX_reqPtr = req;
1575                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1576                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1577            } else {
1578                _diffLeaderFX_reqPtr = new Diff_DutyCycleOut_Velocity(AverageRequest, DifferentialRequest);
1579                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1580            }
1581            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1582            
1583            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1584            if (retval.isOK()) {
1585                retval = _diffLeaderFX_retval;
1586            }
1587        }
1588        
1589        {
1590            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1591            if (retval.isOK()) {
1592                retval = _diffFollowerFX_retval;
1593            }
1594        }
1595        
1596        return retval;
1597    }
1598    
1599    /**
1600     * Sets the control request for this mechanism.
1601     *
1602     * @param AverageRequest Average PositionDutyCycle request of the mechanism.
1603     * @param DifferentialRequest Differential VelocityDutyCycle request of the
1604     *                            mechanism. Note: The UpdateFreqHz parameter for
1605     *                            this control request will be ignored by the
1606     *                            control frame.
1607     * @return Status code of the request
1608     */
1609    public final StatusCode setControl(PositionDutyCycle AverageRequest, VelocityDutyCycle DifferentialRequest) {
1610        var retval = StatusCode.OK;
1611    
1612        retval = beforeControl();
1613        if (!retval.isOK()) {
1614            return retval;
1615        }
1616        
1617        {
1618            final Diff_PositionDutyCycle_Velocity _diffLeaderFX_reqPtr;
1619            if (_diffLeaderFX_req instanceof Diff_PositionDutyCycle_Velocity req) {
1620                _diffLeaderFX_reqPtr = req;
1621                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1622                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1623            } else {
1624                _diffLeaderFX_reqPtr = new Diff_PositionDutyCycle_Velocity(AverageRequest, DifferentialRequest);
1625                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1626            }
1627            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1628            
1629            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1630            if (retval.isOK()) {
1631                retval = _diffLeaderFX_retval;
1632            }
1633        }
1634        
1635        {
1636            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1637            if (retval.isOK()) {
1638                retval = _diffFollowerFX_retval;
1639            }
1640        }
1641        
1642        return retval;
1643    }
1644    
1645    /**
1646     * Sets the control request for this mechanism.
1647     *
1648     * @param AverageRequest Average VelocityDutyCycle request of the mechanism.
1649     * @param DifferentialRequest Differential VelocityDutyCycle request of the
1650     *                            mechanism. Note: The UpdateFreqHz parameter for
1651     *                            this control request will be ignored by the
1652     *                            control frame.
1653     * @return Status code of the request
1654     */
1655    public final StatusCode setControl(VelocityDutyCycle AverageRequest, VelocityDutyCycle DifferentialRequest) {
1656        var retval = StatusCode.OK;
1657    
1658        retval = beforeControl();
1659        if (!retval.isOK()) {
1660            return retval;
1661        }
1662        
1663        {
1664            final Diff_VelocityDutyCycle_Velocity _diffLeaderFX_reqPtr;
1665            if (_diffLeaderFX_req instanceof Diff_VelocityDutyCycle_Velocity req) {
1666                _diffLeaderFX_reqPtr = req;
1667                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1668                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1669            } else {
1670                _diffLeaderFX_reqPtr = new Diff_VelocityDutyCycle_Velocity(AverageRequest, DifferentialRequest);
1671                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1672            }
1673            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1674            
1675            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1676            if (retval.isOK()) {
1677                retval = _diffLeaderFX_retval;
1678            }
1679        }
1680        
1681        {
1682            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1683            if (retval.isOK()) {
1684                retval = _diffFollowerFX_retval;
1685            }
1686        }
1687        
1688        return retval;
1689    }
1690    
1691    /**
1692     * Sets the control request for this mechanism.
1693     *
1694     * @param AverageRequest Average MotionMagicDutyCycle request of the mechanism.
1695     * @param DifferentialRequest Differential VelocityDutyCycle request of the
1696     *                            mechanism. Note: The UpdateFreqHz parameter for
1697     *                            this control request will be ignored by the
1698     *                            control frame.
1699     * @return Status code of the request
1700     */
1701    public final StatusCode setControl(MotionMagicDutyCycle AverageRequest, VelocityDutyCycle DifferentialRequest) {
1702        var retval = StatusCode.OK;
1703    
1704        retval = beforeControl();
1705        if (!retval.isOK()) {
1706            return retval;
1707        }
1708        
1709        {
1710            final Diff_MotionMagicDutyCycle_Velocity _diffLeaderFX_reqPtr;
1711            if (_diffLeaderFX_req instanceof Diff_MotionMagicDutyCycle_Velocity req) {
1712                _diffLeaderFX_reqPtr = req;
1713                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1714                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1715            } else {
1716                _diffLeaderFX_reqPtr = new Diff_MotionMagicDutyCycle_Velocity(AverageRequest, DifferentialRequest);
1717                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1718            }
1719            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1720            
1721            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1722            if (retval.isOK()) {
1723                retval = _diffLeaderFX_retval;
1724            }
1725        }
1726        
1727        {
1728            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1729            if (retval.isOK()) {
1730                retval = _diffFollowerFX_retval;
1731            }
1732        }
1733        
1734        return retval;
1735    }
1736    
1737    /**
1738     * Sets the control request for this mechanism.
1739     *
1740     * @param AverageRequest Average MotionMagicExpoDutyCycle request of the
1741     *                       mechanism.
1742     * @param DifferentialRequest Differential VelocityDutyCycle request of the
1743     *                            mechanism. Note: The UpdateFreqHz parameter for
1744     *                            this control request will be ignored by the
1745     *                            control frame.
1746     * @return Status code of the request
1747     */
1748    public final StatusCode setControl(MotionMagicExpoDutyCycle AverageRequest, VelocityDutyCycle DifferentialRequest) {
1749        var retval = StatusCode.OK;
1750    
1751        retval = beforeControl();
1752        if (!retval.isOK()) {
1753            return retval;
1754        }
1755        
1756        {
1757            final Diff_MotionMagicExpoDutyCycle_Velocity _diffLeaderFX_reqPtr;
1758            if (_diffLeaderFX_req instanceof Diff_MotionMagicExpoDutyCycle_Velocity req) {
1759                _diffLeaderFX_reqPtr = req;
1760                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1761                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1762            } else {
1763                _diffLeaderFX_reqPtr = new Diff_MotionMagicExpoDutyCycle_Velocity(AverageRequest, DifferentialRequest);
1764                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1765            }
1766            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1767            
1768            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1769            if (retval.isOK()) {
1770                retval = _diffLeaderFX_retval;
1771            }
1772        }
1773        
1774        {
1775            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1776            if (retval.isOK()) {
1777                retval = _diffFollowerFX_retval;
1778            }
1779        }
1780        
1781        return retval;
1782    }
1783    
1784    /**
1785     * Sets the control request for this mechanism.
1786     *
1787     * @param AverageRequest Average MotionMagicVelocityDutyCycle request of the
1788     *                       mechanism.
1789     * @param DifferentialRequest Differential VelocityDutyCycle request of the
1790     *                            mechanism. Note: The UpdateFreqHz parameter for
1791     *                            this control request will be ignored by the
1792     *                            control frame.
1793     * @return Status code of the request
1794     */
1795    public final StatusCode setControl(MotionMagicVelocityDutyCycle AverageRequest, VelocityDutyCycle DifferentialRequest) {
1796        var retval = StatusCode.OK;
1797    
1798        retval = beforeControl();
1799        if (!retval.isOK()) {
1800            return retval;
1801        }
1802        
1803        {
1804            final Diff_MotionMagicVelocityDutyCycle_Velocity _diffLeaderFX_reqPtr;
1805            if (_diffLeaderFX_req instanceof Diff_MotionMagicVelocityDutyCycle_Velocity req) {
1806                _diffLeaderFX_reqPtr = req;
1807                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1808                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1809            } else {
1810                _diffLeaderFX_reqPtr = new Diff_MotionMagicVelocityDutyCycle_Velocity(AverageRequest, DifferentialRequest);
1811                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1812            }
1813            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1814            
1815            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1816            if (retval.isOK()) {
1817                retval = _diffLeaderFX_retval;
1818            }
1819        }
1820        
1821        {
1822            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1823            if (retval.isOK()) {
1824                retval = _diffFollowerFX_retval;
1825            }
1826        }
1827        
1828        return retval;
1829    }
1830    
1831    /**
1832     * Sets the control request for this mechanism.
1833     *
1834     * @param AverageRequest Average DutyCycleOut request of the mechanism.
1835     * @param DifferentialRequest Differential DutyCycleOut request of the
1836     *                            mechanism. Note: The UpdateFreqHz parameter for
1837     *                            this control request will be ignored by the
1838     *                            control frame.
1839     * @return Status code of the request
1840     */
1841    public final StatusCode setControl(DutyCycleOut AverageRequest, DutyCycleOut DifferentialRequest) {
1842        var retval = StatusCode.OK;
1843    
1844        retval = beforeControl();
1845        if (!retval.isOK()) {
1846            return retval;
1847        }
1848        
1849        {
1850            final Diff_DutyCycleOut_Open _diffLeaderFX_reqPtr;
1851            if (_diffLeaderFX_req instanceof Diff_DutyCycleOut_Open req) {
1852                _diffLeaderFX_reqPtr = req;
1853                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1854                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1855            } else {
1856                _diffLeaderFX_reqPtr = new Diff_DutyCycleOut_Open(AverageRequest, DifferentialRequest);
1857                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1858            }
1859            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1860            
1861            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1862            if (retval.isOK()) {
1863                retval = _diffLeaderFX_retval;
1864            }
1865        }
1866        
1867        {
1868            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1869            if (retval.isOK()) {
1870                retval = _diffFollowerFX_retval;
1871            }
1872        }
1873        
1874        return retval;
1875    }
1876    
1877    /**
1878     * Sets the control request for this mechanism.
1879     *
1880     * @param AverageRequest Average PositionDutyCycle request of the mechanism.
1881     * @param DifferentialRequest Differential DutyCycleOut request of the
1882     *                            mechanism. Note: The UpdateFreqHz parameter for
1883     *                            this control request will be ignored by the
1884     *                            control frame.
1885     * @return Status code of the request
1886     */
1887    public final StatusCode setControl(PositionDutyCycle AverageRequest, DutyCycleOut DifferentialRequest) {
1888        var retval = StatusCode.OK;
1889    
1890        retval = beforeControl();
1891        if (!retval.isOK()) {
1892            return retval;
1893        }
1894        
1895        {
1896            final Diff_PositionDutyCycle_Open _diffLeaderFX_reqPtr;
1897            if (_diffLeaderFX_req instanceof Diff_PositionDutyCycle_Open req) {
1898                _diffLeaderFX_reqPtr = req;
1899                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1900                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1901            } else {
1902                _diffLeaderFX_reqPtr = new Diff_PositionDutyCycle_Open(AverageRequest, DifferentialRequest);
1903                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1904            }
1905            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1906            
1907            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1908            if (retval.isOK()) {
1909                retval = _diffLeaderFX_retval;
1910            }
1911        }
1912        
1913        {
1914            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1915            if (retval.isOK()) {
1916                retval = _diffFollowerFX_retval;
1917            }
1918        }
1919        
1920        return retval;
1921    }
1922    
1923    /**
1924     * Sets the control request for this mechanism.
1925     *
1926     * @param AverageRequest Average VelocityDutyCYcle request of the mechanism.
1927     * @param DifferentialRequest Differential DutyCycleOut request of the
1928     *                            mechanism. Note: The UpdateFreqHz parameter for
1929     *                            this control request will be ignored by the
1930     *                            control frame.
1931     * @return Status code of the request
1932     */
1933    public final StatusCode setControl(VelocityDutyCycle AverageRequest, DutyCycleOut DifferentialRequest) {
1934        var retval = StatusCode.OK;
1935    
1936        retval = beforeControl();
1937        if (!retval.isOK()) {
1938            return retval;
1939        }
1940        
1941        {
1942            final Diff_VelocityDutyCycle_Open _diffLeaderFX_reqPtr;
1943            if (_diffLeaderFX_req instanceof Diff_VelocityDutyCycle_Open req) {
1944                _diffLeaderFX_reqPtr = req;
1945                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1946                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1947            } else {
1948                _diffLeaderFX_reqPtr = new Diff_VelocityDutyCycle_Open(AverageRequest, DifferentialRequest);
1949                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1950            }
1951            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1952            
1953            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
1954            if (retval.isOK()) {
1955                retval = _diffLeaderFX_retval;
1956            }
1957        }
1958        
1959        {
1960            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
1961            if (retval.isOK()) {
1962                retval = _diffFollowerFX_retval;
1963            }
1964        }
1965        
1966        return retval;
1967    }
1968    
1969    /**
1970     * Sets the control request for this mechanism.
1971     *
1972     * @param AverageRequest Average MotionMagicDutyCycle request of the mechanism.
1973     * @param DifferentialRequest Differential DutyCycleOut request of the
1974     *                            mechanism. Note: The UpdateFreqHz parameter for
1975     *                            this control request will be ignored by the
1976     *                            control frame.
1977     * @return Status code of the request
1978     */
1979    public final StatusCode setControl(MotionMagicDutyCycle AverageRequest, DutyCycleOut DifferentialRequest) {
1980        var retval = StatusCode.OK;
1981    
1982        retval = beforeControl();
1983        if (!retval.isOK()) {
1984            return retval;
1985        }
1986        
1987        {
1988            final Diff_MotionMagicDutyCycle_Open _diffLeaderFX_reqPtr;
1989            if (_diffLeaderFX_req instanceof Diff_MotionMagicDutyCycle_Open req) {
1990                _diffLeaderFX_reqPtr = req;
1991                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
1992                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
1993            } else {
1994                _diffLeaderFX_reqPtr = new Diff_MotionMagicDutyCycle_Open(AverageRequest, DifferentialRequest);
1995                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
1996            }
1997            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
1998            
1999            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2000            if (retval.isOK()) {
2001                retval = _diffLeaderFX_retval;
2002            }
2003        }
2004        
2005        {
2006            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2007            if (retval.isOK()) {
2008                retval = _diffFollowerFX_retval;
2009            }
2010        }
2011        
2012        return retval;
2013    }
2014    
2015    /**
2016     * Sets the control request for this mechanism.
2017     *
2018     * @param AverageRequest Average MotionMagicExpoDutyCycle request of the
2019     *                       mechanism.
2020     * @param DifferentialRequest Differential DutyCycleOut request of the
2021     *                            mechanism. Note: The UpdateFreqHz parameter for
2022     *                            this control request will be ignored by the
2023     *                            control frame.
2024     * @return Status code of the request
2025     */
2026    public final StatusCode setControl(MotionMagicExpoDutyCycle AverageRequest, DutyCycleOut DifferentialRequest) {
2027        var retval = StatusCode.OK;
2028    
2029        retval = beforeControl();
2030        if (!retval.isOK()) {
2031            return retval;
2032        }
2033        
2034        {
2035            final Diff_MotionMagicExpoDutyCycle_Open _diffLeaderFX_reqPtr;
2036            if (_diffLeaderFX_req instanceof Diff_MotionMagicExpoDutyCycle_Open req) {
2037                _diffLeaderFX_reqPtr = req;
2038                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2039                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2040            } else {
2041                _diffLeaderFX_reqPtr = new Diff_MotionMagicExpoDutyCycle_Open(AverageRequest, DifferentialRequest);
2042                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2043            }
2044            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2045            
2046            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2047            if (retval.isOK()) {
2048                retval = _diffLeaderFX_retval;
2049            }
2050        }
2051        
2052        {
2053            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2054            if (retval.isOK()) {
2055                retval = _diffFollowerFX_retval;
2056            }
2057        }
2058        
2059        return retval;
2060    }
2061    
2062    /**
2063     * Sets the control request for this mechanism.
2064     *
2065     * @param AverageRequest Average MotionMagicVelocityDutyCycle request of the
2066     *                       mechanism.
2067     * @param DifferentialRequest Differential DutyCycleOut request of the
2068     *                            mechanism. Note: The UpdateFreqHz parameter for
2069     *                            this control request will be ignored by the
2070     *                            control frame.
2071     * @return Status code of the request
2072     */
2073    public final StatusCode setControl(MotionMagicVelocityDutyCycle AverageRequest, DutyCycleOut DifferentialRequest) {
2074        var retval = StatusCode.OK;
2075    
2076        retval = beforeControl();
2077        if (!retval.isOK()) {
2078            return retval;
2079        }
2080        
2081        {
2082            final Diff_MotionMagicVelocityDutyCycle_Open _diffLeaderFX_reqPtr;
2083            if (_diffLeaderFX_req instanceof Diff_MotionMagicVelocityDutyCycle_Open req) {
2084                _diffLeaderFX_reqPtr = req;
2085                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2086                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2087            } else {
2088                _diffLeaderFX_reqPtr = new Diff_MotionMagicVelocityDutyCycle_Open(AverageRequest, DifferentialRequest);
2089                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2090            }
2091            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2092            
2093            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2094            if (retval.isOK()) {
2095                retval = _diffLeaderFX_retval;
2096            }
2097        }
2098        
2099        {
2100            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2101            if (retval.isOK()) {
2102                retval = _diffFollowerFX_retval;
2103            }
2104        }
2105        
2106        return retval;
2107    }
2108    
2109    /**
2110     * Sets the control request for this mechanism.
2111     *
2112     * @param AverageRequest Average VoltageOut request of the mechanism.
2113     * @param DifferentialRequest Differential PositionVoltage request of the
2114     *                            mechanism. Note: The UpdateFreqHz parameter for
2115     *                            this control request will be ignored by the
2116     *                            control frame.
2117     * @return Status code of the request
2118     */
2119    public final StatusCode setControl(VoltageOut AverageRequest, PositionVoltage DifferentialRequest) {
2120        var retval = StatusCode.OK;
2121    
2122        retval = beforeControl();
2123        if (!retval.isOK()) {
2124            return retval;
2125        }
2126        
2127        {
2128            final Diff_VoltageOut_Position _diffLeaderFX_reqPtr;
2129            if (_diffLeaderFX_req instanceof Diff_VoltageOut_Position req) {
2130                _diffLeaderFX_reqPtr = req;
2131                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2132                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2133            } else {
2134                _diffLeaderFX_reqPtr = new Diff_VoltageOut_Position(AverageRequest, DifferentialRequest);
2135                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2136            }
2137            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2138            
2139            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2140            if (retval.isOK()) {
2141                retval = _diffLeaderFX_retval;
2142            }
2143        }
2144        
2145        {
2146            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2147            if (retval.isOK()) {
2148                retval = _diffFollowerFX_retval;
2149            }
2150        }
2151        
2152        return retval;
2153    }
2154    
2155    /**
2156     * Sets the control request for this mechanism.
2157     *
2158     * @param AverageRequest Average PositionVoltage request of the mechanism.
2159     * @param DifferentialRequest Differential PositionVoltage request of the
2160     *                            mechanism. Note: The UpdateFreqHz parameter for
2161     *                            this control request will be ignored by the
2162     *                            control frame.
2163     * @return Status code of the request
2164     */
2165    public final StatusCode setControl(PositionVoltage AverageRequest, PositionVoltage DifferentialRequest) {
2166        var retval = StatusCode.OK;
2167    
2168        retval = beforeControl();
2169        if (!retval.isOK()) {
2170            return retval;
2171        }
2172        
2173        {
2174            final Diff_PositionVoltage_Position _diffLeaderFX_reqPtr;
2175            if (_diffLeaderFX_req instanceof Diff_PositionVoltage_Position req) {
2176                _diffLeaderFX_reqPtr = req;
2177                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2178                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2179            } else {
2180                _diffLeaderFX_reqPtr = new Diff_PositionVoltage_Position(AverageRequest, DifferentialRequest);
2181                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2182            }
2183            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2184            
2185            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2186            if (retval.isOK()) {
2187                retval = _diffLeaderFX_retval;
2188            }
2189        }
2190        
2191        {
2192            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2193            if (retval.isOK()) {
2194                retval = _diffFollowerFX_retval;
2195            }
2196        }
2197        
2198        return retval;
2199    }
2200    
2201    /**
2202     * Sets the control request for this mechanism.
2203     *
2204     * @param AverageRequest Average VelocityVoltage request of the mechanism.
2205     * @param DifferentialRequest Differential PositionVoltage request of the
2206     *                            mechanism. Note: The UpdateFreqHz parameter for
2207     *                            this control request will be ignored by the
2208     *                            control frame.
2209     * @return Status code of the request
2210     */
2211    public final StatusCode setControl(VelocityVoltage AverageRequest, PositionVoltage DifferentialRequest) {
2212        var retval = StatusCode.OK;
2213    
2214        retval = beforeControl();
2215        if (!retval.isOK()) {
2216            return retval;
2217        }
2218        
2219        {
2220            final Diff_VelocityVoltage_Position _diffLeaderFX_reqPtr;
2221            if (_diffLeaderFX_req instanceof Diff_VelocityVoltage_Position req) {
2222                _diffLeaderFX_reqPtr = req;
2223                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2224                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2225            } else {
2226                _diffLeaderFX_reqPtr = new Diff_VelocityVoltage_Position(AverageRequest, DifferentialRequest);
2227                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2228            }
2229            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2230            
2231            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2232            if (retval.isOK()) {
2233                retval = _diffLeaderFX_retval;
2234            }
2235        }
2236        
2237        {
2238            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2239            if (retval.isOK()) {
2240                retval = _diffFollowerFX_retval;
2241            }
2242        }
2243        
2244        return retval;
2245    }
2246    
2247    /**
2248     * Sets the control request for this mechanism.
2249     *
2250     * @param AverageRequest Average MotionMagicVoltage request of the mechanism.
2251     * @param DifferentialRequest Differential PositionVoltage request of the
2252     *                            mechanism. Note: The UpdateFreqHz parameter for
2253     *                            this control request will be ignored by the
2254     *                            control frame.
2255     * @return Status code of the request
2256     */
2257    public final StatusCode setControl(MotionMagicVoltage AverageRequest, PositionVoltage DifferentialRequest) {
2258        var retval = StatusCode.OK;
2259    
2260        retval = beforeControl();
2261        if (!retval.isOK()) {
2262            return retval;
2263        }
2264        
2265        {
2266            final Diff_MotionMagicVoltage_Position _diffLeaderFX_reqPtr;
2267            if (_diffLeaderFX_req instanceof Diff_MotionMagicVoltage_Position req) {
2268                _diffLeaderFX_reqPtr = req;
2269                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2270                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2271            } else {
2272                _diffLeaderFX_reqPtr = new Diff_MotionMagicVoltage_Position(AverageRequest, DifferentialRequest);
2273                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2274            }
2275            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2276            
2277            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2278            if (retval.isOK()) {
2279                retval = _diffLeaderFX_retval;
2280            }
2281        }
2282        
2283        {
2284            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2285            if (retval.isOK()) {
2286                retval = _diffFollowerFX_retval;
2287            }
2288        }
2289        
2290        return retval;
2291    }
2292    
2293    /**
2294     * Sets the control request for this mechanism.
2295     *
2296     * @param AverageRequest Average MotionMagicExpoVoltage request of the
2297     *                       mechanism.
2298     * @param DifferentialRequest Differential PositionVoltage request of the
2299     *                            mechanism. Note: The UpdateFreqHz parameter for
2300     *                            this control request will be ignored by the
2301     *                            control frame.
2302     * @return Status code of the request
2303     */
2304    public final StatusCode setControl(MotionMagicExpoVoltage AverageRequest, PositionVoltage DifferentialRequest) {
2305        var retval = StatusCode.OK;
2306    
2307        retval = beforeControl();
2308        if (!retval.isOK()) {
2309            return retval;
2310        }
2311        
2312        {
2313            final Diff_MotionMagicExpoVoltage_Position _diffLeaderFX_reqPtr;
2314            if (_diffLeaderFX_req instanceof Diff_MotionMagicExpoVoltage_Position req) {
2315                _diffLeaderFX_reqPtr = req;
2316                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2317                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2318            } else {
2319                _diffLeaderFX_reqPtr = new Diff_MotionMagicExpoVoltage_Position(AverageRequest, DifferentialRequest);
2320                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2321            }
2322            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2323            
2324            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2325            if (retval.isOK()) {
2326                retval = _diffLeaderFX_retval;
2327            }
2328        }
2329        
2330        {
2331            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2332            if (retval.isOK()) {
2333                retval = _diffFollowerFX_retval;
2334            }
2335        }
2336        
2337        return retval;
2338    }
2339    
2340    /**
2341     * Sets the control request for this mechanism.
2342     *
2343     * @param AverageRequest Average MotionMagicVelocityVoltage request of the
2344     *                       mechanism.
2345     * @param DifferentialRequest Differential PositionVoltage request of the
2346     *                            mechanism. Note: The UpdateFreqHz parameter for
2347     *                            this control request will be ignored by the
2348     *                            control frame.
2349     * @return Status code of the request
2350     */
2351    public final StatusCode setControl(MotionMagicVelocityVoltage AverageRequest, PositionVoltage DifferentialRequest) {
2352        var retval = StatusCode.OK;
2353    
2354        retval = beforeControl();
2355        if (!retval.isOK()) {
2356            return retval;
2357        }
2358        
2359        {
2360            final Diff_MotionMagicVelocityVoltage_Position _diffLeaderFX_reqPtr;
2361            if (_diffLeaderFX_req instanceof Diff_MotionMagicVelocityVoltage_Position req) {
2362                _diffLeaderFX_reqPtr = req;
2363                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2364                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2365            } else {
2366                _diffLeaderFX_reqPtr = new Diff_MotionMagicVelocityVoltage_Position(AverageRequest, DifferentialRequest);
2367                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2368            }
2369            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2370            
2371            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2372            if (retval.isOK()) {
2373                retval = _diffLeaderFX_retval;
2374            }
2375        }
2376        
2377        {
2378            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2379            if (retval.isOK()) {
2380                retval = _diffFollowerFX_retval;
2381            }
2382        }
2383        
2384        return retval;
2385    }
2386    
2387    /**
2388     * Sets the control request for this mechanism.
2389     *
2390     * @param AverageRequest Average VoltageOut request of the mechanism.
2391     * @param DifferentialRequest Differential VelocityVoltage request of the
2392     *                            mechanism. Note: The UpdateFreqHz parameter for
2393     *                            this control request will be ignored by the
2394     *                            control frame.
2395     * @return Status code of the request
2396     */
2397    public final StatusCode setControl(VoltageOut AverageRequest, VelocityVoltage DifferentialRequest) {
2398        var retval = StatusCode.OK;
2399    
2400        retval = beforeControl();
2401        if (!retval.isOK()) {
2402            return retval;
2403        }
2404        
2405        {
2406            final Diff_VoltageOut_Velocity _diffLeaderFX_reqPtr;
2407            if (_diffLeaderFX_req instanceof Diff_VoltageOut_Velocity req) {
2408                _diffLeaderFX_reqPtr = req;
2409                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2410                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2411            } else {
2412                _diffLeaderFX_reqPtr = new Diff_VoltageOut_Velocity(AverageRequest, DifferentialRequest);
2413                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2414            }
2415            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2416            
2417            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2418            if (retval.isOK()) {
2419                retval = _diffLeaderFX_retval;
2420            }
2421        }
2422        
2423        {
2424            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2425            if (retval.isOK()) {
2426                retval = _diffFollowerFX_retval;
2427            }
2428        }
2429        
2430        return retval;
2431    }
2432    
2433    /**
2434     * Sets the control request for this mechanism.
2435     *
2436     * @param AverageRequest Average PositionVoltage request of the mechanism.
2437     * @param DifferentialRequest Differential VelocityVoltage request of the
2438     *                            mechanism. Note: The UpdateFreqHz parameter for
2439     *                            this control request will be ignored by the
2440     *                            control frame.
2441     * @return Status code of the request
2442     */
2443    public final StatusCode setControl(PositionVoltage AverageRequest, VelocityVoltage DifferentialRequest) {
2444        var retval = StatusCode.OK;
2445    
2446        retval = beforeControl();
2447        if (!retval.isOK()) {
2448            return retval;
2449        }
2450        
2451        {
2452            final Diff_PositionVoltage_Velocity _diffLeaderFX_reqPtr;
2453            if (_diffLeaderFX_req instanceof Diff_PositionVoltage_Velocity req) {
2454                _diffLeaderFX_reqPtr = req;
2455                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2456                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2457            } else {
2458                _diffLeaderFX_reqPtr = new Diff_PositionVoltage_Velocity(AverageRequest, DifferentialRequest);
2459                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2460            }
2461            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2462            
2463            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2464            if (retval.isOK()) {
2465                retval = _diffLeaderFX_retval;
2466            }
2467        }
2468        
2469        {
2470            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2471            if (retval.isOK()) {
2472                retval = _diffFollowerFX_retval;
2473            }
2474        }
2475        
2476        return retval;
2477    }
2478    
2479    /**
2480     * Sets the control request for this mechanism.
2481     *
2482     * @param AverageRequest Average VelocityVoltage request of the mechanism.
2483     * @param DifferentialRequest Differential VelocityVoltage request of the
2484     *                            mechanism. Note: The UpdateFreqHz parameter for
2485     *                            this control request will be ignored by the
2486     *                            control frame.
2487     * @return Status code of the request
2488     */
2489    public final StatusCode setControl(VelocityVoltage AverageRequest, VelocityVoltage DifferentialRequest) {
2490        var retval = StatusCode.OK;
2491    
2492        retval = beforeControl();
2493        if (!retval.isOK()) {
2494            return retval;
2495        }
2496        
2497        {
2498            final Diff_VelocityVoltage_Velocity _diffLeaderFX_reqPtr;
2499            if (_diffLeaderFX_req instanceof Diff_VelocityVoltage_Velocity req) {
2500                _diffLeaderFX_reqPtr = req;
2501                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2502                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2503            } else {
2504                _diffLeaderFX_reqPtr = new Diff_VelocityVoltage_Velocity(AverageRequest, DifferentialRequest);
2505                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2506            }
2507            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2508            
2509            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2510            if (retval.isOK()) {
2511                retval = _diffLeaderFX_retval;
2512            }
2513        }
2514        
2515        {
2516            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2517            if (retval.isOK()) {
2518                retval = _diffFollowerFX_retval;
2519            }
2520        }
2521        
2522        return retval;
2523    }
2524    
2525    /**
2526     * Sets the control request for this mechanism.
2527     *
2528     * @param AverageRequest Average MotionMagicVoltage request of the mechanism.
2529     * @param DifferentialRequest Differential VelocityVoltage request of the
2530     *                            mechanism. Note: The UpdateFreqHz parameter for
2531     *                            this control request will be ignored by the
2532     *                            control frame.
2533     * @return Status code of the request
2534     */
2535    public final StatusCode setControl(MotionMagicVoltage AverageRequest, VelocityVoltage DifferentialRequest) {
2536        var retval = StatusCode.OK;
2537    
2538        retval = beforeControl();
2539        if (!retval.isOK()) {
2540            return retval;
2541        }
2542        
2543        {
2544            final Diff_MotionMagicVoltage_Velocity _diffLeaderFX_reqPtr;
2545            if (_diffLeaderFX_req instanceof Diff_MotionMagicVoltage_Velocity req) {
2546                _diffLeaderFX_reqPtr = req;
2547                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2548                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2549            } else {
2550                _diffLeaderFX_reqPtr = new Diff_MotionMagicVoltage_Velocity(AverageRequest, DifferentialRequest);
2551                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2552            }
2553            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2554            
2555            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2556            if (retval.isOK()) {
2557                retval = _diffLeaderFX_retval;
2558            }
2559        }
2560        
2561        {
2562            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2563            if (retval.isOK()) {
2564                retval = _diffFollowerFX_retval;
2565            }
2566        }
2567        
2568        return retval;
2569    }
2570    
2571    /**
2572     * Sets the control request for this mechanism.
2573     *
2574     * @param AverageRequest Average MotionMagicExpoVoltage request of the
2575     *                       mechanism.
2576     * @param DifferentialRequest Differential VelocityVoltage request of the
2577     *                            mechanism. Note: The UpdateFreqHz parameter for
2578     *                            this control request will be ignored by the
2579     *                            control frame.
2580     * @return Status code of the request
2581     */
2582    public final StatusCode setControl(MotionMagicExpoVoltage AverageRequest, VelocityVoltage DifferentialRequest) {
2583        var retval = StatusCode.OK;
2584    
2585        retval = beforeControl();
2586        if (!retval.isOK()) {
2587            return retval;
2588        }
2589        
2590        {
2591            final Diff_MotionMagicExpoVoltage_Velocity _diffLeaderFX_reqPtr;
2592            if (_diffLeaderFX_req instanceof Diff_MotionMagicExpoVoltage_Velocity req) {
2593                _diffLeaderFX_reqPtr = req;
2594                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2595                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2596            } else {
2597                _diffLeaderFX_reqPtr = new Diff_MotionMagicExpoVoltage_Velocity(AverageRequest, DifferentialRequest);
2598                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2599            }
2600            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2601            
2602            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2603            if (retval.isOK()) {
2604                retval = _diffLeaderFX_retval;
2605            }
2606        }
2607        
2608        {
2609            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2610            if (retval.isOK()) {
2611                retval = _diffFollowerFX_retval;
2612            }
2613        }
2614        
2615        return retval;
2616    }
2617    
2618    /**
2619     * Sets the control request for this mechanism.
2620     *
2621     * @param AverageRequest Average MotionMagicVelocityVoltage request of the
2622     *                       mechanism.
2623     * @param DifferentialRequest Differential VelocityVoltage request of the
2624     *                            mechanism. Note: The UpdateFreqHz parameter for
2625     *                            this control request will be ignored by the
2626     *                            control frame.
2627     * @return Status code of the request
2628     */
2629    public final StatusCode setControl(MotionMagicVelocityVoltage AverageRequest, VelocityVoltage DifferentialRequest) {
2630        var retval = StatusCode.OK;
2631    
2632        retval = beforeControl();
2633        if (!retval.isOK()) {
2634            return retval;
2635        }
2636        
2637        {
2638            final Diff_MotionMagicVelocityVoltage_Velocity _diffLeaderFX_reqPtr;
2639            if (_diffLeaderFX_req instanceof Diff_MotionMagicVelocityVoltage_Velocity req) {
2640                _diffLeaderFX_reqPtr = req;
2641                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2642                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2643            } else {
2644                _diffLeaderFX_reqPtr = new Diff_MotionMagicVelocityVoltage_Velocity(AverageRequest, DifferentialRequest);
2645                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2646            }
2647            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2648            
2649            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2650            if (retval.isOK()) {
2651                retval = _diffLeaderFX_retval;
2652            }
2653        }
2654        
2655        {
2656            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2657            if (retval.isOK()) {
2658                retval = _diffFollowerFX_retval;
2659            }
2660        }
2661        
2662        return retval;
2663    }
2664    
2665    /**
2666     * Sets the control request for this mechanism.
2667     *
2668     * @param AverageRequest Average VoltageOut request of the mechanism.
2669     * @param DifferentialRequest Differential VoltageOut request of the mechanism.
2670     *                            Note: The UpdateFreqHz parameter for this control
2671     *                            request will be ignored by the control frame.
2672     * @return Status code of the request
2673     */
2674    public final StatusCode setControl(VoltageOut AverageRequest, VoltageOut DifferentialRequest) {
2675        var retval = StatusCode.OK;
2676    
2677        retval = beforeControl();
2678        if (!retval.isOK()) {
2679            return retval;
2680        }
2681        
2682        {
2683            final Diff_VoltageOut_Open _diffLeaderFX_reqPtr;
2684            if (_diffLeaderFX_req instanceof Diff_VoltageOut_Open req) {
2685                _diffLeaderFX_reqPtr = req;
2686                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2687                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2688            } else {
2689                _diffLeaderFX_reqPtr = new Diff_VoltageOut_Open(AverageRequest, DifferentialRequest);
2690                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2691            }
2692            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2693            
2694            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2695            if (retval.isOK()) {
2696                retval = _diffLeaderFX_retval;
2697            }
2698        }
2699        
2700        {
2701            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2702            if (retval.isOK()) {
2703                retval = _diffFollowerFX_retval;
2704            }
2705        }
2706        
2707        return retval;
2708    }
2709    
2710    /**
2711     * Sets the control request for this mechanism.
2712     *
2713     * @param AverageRequest Average PositionVoltage request of the mechanism.
2714     * @param DifferentialRequest Differential VoltageOut request of the mechanism.
2715     *                            Note: The UpdateFreqHz parameter for this control
2716     *                            request will be ignored by the control frame.
2717     * @return Status code of the request
2718     */
2719    public final StatusCode setControl(PositionVoltage AverageRequest, VoltageOut DifferentialRequest) {
2720        var retval = StatusCode.OK;
2721    
2722        retval = beforeControl();
2723        if (!retval.isOK()) {
2724            return retval;
2725        }
2726        
2727        {
2728            final Diff_PositionVoltage_Open _diffLeaderFX_reqPtr;
2729            if (_diffLeaderFX_req instanceof Diff_PositionVoltage_Open req) {
2730                _diffLeaderFX_reqPtr = req;
2731                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2732                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2733            } else {
2734                _diffLeaderFX_reqPtr = new Diff_PositionVoltage_Open(AverageRequest, DifferentialRequest);
2735                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2736            }
2737            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2738            
2739            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2740            if (retval.isOK()) {
2741                retval = _diffLeaderFX_retval;
2742            }
2743        }
2744        
2745        {
2746            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2747            if (retval.isOK()) {
2748                retval = _diffFollowerFX_retval;
2749            }
2750        }
2751        
2752        return retval;
2753    }
2754    
2755    /**
2756     * Sets the control request for this mechanism.
2757     *
2758     * @param AverageRequest Average VelocityVoltage request of the mechanism.
2759     * @param DifferentialRequest Differential VoltageOut request of the mechanism.
2760     *                            Note: The UpdateFreqHz parameter for this control
2761     *                            request will be ignored by the control frame.
2762     * @return Status code of the request
2763     */
2764    public final StatusCode setControl(VelocityVoltage AverageRequest, VoltageOut DifferentialRequest) {
2765        var retval = StatusCode.OK;
2766    
2767        retval = beforeControl();
2768        if (!retval.isOK()) {
2769            return retval;
2770        }
2771        
2772        {
2773            final Diff_VelocityVoltage_Open _diffLeaderFX_reqPtr;
2774            if (_diffLeaderFX_req instanceof Diff_VelocityVoltage_Open req) {
2775                _diffLeaderFX_reqPtr = req;
2776                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2777                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2778            } else {
2779                _diffLeaderFX_reqPtr = new Diff_VelocityVoltage_Open(AverageRequest, DifferentialRequest);
2780                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2781            }
2782            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2783            
2784            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2785            if (retval.isOK()) {
2786                retval = _diffLeaderFX_retval;
2787            }
2788        }
2789        
2790        {
2791            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2792            if (retval.isOK()) {
2793                retval = _diffFollowerFX_retval;
2794            }
2795        }
2796        
2797        return retval;
2798    }
2799    
2800    /**
2801     * Sets the control request for this mechanism.
2802     *
2803     * @param AverageRequest Average MotionMagicVoltage request of the mechanism.
2804     * @param DifferentialRequest Differential VoltageOut request of the mechanism.
2805     *                            Note: The UpdateFreqHz parameter for this control
2806     *                            request will be ignored by the control frame.
2807     * @return Status code of the request
2808     */
2809    public final StatusCode setControl(MotionMagicVoltage AverageRequest, VoltageOut DifferentialRequest) {
2810        var retval = StatusCode.OK;
2811    
2812        retval = beforeControl();
2813        if (!retval.isOK()) {
2814            return retval;
2815        }
2816        
2817        {
2818            final Diff_MotionMagicVoltage_Open _diffLeaderFX_reqPtr;
2819            if (_diffLeaderFX_req instanceof Diff_MotionMagicVoltage_Open req) {
2820                _diffLeaderFX_reqPtr = req;
2821                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2822                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2823            } else {
2824                _diffLeaderFX_reqPtr = new Diff_MotionMagicVoltage_Open(AverageRequest, DifferentialRequest);
2825                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2826            }
2827            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2828            
2829            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2830            if (retval.isOK()) {
2831                retval = _diffLeaderFX_retval;
2832            }
2833        }
2834        
2835        {
2836            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2837            if (retval.isOK()) {
2838                retval = _diffFollowerFX_retval;
2839            }
2840        }
2841        
2842        return retval;
2843    }
2844    
2845    /**
2846     * Sets the control request for this mechanism.
2847     *
2848     * @param AverageRequest Average MotionMagicExpoVoltage request of the
2849     *                       mechanism.
2850     * @param DifferentialRequest Differential VoltageOut request of the mechanism.
2851     *                            Note: The UpdateFreqHz parameter for this control
2852     *                            request will be ignored by the control frame.
2853     * @return Status code of the request
2854     */
2855    public final StatusCode setControl(MotionMagicExpoVoltage AverageRequest, VoltageOut DifferentialRequest) {
2856        var retval = StatusCode.OK;
2857    
2858        retval = beforeControl();
2859        if (!retval.isOK()) {
2860            return retval;
2861        }
2862        
2863        {
2864            final Diff_MotionMagicExpoVoltage_Open _diffLeaderFX_reqPtr;
2865            if (_diffLeaderFX_req instanceof Diff_MotionMagicExpoVoltage_Open req) {
2866                _diffLeaderFX_reqPtr = req;
2867                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2868                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2869            } else {
2870                _diffLeaderFX_reqPtr = new Diff_MotionMagicExpoVoltage_Open(AverageRequest, DifferentialRequest);
2871                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2872            }
2873            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2874            
2875            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2876            if (retval.isOK()) {
2877                retval = _diffLeaderFX_retval;
2878            }
2879        }
2880        
2881        {
2882            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2883            if (retval.isOK()) {
2884                retval = _diffFollowerFX_retval;
2885            }
2886        }
2887        
2888        return retval;
2889    }
2890    
2891    /**
2892     * Sets the control request for this mechanism.
2893     *
2894     * @param AverageRequest Average MotionMagicVelocityVoltage request of the
2895     *                       mechanism.
2896     * @param DifferentialRequest Differential VoltageOut request of the mechanism.
2897     *                            Note: The UpdateFreqHz parameter for this control
2898     *                            request will be ignored by the control frame.
2899     * @return Status code of the request
2900     */
2901    public final StatusCode setControl(MotionMagicVelocityVoltage AverageRequest, VoltageOut DifferentialRequest) {
2902        var retval = StatusCode.OK;
2903    
2904        retval = beforeControl();
2905        if (!retval.isOK()) {
2906            return retval;
2907        }
2908        
2909        {
2910            final Diff_MotionMagicVelocityVoltage_Open _diffLeaderFX_reqPtr;
2911            if (_diffLeaderFX_req instanceof Diff_MotionMagicVelocityVoltage_Open req) {
2912                _diffLeaderFX_reqPtr = req;
2913                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2914                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2915            } else {
2916                _diffLeaderFX_reqPtr = new Diff_MotionMagicVelocityVoltage_Open(AverageRequest, DifferentialRequest);
2917                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2918            }
2919            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2920            
2921            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2922            if (retval.isOK()) {
2923                retval = _diffLeaderFX_retval;
2924            }
2925        }
2926        
2927        {
2928            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2929            if (retval.isOK()) {
2930                retval = _diffFollowerFX_retval;
2931            }
2932        }
2933        
2934        return retval;
2935    }
2936    
2937    /**
2938     * Sets the control request for this mechanism.
2939     *
2940     * @param AverageRequest Average TorqueCurrentFOC request of the mechanism.
2941     * @param DifferentialRequest Differential PositionTorqueCurrentFOC request of
2942     *                            the mechanism. Note: The UpdateFreqHz parameter
2943     *                            for this control request will be ignored by the
2944     *                            control frame.
2945     * @return Status code of the request
2946     */
2947    public final StatusCode setControl(TorqueCurrentFOC AverageRequest, PositionTorqueCurrentFOC DifferentialRequest) {
2948        var retval = StatusCode.OK;
2949    
2950        retval = beforeControl();
2951        if (!retval.isOK()) {
2952            return retval;
2953        }
2954        
2955        {
2956            final Diff_TorqueCurrentFOC_Position _diffLeaderFX_reqPtr;
2957            if (_diffLeaderFX_req instanceof Diff_TorqueCurrentFOC_Position req) {
2958                _diffLeaderFX_reqPtr = req;
2959                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
2960                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
2961            } else {
2962                _diffLeaderFX_reqPtr = new Diff_TorqueCurrentFOC_Position(AverageRequest, DifferentialRequest);
2963                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
2964            }
2965            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
2966            
2967            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
2968            if (retval.isOK()) {
2969                retval = _diffLeaderFX_retval;
2970            }
2971        }
2972        
2973        {
2974            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
2975            if (retval.isOK()) {
2976                retval = _diffFollowerFX_retval;
2977            }
2978        }
2979        
2980        return retval;
2981    }
2982    
2983    /**
2984     * Sets the control request for this mechanism.
2985     *
2986     * @param AverageRequest Average PositionTorqueCurrentFOC request of the
2987     *                       mechanism.
2988     * @param DifferentialRequest Differential PositionTorqueCurrentFOC request of
2989     *                            the mechanism. Note: The UpdateFreqHz parameter
2990     *                            for this control request will be ignored by the
2991     *                            control frame.
2992     * @return Status code of the request
2993     */
2994    public final StatusCode setControl(PositionTorqueCurrentFOC AverageRequest, PositionTorqueCurrentFOC DifferentialRequest) {
2995        var retval = StatusCode.OK;
2996    
2997        retval = beforeControl();
2998        if (!retval.isOK()) {
2999            return retval;
3000        }
3001        
3002        {
3003            final Diff_PositionTorqueCurrentFOC_Position _diffLeaderFX_reqPtr;
3004            if (_diffLeaderFX_req instanceof Diff_PositionTorqueCurrentFOC_Position req) {
3005                _diffLeaderFX_reqPtr = req;
3006                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3007                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3008            } else {
3009                _diffLeaderFX_reqPtr = new Diff_PositionTorqueCurrentFOC_Position(AverageRequest, DifferentialRequest);
3010                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3011            }
3012            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3013            
3014            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3015            if (retval.isOK()) {
3016                retval = _diffLeaderFX_retval;
3017            }
3018        }
3019        
3020        {
3021            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3022            if (retval.isOK()) {
3023                retval = _diffFollowerFX_retval;
3024            }
3025        }
3026        
3027        return retval;
3028    }
3029    
3030    /**
3031     * Sets the control request for this mechanism.
3032     *
3033     * @param AverageRequest Average VelocityTorqueCurrentFOC request of the
3034     *                       mechanism.
3035     * @param DifferentialRequest Differential PositionTorqueCurrentFOC request of
3036     *                            the mechanism. Note: The UpdateFreqHz parameter
3037     *                            for this control request will be ignored by the
3038     *                            control frame.
3039     * @return Status code of the request
3040     */
3041    public final StatusCode setControl(VelocityTorqueCurrentFOC AverageRequest, PositionTorqueCurrentFOC DifferentialRequest) {
3042        var retval = StatusCode.OK;
3043    
3044        retval = beforeControl();
3045        if (!retval.isOK()) {
3046            return retval;
3047        }
3048        
3049        {
3050            final Diff_VelocityTorqueCurrentFOC_Position _diffLeaderFX_reqPtr;
3051            if (_diffLeaderFX_req instanceof Diff_VelocityTorqueCurrentFOC_Position req) {
3052                _diffLeaderFX_reqPtr = req;
3053                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3054                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3055            } else {
3056                _diffLeaderFX_reqPtr = new Diff_VelocityTorqueCurrentFOC_Position(AverageRequest, DifferentialRequest);
3057                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3058            }
3059            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3060            
3061            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3062            if (retval.isOK()) {
3063                retval = _diffLeaderFX_retval;
3064            }
3065        }
3066        
3067        {
3068            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3069            if (retval.isOK()) {
3070                retval = _diffFollowerFX_retval;
3071            }
3072        }
3073        
3074        return retval;
3075    }
3076    
3077    /**
3078     * Sets the control request for this mechanism.
3079     *
3080     * @param AverageRequest Average MotionMagicTorqueCurrentFOC request of the
3081     *                       mechanism.
3082     * @param DifferentialRequest Differential PositionTorqueCurrentFOC request of
3083     *                            the mechanism. Note: The UpdateFreqHz parameter
3084     *                            for this control request will be ignored by the
3085     *                            control frame.
3086     * @return Status code of the request
3087     */
3088    public final StatusCode setControl(MotionMagicTorqueCurrentFOC AverageRequest, PositionTorqueCurrentFOC DifferentialRequest) {
3089        var retval = StatusCode.OK;
3090    
3091        retval = beforeControl();
3092        if (!retval.isOK()) {
3093            return retval;
3094        }
3095        
3096        {
3097            final Diff_MotionMagicTorqueCurrentFOC_Position _diffLeaderFX_reqPtr;
3098            if (_diffLeaderFX_req instanceof Diff_MotionMagicTorqueCurrentFOC_Position req) {
3099                _diffLeaderFX_reqPtr = req;
3100                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3101                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3102            } else {
3103                _diffLeaderFX_reqPtr = new Diff_MotionMagicTorqueCurrentFOC_Position(AverageRequest, DifferentialRequest);
3104                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3105            }
3106            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3107            
3108            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3109            if (retval.isOK()) {
3110                retval = _diffLeaderFX_retval;
3111            }
3112        }
3113        
3114        {
3115            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3116            if (retval.isOK()) {
3117                retval = _diffFollowerFX_retval;
3118            }
3119        }
3120        
3121        return retval;
3122    }
3123    
3124    /**
3125     * Sets the control request for this mechanism.
3126     *
3127     * @param AverageRequest Average MotionMagicExpoTorqueCurrentFOC request of the
3128     *                       mechanism.
3129     * @param DifferentialRequest Differential PositionTorqueCurrentFOC request of
3130     *                            the mechanism. Note: The UpdateFreqHz parameter
3131     *                            for this control request will be ignored by the
3132     *                            control frame.
3133     * @return Status code of the request
3134     */
3135    public final StatusCode setControl(MotionMagicExpoTorqueCurrentFOC AverageRequest, PositionTorqueCurrentFOC DifferentialRequest) {
3136        var retval = StatusCode.OK;
3137    
3138        retval = beforeControl();
3139        if (!retval.isOK()) {
3140            return retval;
3141        }
3142        
3143        {
3144            final Diff_MotionMagicExpoTorqueCurrentFOC_Position _diffLeaderFX_reqPtr;
3145            if (_diffLeaderFX_req instanceof Diff_MotionMagicExpoTorqueCurrentFOC_Position req) {
3146                _diffLeaderFX_reqPtr = req;
3147                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3148                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3149            } else {
3150                _diffLeaderFX_reqPtr = new Diff_MotionMagicExpoTorqueCurrentFOC_Position(AverageRequest, DifferentialRequest);
3151                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3152            }
3153            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3154            
3155            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3156            if (retval.isOK()) {
3157                retval = _diffLeaderFX_retval;
3158            }
3159        }
3160        
3161        {
3162            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3163            if (retval.isOK()) {
3164                retval = _diffFollowerFX_retval;
3165            }
3166        }
3167        
3168        return retval;
3169    }
3170    
3171    /**
3172     * Sets the control request for this mechanism.
3173     *
3174     * @param AverageRequest Average MotionMagicVelocityTorqueCurrentFOC request of
3175     *                       the mechanism.
3176     * @param DifferentialRequest Differential PositionTorqueCurrentFOC request of
3177     *                            the mechanism. Note: The UpdateFreqHz parameter
3178     *                            for this control request will be ignored by the
3179     *                            control frame.
3180     * @return Status code of the request
3181     */
3182    public final StatusCode setControl(MotionMagicVelocityTorqueCurrentFOC AverageRequest, PositionTorqueCurrentFOC DifferentialRequest) {
3183        var retval = StatusCode.OK;
3184    
3185        retval = beforeControl();
3186        if (!retval.isOK()) {
3187            return retval;
3188        }
3189        
3190        {
3191            final Diff_MotionMagicVelocityTorqueCurrentFOC_Position _diffLeaderFX_reqPtr;
3192            if (_diffLeaderFX_req instanceof Diff_MotionMagicVelocityTorqueCurrentFOC_Position req) {
3193                _diffLeaderFX_reqPtr = req;
3194                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3195                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3196            } else {
3197                _diffLeaderFX_reqPtr = new Diff_MotionMagicVelocityTorqueCurrentFOC_Position(AverageRequest, DifferentialRequest);
3198                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3199            }
3200            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3201            
3202            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3203            if (retval.isOK()) {
3204                retval = _diffLeaderFX_retval;
3205            }
3206        }
3207        
3208        {
3209            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3210            if (retval.isOK()) {
3211                retval = _diffFollowerFX_retval;
3212            }
3213        }
3214        
3215        return retval;
3216    }
3217    
3218    /**
3219     * Sets the control request for this mechanism.
3220     *
3221     * @param AverageRequest Average TorqueCurrentFOC request of the mechanism.
3222     * @param DifferentialRequest Differential VelocityTorqueCurrentFOC request of
3223     *                            the mechanism. Note: The UpdateFreqHz parameter
3224     *                            for this control request will be ignored by the
3225     *                            control frame.
3226     * @return Status code of the request
3227     */
3228    public final StatusCode setControl(TorqueCurrentFOC AverageRequest, VelocityTorqueCurrentFOC DifferentialRequest) {
3229        var retval = StatusCode.OK;
3230    
3231        retval = beforeControl();
3232        if (!retval.isOK()) {
3233            return retval;
3234        }
3235        
3236        {
3237            final Diff_TorqueCurrentFOC_Velocity _diffLeaderFX_reqPtr;
3238            if (_diffLeaderFX_req instanceof Diff_TorqueCurrentFOC_Velocity req) {
3239                _diffLeaderFX_reqPtr = req;
3240                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3241                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3242            } else {
3243                _diffLeaderFX_reqPtr = new Diff_TorqueCurrentFOC_Velocity(AverageRequest, DifferentialRequest);
3244                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3245            }
3246            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3247            
3248            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3249            if (retval.isOK()) {
3250                retval = _diffLeaderFX_retval;
3251            }
3252        }
3253        
3254        {
3255            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3256            if (retval.isOK()) {
3257                retval = _diffFollowerFX_retval;
3258            }
3259        }
3260        
3261        return retval;
3262    }
3263    
3264    /**
3265     * Sets the control request for this mechanism.
3266     *
3267     * @param AverageRequest Average PositionTorqueCurrentFOC request of the
3268     *                       mechanism.
3269     * @param DifferentialRequest Differential VelocityTorqueCurrentFOC request of
3270     *                            the mechanism. Note: The UpdateFreqHz parameter
3271     *                            for this control request will be ignored by the
3272     *                            control frame.
3273     * @return Status code of the request
3274     */
3275    public final StatusCode setControl(PositionTorqueCurrentFOC AverageRequest, VelocityTorqueCurrentFOC DifferentialRequest) {
3276        var retval = StatusCode.OK;
3277    
3278        retval = beforeControl();
3279        if (!retval.isOK()) {
3280            return retval;
3281        }
3282        
3283        {
3284            final Diff_PositionTorqueCurrentFOC_Velocity _diffLeaderFX_reqPtr;
3285            if (_diffLeaderFX_req instanceof Diff_PositionTorqueCurrentFOC_Velocity req) {
3286                _diffLeaderFX_reqPtr = req;
3287                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3288                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3289            } else {
3290                _diffLeaderFX_reqPtr = new Diff_PositionTorqueCurrentFOC_Velocity(AverageRequest, DifferentialRequest);
3291                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3292            }
3293            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3294            
3295            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3296            if (retval.isOK()) {
3297                retval = _diffLeaderFX_retval;
3298            }
3299        }
3300        
3301        {
3302            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3303            if (retval.isOK()) {
3304                retval = _diffFollowerFX_retval;
3305            }
3306        }
3307        
3308        return retval;
3309    }
3310    
3311    /**
3312     * Sets the control request for this mechanism.
3313     *
3314     * @param AverageRequest Average VelocityTorqueCurrentFOC request of the
3315     *                       mechanism.
3316     * @param DifferentialRequest Differential VelocityTorqueCurrentFOC request of
3317     *                            the mechanism. Note: The UpdateFreqHz parameter
3318     *                            for this control request will be ignored by the
3319     *                            control frame.
3320     * @return Status code of the request
3321     */
3322    public final StatusCode setControl(VelocityTorqueCurrentFOC AverageRequest, VelocityTorqueCurrentFOC DifferentialRequest) {
3323        var retval = StatusCode.OK;
3324    
3325        retval = beforeControl();
3326        if (!retval.isOK()) {
3327            return retval;
3328        }
3329        
3330        {
3331            final Diff_VelocityTorqueCurrentFOC_Velocity _diffLeaderFX_reqPtr;
3332            if (_diffLeaderFX_req instanceof Diff_VelocityTorqueCurrentFOC_Velocity req) {
3333                _diffLeaderFX_reqPtr = req;
3334                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3335                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3336            } else {
3337                _diffLeaderFX_reqPtr = new Diff_VelocityTorqueCurrentFOC_Velocity(AverageRequest, DifferentialRequest);
3338                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3339            }
3340            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3341            
3342            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3343            if (retval.isOK()) {
3344                retval = _diffLeaderFX_retval;
3345            }
3346        }
3347        
3348        {
3349            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3350            if (retval.isOK()) {
3351                retval = _diffFollowerFX_retval;
3352            }
3353        }
3354        
3355        return retval;
3356    }
3357    
3358    /**
3359     * Sets the control request for this mechanism.
3360     *
3361     * @param AverageRequest Average MotionMagicTorqueCurrentFOC request of the
3362     *                       mechanism.
3363     * @param DifferentialRequest Differential VelocityTorqueCurrentFOC request of
3364     *                            the mechanism. Note: The UpdateFreqHz parameter
3365     *                            for this control request will be ignored by the
3366     *                            control frame.
3367     * @return Status code of the request
3368     */
3369    public final StatusCode setControl(MotionMagicTorqueCurrentFOC AverageRequest, VelocityTorqueCurrentFOC DifferentialRequest) {
3370        var retval = StatusCode.OK;
3371    
3372        retval = beforeControl();
3373        if (!retval.isOK()) {
3374            return retval;
3375        }
3376        
3377        {
3378            final Diff_MotionMagicTorqueCurrentFOC_Velocity _diffLeaderFX_reqPtr;
3379            if (_diffLeaderFX_req instanceof Diff_MotionMagicTorqueCurrentFOC_Velocity req) {
3380                _diffLeaderFX_reqPtr = req;
3381                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3382                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3383            } else {
3384                _diffLeaderFX_reqPtr = new Diff_MotionMagicTorqueCurrentFOC_Velocity(AverageRequest, DifferentialRequest);
3385                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3386            }
3387            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3388            
3389            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3390            if (retval.isOK()) {
3391                retval = _diffLeaderFX_retval;
3392            }
3393        }
3394        
3395        {
3396            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3397            if (retval.isOK()) {
3398                retval = _diffFollowerFX_retval;
3399            }
3400        }
3401        
3402        return retval;
3403    }
3404    
3405    /**
3406     * Sets the control request for this mechanism.
3407     *
3408     * @param AverageRequest Average MotionMagicExpoTorqueCurrentFOC request of the
3409     *                       mechanism.
3410     * @param DifferentialRequest Differential VelocityTorqueCurrentFOC request of
3411     *                            the mechanism. Note: The UpdateFreqHz parameter
3412     *                            for this control request will be ignored by the
3413     *                            control frame.
3414     * @return Status code of the request
3415     */
3416    public final StatusCode setControl(MotionMagicExpoTorqueCurrentFOC AverageRequest, VelocityTorqueCurrentFOC DifferentialRequest) {
3417        var retval = StatusCode.OK;
3418    
3419        retval = beforeControl();
3420        if (!retval.isOK()) {
3421            return retval;
3422        }
3423        
3424        {
3425            final Diff_MotionMagicExpoTorqueCurrentFOC_Velocity _diffLeaderFX_reqPtr;
3426            if (_diffLeaderFX_req instanceof Diff_MotionMagicExpoTorqueCurrentFOC_Velocity req) {
3427                _diffLeaderFX_reqPtr = req;
3428                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3429                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3430            } else {
3431                _diffLeaderFX_reqPtr = new Diff_MotionMagicExpoTorqueCurrentFOC_Velocity(AverageRequest, DifferentialRequest);
3432                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3433            }
3434            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3435            
3436            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3437            if (retval.isOK()) {
3438                retval = _diffLeaderFX_retval;
3439            }
3440        }
3441        
3442        {
3443            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3444            if (retval.isOK()) {
3445                retval = _diffFollowerFX_retval;
3446            }
3447        }
3448        
3449        return retval;
3450    }
3451    
3452    /**
3453     * Sets the control request for this mechanism.
3454     *
3455     * @param AverageRequest Average MotionMagicVelocityTorqueCurrentFOC request of
3456     *                       the mechanism.
3457     * @param DifferentialRequest Differential VelocityTorqueCurrentFOC request of
3458     *                            the mechanism. Note: The UpdateFreqHz parameter
3459     *                            for this control request will be ignored by the
3460     *                            control frame.
3461     * @return Status code of the request
3462     */
3463    public final StatusCode setControl(MotionMagicVelocityTorqueCurrentFOC AverageRequest, VelocityTorqueCurrentFOC DifferentialRequest) {
3464        var retval = StatusCode.OK;
3465    
3466        retval = beforeControl();
3467        if (!retval.isOK()) {
3468            return retval;
3469        }
3470        
3471        {
3472            final Diff_MotionMagicVelocityTorqueCurrentFOC_Velocity _diffLeaderFX_reqPtr;
3473            if (_diffLeaderFX_req instanceof Diff_MotionMagicVelocityTorqueCurrentFOC_Velocity req) {
3474                _diffLeaderFX_reqPtr = req;
3475                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3476                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3477            } else {
3478                _diffLeaderFX_reqPtr = new Diff_MotionMagicVelocityTorqueCurrentFOC_Velocity(AverageRequest, DifferentialRequest);
3479                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3480            }
3481            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3482            
3483            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3484            if (retval.isOK()) {
3485                retval = _diffLeaderFX_retval;
3486            }
3487        }
3488        
3489        {
3490            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3491            if (retval.isOK()) {
3492                retval = _diffFollowerFX_retval;
3493            }
3494        }
3495        
3496        return retval;
3497    }
3498    
3499    /**
3500     * Sets the control request for this mechanism.
3501     *
3502     * @param AverageRequest Average TorqueCurrentFOC request of the mechanism.
3503     * @param DifferentialRequest Differential TorqueCurrentFOC request of the
3504     *                            mechanism. Note: The UpdateFreqHz parameter for
3505     *                            this control request will be ignored by the
3506     *                            control frame.
3507     * @return Status code of the request
3508     */
3509    public final StatusCode setControl(TorqueCurrentFOC AverageRequest, TorqueCurrentFOC DifferentialRequest) {
3510        var retval = StatusCode.OK;
3511    
3512        retval = beforeControl();
3513        if (!retval.isOK()) {
3514            return retval;
3515        }
3516        
3517        {
3518            final Diff_TorqueCurrentFOC_Open _diffLeaderFX_reqPtr;
3519            if (_diffLeaderFX_req instanceof Diff_TorqueCurrentFOC_Open req) {
3520                _diffLeaderFX_reqPtr = req;
3521                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3522                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3523            } else {
3524                _diffLeaderFX_reqPtr = new Diff_TorqueCurrentFOC_Open(AverageRequest, DifferentialRequest);
3525                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3526            }
3527            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3528            
3529            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3530            if (retval.isOK()) {
3531                retval = _diffLeaderFX_retval;
3532            }
3533        }
3534        
3535        {
3536            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3537            if (retval.isOK()) {
3538                retval = _diffFollowerFX_retval;
3539            }
3540        }
3541        
3542        return retval;
3543    }
3544    
3545    /**
3546     * Sets the control request for this mechanism.
3547     *
3548     * @param AverageRequest Average PositionTorqueCurrentFOC request of the
3549     *                       mechanism.
3550     * @param DifferentialRequest Differential TorqueCurrentFOC request of the
3551     *                            mechanism. Note: The UpdateFreqHz parameter for
3552     *                            this control request will be ignored by the
3553     *                            control frame.
3554     * @return Status code of the request
3555     */
3556    public final StatusCode setControl(PositionTorqueCurrentFOC AverageRequest, TorqueCurrentFOC DifferentialRequest) {
3557        var retval = StatusCode.OK;
3558    
3559        retval = beforeControl();
3560        if (!retval.isOK()) {
3561            return retval;
3562        }
3563        
3564        {
3565            final Diff_PositionTorqueCurrentFOC_Open _diffLeaderFX_reqPtr;
3566            if (_diffLeaderFX_req instanceof Diff_PositionTorqueCurrentFOC_Open req) {
3567                _diffLeaderFX_reqPtr = req;
3568                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3569                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3570            } else {
3571                _diffLeaderFX_reqPtr = new Diff_PositionTorqueCurrentFOC_Open(AverageRequest, DifferentialRequest);
3572                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3573            }
3574            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3575            
3576            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3577            if (retval.isOK()) {
3578                retval = _diffLeaderFX_retval;
3579            }
3580        }
3581        
3582        {
3583            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3584            if (retval.isOK()) {
3585                retval = _diffFollowerFX_retval;
3586            }
3587        }
3588        
3589        return retval;
3590    }
3591    
3592    /**
3593     * Sets the control request for this mechanism.
3594     *
3595     * @param AverageRequest Average VelocityTorqueCurrentFOC request of the
3596     *                       mechanism.
3597     * @param DifferentialRequest Differential TorqueCurrentFOC request of the
3598     *                            mechanism. Note: The UpdateFreqHz parameter for
3599     *                            this control request will be ignored by the
3600     *                            control frame.
3601     * @return Status code of the request
3602     */
3603    public final StatusCode setControl(VelocityTorqueCurrentFOC AverageRequest, TorqueCurrentFOC DifferentialRequest) {
3604        var retval = StatusCode.OK;
3605    
3606        retval = beforeControl();
3607        if (!retval.isOK()) {
3608            return retval;
3609        }
3610        
3611        {
3612            final Diff_VelocityTorqueCurrentFOC_Open _diffLeaderFX_reqPtr;
3613            if (_diffLeaderFX_req instanceof Diff_VelocityTorqueCurrentFOC_Open req) {
3614                _diffLeaderFX_reqPtr = req;
3615                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3616                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3617            } else {
3618                _diffLeaderFX_reqPtr = new Diff_VelocityTorqueCurrentFOC_Open(AverageRequest, DifferentialRequest);
3619                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3620            }
3621            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3622            
3623            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3624            if (retval.isOK()) {
3625                retval = _diffLeaderFX_retval;
3626            }
3627        }
3628        
3629        {
3630            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3631            if (retval.isOK()) {
3632                retval = _diffFollowerFX_retval;
3633            }
3634        }
3635        
3636        return retval;
3637    }
3638    
3639    /**
3640     * Sets the control request for this mechanism.
3641     *
3642     * @param AverageRequest Average MotionMagicTorqueCurrentFOC request of the
3643     *                       mechanism.
3644     * @param DifferentialRequest Differential TorqueCurrentFOC request of the
3645     *                            mechanism. Note: The UpdateFreqHz parameter for
3646     *                            this control request will be ignored by the
3647     *                            control frame.
3648     * @return Status code of the request
3649     */
3650    public final StatusCode setControl(MotionMagicTorqueCurrentFOC AverageRequest, TorqueCurrentFOC DifferentialRequest) {
3651        var retval = StatusCode.OK;
3652    
3653        retval = beforeControl();
3654        if (!retval.isOK()) {
3655            return retval;
3656        }
3657        
3658        {
3659            final Diff_MotionMagicTorqueCurrentFOC_Open _diffLeaderFX_reqPtr;
3660            if (_diffLeaderFX_req instanceof Diff_MotionMagicTorqueCurrentFOC_Open req) {
3661                _diffLeaderFX_reqPtr = req;
3662                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3663                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3664            } else {
3665                _diffLeaderFX_reqPtr = new Diff_MotionMagicTorqueCurrentFOC_Open(AverageRequest, DifferentialRequest);
3666                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3667            }
3668            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3669            
3670            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3671            if (retval.isOK()) {
3672                retval = _diffLeaderFX_retval;
3673            }
3674        }
3675        
3676        {
3677            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3678            if (retval.isOK()) {
3679                retval = _diffFollowerFX_retval;
3680            }
3681        }
3682        
3683        return retval;
3684    }
3685    
3686    /**
3687     * Sets the control request for this mechanism.
3688     *
3689     * @param AverageRequest Average MotionMagicExpoTorqueCurrentFOC request of the
3690     *                       mechanism.
3691     * @param DifferentialRequest Differential TorqueCurrentFOC request of the
3692     *                            mechanism. Note: The UpdateFreqHz parameter for
3693     *                            this control request will be ignored by the
3694     *                            control frame.
3695     * @return Status code of the request
3696     */
3697    public final StatusCode setControl(MotionMagicExpoTorqueCurrentFOC AverageRequest, TorqueCurrentFOC DifferentialRequest) {
3698        var retval = StatusCode.OK;
3699    
3700        retval = beforeControl();
3701        if (!retval.isOK()) {
3702            return retval;
3703        }
3704        
3705        {
3706            final Diff_MotionMagicExpoTorqueCurrentFOC_Open _diffLeaderFX_reqPtr;
3707            if (_diffLeaderFX_req instanceof Diff_MotionMagicExpoTorqueCurrentFOC_Open req) {
3708                _diffLeaderFX_reqPtr = req;
3709                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3710                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3711            } else {
3712                _diffLeaderFX_reqPtr = new Diff_MotionMagicExpoTorqueCurrentFOC_Open(AverageRequest, DifferentialRequest);
3713                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3714            }
3715            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3716            
3717            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3718            if (retval.isOK()) {
3719                retval = _diffLeaderFX_retval;
3720            }
3721        }
3722        
3723        {
3724            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3725            if (retval.isOK()) {
3726                retval = _diffFollowerFX_retval;
3727            }
3728        }
3729        
3730        return retval;
3731    }
3732    
3733    /**
3734     * Sets the control request for this mechanism.
3735     *
3736     * @param AverageRequest Average MotionMagicVelocityTorqueCurrentFOC request of
3737     *                       the mechanism.
3738     * @param DifferentialRequest Differential TorqueCurrentFOC request of the
3739     *                            mechanism. Note: The UpdateFreqHz parameter for
3740     *                            this control request will be ignored by the
3741     *                            control frame.
3742     * @return Status code of the request
3743     */
3744    public final StatusCode setControl(MotionMagicVelocityTorqueCurrentFOC AverageRequest, TorqueCurrentFOC DifferentialRequest) {
3745        var retval = StatusCode.OK;
3746    
3747        retval = beforeControl();
3748        if (!retval.isOK()) {
3749            return retval;
3750        }
3751        
3752        {
3753            final Diff_MotionMagicVelocityTorqueCurrentFOC_Open _diffLeaderFX_reqPtr;
3754            if (_diffLeaderFX_req instanceof Diff_MotionMagicVelocityTorqueCurrentFOC_Open req) {
3755                _diffLeaderFX_reqPtr = req;
3756                _diffLeaderFX_reqPtr.AverageRequest = AverageRequest;
3757                _diffLeaderFX_reqPtr.DifferentialRequest = DifferentialRequest;
3758            } else {
3759                _diffLeaderFX_reqPtr = new Diff_MotionMagicVelocityTorqueCurrentFOC_Open(AverageRequest, DifferentialRequest);
3760                _diffLeaderFX_req = _diffLeaderFX_reqPtr;
3761            }
3762            _diffLeaderFX_reqPtr.UpdateFreqHz = _diffLeaderFX_reqPtr.AverageRequest.UpdateFreqHz;
3763            
3764            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
3765            if (retval.isOK()) {
3766                retval = _diffLeaderFX_retval;
3767            }
3768        }
3769        
3770        {
3771            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
3772            if (retval.isOK()) {
3773                retval = _diffFollowerFX_retval;
3774            }
3775        }
3776        
3777        return retval;
3778    }
3779    
3780}