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.swerve;
008
009import static edu.wpi.first.units.Units.*;
010
011import java.util.HashMap;
012
013import com.ctre.phoenix6.StatusCode;
014import com.ctre.phoenix6.controls.CoastOut;
015import com.ctre.phoenix6.controls.PositionVoltage;
016import com.ctre.phoenix6.controls.VoltageOut;
017import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveControlParameters;
018import com.ctre.phoenix6.swerve.jni.SwerveJNI;
019import com.ctre.phoenix6.swerve.utility.PhoenixPIDController;
020
021import edu.wpi.first.math.geometry.Rotation2d;
022import edu.wpi.first.math.geometry.Translation2d;
023import edu.wpi.first.math.kinematics.ChassisSpeeds;
024import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
025import edu.wpi.first.units.measure.*;
026
027/**
028 * Container for all the Swerve Requests. Use this to find all applicable swerve
029 * drive requests.
030 * <p>
031 * This is also an interface common to all swerve drive control requests that
032 * allow the request to calculate the state to apply to the modules.
033 */
034public interface SwerveRequest {
035    /**
036     * In field-centric control, the direction of "forward" is sometimes different
037     * depending on perspective. This addresses which forward to use.
038     */
039    public enum ForwardPerspectiveValue {
040        /**
041         * "Forward" (positive X) is determined from the operator's perspective. This
042         * is important for most teleop driven field-centric requests, where positive
043         * X means to drive away from the operator.
044         * <p>
045         * Important: Users must specify the OperatorPerspective in the SwerveDrivetrain object
046         */
047        OperatorPerspective(0),
048        /**
049         * "Forward" (positive X) is always from the perspective of the blue alliance (i.e. towards
050         * the red alliance). This is important in situations such as path following where positive
051         * X is always from the blue alliance perspective, regardless of where the operator is
052         * physically located.
053         */
054        BlueAlliance(1);
055
056        public final int value;
057
058        ForwardPerspectiveValue(int initValue) {
059            this.value = initValue;
060        }
061
062        private static HashMap<Integer, ForwardPerspectiveValue> _map = null;
063        static {
064            _map = new HashMap<Integer, ForwardPerspectiveValue>();
065            for (ForwardPerspectiveValue type : ForwardPerspectiveValue.values()) {
066                _map.put(type.value, type);
067            }
068        }
069
070        /**
071         * Gets ForwardPerspectiveValue from specified value
072         *
073         * @param value Value of ForwardPerspectiveValue
074         * @return ForwardPerspectiveValue of specified value
075         */
076        public static ForwardPerspectiveValue valueOf(int value) {
077            ForwardPerspectiveValue retval = _map.get(value);
078            if (retval != null)
079                return retval;
080            return ForwardPerspectiveValue.values()[0];
081        }
082    }
083
084    /**
085     * Applies this swerve request to the given modules. This is
086     * typically called by the SwerveDrivetrain odometry thread.
087     * <p>
088     * For native swerve requests, this API can be called from a
089     * non-native request's {@code apply} to compose the two together.
090     *
091     * @param parameters Parameters the control request needs to calculate the module state
092     * @param modulesToApply Modules to which the control request is applied
093     * @return Status code of sending the request
094     */
095    public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply);
096
097    /** Swerve requests implemented in native code. */
098    public interface NativeSwerveRequest extends SwerveRequest {
099        /**
100         * Applies a native swerve request to the native drivetrain with the provided ID.
101         * <p>
102         * When this is implemented, the regular {@link #apply} function should do nothing
103         * (return OK). Additionally, this cannot be called from another swerve request's
104         * apply method, as this overrides the native swerve request of the drivetrain.
105         * <p>
106         * Unlike {@link #apply}, this function is called every time {@link SwerveDrivetrain#setControl}
107         * is run, not every loop of the odometry thread. Instead, the underlying native
108         * request is run at the full update frequency of the odometry thread.
109         *
110         * @param id ID of the native swerve drivetrain
111         */
112        public void applyNative(int id);
113    }
114
115    
116    /**
117     * Does nothing to the swerve module state. This is the default state of a newly
118     * created swerve drive mechanism.
119     */
120    public static class Idle implements NativeSwerveRequest {
121        
122        
123        
124        public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) {
125            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_Idle(parameters.drivetrainId));
126        }
127        
128        public void applyNative(int id) {
129            SwerveJNI.JNI_SetControl_Idle(id);
130        }
131    }
132    
133    /**
134     * Sets the swerve drive module states to point inward on the robot in an "X"
135     * fashion, creating a natural brake which will oppose any motion.
136     */
137    public static class SwerveDriveBrake implements NativeSwerveRequest {
138        /**
139         * The type of control request to use for the drive motor.
140         */
141        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
142        /**
143         * The type of control request to use for the drive motor.
144         */
145        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
146        
147        /**
148         * Modifies the DriveRequestType parameter and returns itself.
149         * <p>
150         * The type of control request to use for the drive motor.
151         *
152         * @param newDriveRequestType Parameter to modify
153         * @return this object
154         */
155        public SwerveDriveBrake withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
156            this.DriveRequestType = newDriveRequestType;
157            return this;
158        }
159        
160        /**
161         * Modifies the SteerRequestType parameter and returns itself.
162         * <p>
163         * The type of control request to use for the drive motor.
164         *
165         * @param newSteerRequestType Parameter to modify
166         * @return this object
167         */
168        public SwerveDriveBrake withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
169            this.SteerRequestType = newSteerRequestType;
170            return this;
171        }
172        
173        public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) {
174            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_SwerveDriveBrake(parameters.drivetrainId,
175                DriveRequestType.value,
176                SteerRequestType.value));
177        }
178        
179        public void applyNative(int id) {
180            SwerveJNI.JNI_SetControl_SwerveDriveBrake(id,
181                DriveRequestType.value,
182                SteerRequestType.value);
183        }
184    }
185    
186    /**
187     * Drives the swerve drivetrain in a field-centric manner.
188     * <p>
189     * When users use this request, they specify the direction the robot should
190     * travel oriented against the field, and the rate at which their robot should
191     * rotate about the center of the robot.
192     * <p>
193     * An example scenario is that the robot is oriented to the east, the VelocityX
194     * is +5 m/s, VelocityY is 0 m/s, and RotationRate is 0.5 rad/s. In this
195     * scenario, the robot would drive northward at 5 m/s and turn counterclockwise
196     * at 0.5 rad/s.
197     */
198    public static class FieldCentric implements NativeSwerveRequest {
199        /**
200         * The velocity in the X direction, in m/s. X is defined as forward according to
201         * WPILib convention, so this determines how fast to travel forward.
202         */
203        public double VelocityX = 0;
204        /**
205         * The velocity in the Y direction, in m/s. Y is defined as to the left
206         * according to WPILib convention, so this determines how fast to travel to the
207         * left.
208         */
209        public double VelocityY = 0;
210        /**
211         * The angular rate to rotate at, in radians per second. Angular rate is defined
212         * as counterclockwise positive, so this determines how fast to turn
213         * counterclockwise.
214         */
215        public double RotationalRate = 0;
216        /**
217         * The allowable deadband of the request, in m/s.
218         */
219        public double Deadband = 0;
220        /**
221         * The rotational deadband of the request, in radians per second.
222         */
223        public double RotationalDeadband = 0;
224        /**
225         * The center of rotation the robot should rotate around. This is (0,0) by
226         * default, which will rotate around the center of the robot.
227         */
228        public Translation2d CenterOfRotation = new Translation2d();
229        /**
230         * The type of control request to use for the drive motor.
231         */
232        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
233        /**
234         * The type of control request to use for the drive motor.
235         */
236        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
237        /**
238         * Whether to desaturate wheel speeds before applying. For more information, see
239         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
240         */
241        public boolean DesaturateWheelSpeeds = true;
242        /**
243         * The perspective to use when determining which direction is forward.
244         */
245        public ForwardPerspectiveValue ForwardPerspective = ForwardPerspectiveValue.OperatorPerspective;
246        
247        /**
248         * Modifies the VelocityX parameter and returns itself.
249         * <p>
250         * The velocity in the X direction, in m/s. X is defined as forward according to
251         * WPILib convention, so this determines how fast to travel forward.
252         *
253         * @param newVelocityX Parameter to modify
254         * @return this object
255         */
256        public FieldCentric withVelocityX(double newVelocityX) {
257            this.VelocityX = newVelocityX;
258            return this;
259        }
260        
261        /**
262         * Modifies the VelocityX parameter and returns itself.
263         * <p>
264         * The velocity in the X direction, in m/s. X is defined as forward according to
265         * WPILib convention, so this determines how fast to travel forward.
266         *
267         * @param newVelocityX Parameter to modify
268         * @return this object
269         */
270        public FieldCentric withVelocityX(LinearVelocity newVelocityX) {
271            this.VelocityX = newVelocityX.in(MetersPerSecond);
272            return this;
273        }
274        
275        /**
276         * Modifies the VelocityY parameter and returns itself.
277         * <p>
278         * The velocity in the Y direction, in m/s. Y is defined as to the left
279         * according to WPILib convention, so this determines how fast to travel to the
280         * left.
281         *
282         * @param newVelocityY Parameter to modify
283         * @return this object
284         */
285        public FieldCentric withVelocityY(double newVelocityY) {
286            this.VelocityY = newVelocityY;
287            return this;
288        }
289        
290        /**
291         * Modifies the VelocityY parameter and returns itself.
292         * <p>
293         * The velocity in the Y direction, in m/s. Y is defined as to the left
294         * according to WPILib convention, so this determines how fast to travel to the
295         * left.
296         *
297         * @param newVelocityY Parameter to modify
298         * @return this object
299         */
300        public FieldCentric withVelocityY(LinearVelocity newVelocityY) {
301            this.VelocityY = newVelocityY.in(MetersPerSecond);
302            return this;
303        }
304        
305        /**
306         * Modifies the RotationalRate parameter and returns itself.
307         * <p>
308         * The angular rate to rotate at, in radians per second. Angular rate is defined
309         * as counterclockwise positive, so this determines how fast to turn
310         * counterclockwise.
311         *
312         * @param newRotationalRate Parameter to modify
313         * @return this object
314         */
315        public FieldCentric withRotationalRate(double newRotationalRate) {
316            this.RotationalRate = newRotationalRate;
317            return this;
318        }
319        
320        /**
321         * Modifies the RotationalRate parameter and returns itself.
322         * <p>
323         * The angular rate to rotate at, in radians per second. Angular rate is defined
324         * as counterclockwise positive, so this determines how fast to turn
325         * counterclockwise.
326         *
327         * @param newRotationalRate Parameter to modify
328         * @return this object
329         */
330        public FieldCentric withRotationalRate(AngularVelocity newRotationalRate) {
331            this.RotationalRate = newRotationalRate.in(RadiansPerSecond);
332            return this;
333        }
334        
335        /**
336         * Modifies the Deadband parameter and returns itself.
337         * <p>
338         * The allowable deadband of the request, in m/s.
339         *
340         * @param newDeadband Parameter to modify
341         * @return this object
342         */
343        public FieldCentric withDeadband(double newDeadband) {
344            this.Deadband = newDeadband;
345            return this;
346        }
347        
348        /**
349         * Modifies the Deadband parameter and returns itself.
350         * <p>
351         * The allowable deadband of the request, in m/s.
352         *
353         * @param newDeadband Parameter to modify
354         * @return this object
355         */
356        public FieldCentric withDeadband(LinearVelocity newDeadband) {
357            this.Deadband = newDeadband.in(MetersPerSecond);
358            return this;
359        }
360        
361        /**
362         * Modifies the RotationalDeadband parameter and returns itself.
363         * <p>
364         * The rotational deadband of the request, in radians per second.
365         *
366         * @param newRotationalDeadband Parameter to modify
367         * @return this object
368         */
369        public FieldCentric withRotationalDeadband(double newRotationalDeadband) {
370            this.RotationalDeadband = newRotationalDeadband;
371            return this;
372        }
373        
374        /**
375         * Modifies the RotationalDeadband parameter and returns itself.
376         * <p>
377         * The rotational deadband of the request, in radians per second.
378         *
379         * @param newRotationalDeadband Parameter to modify
380         * @return this object
381         */
382        public FieldCentric withRotationalDeadband(AngularVelocity newRotationalDeadband) {
383            this.RotationalDeadband = newRotationalDeadband.in(RadiansPerSecond);
384            return this;
385        }
386        
387        /**
388         * Modifies the CenterOfRotation parameter and returns itself.
389         * <p>
390         * The center of rotation the robot should rotate around. This is (0,0) by
391         * default, which will rotate around the center of the robot.
392         *
393         * @param newCenterOfRotation Parameter to modify
394         * @return this object
395         */
396        public FieldCentric withCenterOfRotation(Translation2d newCenterOfRotation) {
397            this.CenterOfRotation = newCenterOfRotation;
398            return this;
399        }
400        
401        /**
402         * Modifies the DriveRequestType parameter and returns itself.
403         * <p>
404         * The type of control request to use for the drive motor.
405         *
406         * @param newDriveRequestType Parameter to modify
407         * @return this object
408         */
409        public FieldCentric withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
410            this.DriveRequestType = newDriveRequestType;
411            return this;
412        }
413        
414        /**
415         * Modifies the SteerRequestType parameter and returns itself.
416         * <p>
417         * The type of control request to use for the drive motor.
418         *
419         * @param newSteerRequestType Parameter to modify
420         * @return this object
421         */
422        public FieldCentric withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
423            this.SteerRequestType = newSteerRequestType;
424            return this;
425        }
426        
427        /**
428         * Modifies the DesaturateWheelSpeeds parameter and returns itself.
429         * <p>
430         * Whether to desaturate wheel speeds before applying. For more information, see
431         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
432         *
433         * @param newDesaturateWheelSpeeds Parameter to modify
434         * @return this object
435         */
436        public FieldCentric withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) {
437            this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds;
438            return this;
439        }
440        
441        /**
442         * Modifies the ForwardPerspective parameter and returns itself.
443         * <p>
444         * The perspective to use when determining which direction is forward.
445         *
446         * @param newForwardPerspective Parameter to modify
447         * @return this object
448         */
449        public FieldCentric withForwardPerspective(ForwardPerspectiveValue newForwardPerspective) {
450            this.ForwardPerspective = newForwardPerspective;
451            return this;
452        }
453        
454        public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) {
455            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_FieldCentric(parameters.drivetrainId,
456                VelocityX,
457                VelocityY,
458                RotationalRate,
459                Deadband,
460                RotationalDeadband,
461                CenterOfRotation.getX(),
462                CenterOfRotation.getY(),
463                DriveRequestType.value,
464                SteerRequestType.value,
465                DesaturateWheelSpeeds,
466                ForwardPerspective.value));
467        }
468        
469        public void applyNative(int id) {
470            SwerveJNI.JNI_SetControl_FieldCentric(id,
471                VelocityX,
472                VelocityY,
473                RotationalRate,
474                Deadband,
475                RotationalDeadband,
476                CenterOfRotation.getX(),
477                CenterOfRotation.getY(),
478                DriveRequestType.value,
479                SteerRequestType.value,
480                DesaturateWheelSpeeds,
481                ForwardPerspective.value);
482        }
483    }
484    
485    /**
486     * Drives the swerve drivetrain in a robot-centric manner.
487     * <p>
488     * When users use this request, they specify the direction the robot should
489     * travel oriented against the robot itself, and the rate at which their robot
490     * should rotate about the center of the robot.
491     * <p>
492     * An example scenario is that the robot is oriented to the east, the VelocityX
493     * is +5 m/s, VelocityY is 0 m/s, and RotationRate is 0.5 rad/s. In this
494     * scenario, the robot would drive eastward at 5 m/s and turn counterclockwise
495     * at 0.5 rad/s.
496     */
497    public static class RobotCentric implements NativeSwerveRequest {
498        /**
499         * The velocity in the X direction, in m/s. X is defined as forward according to
500         * WPILib convention, so this determines how fast to travel forward.
501         */
502        public double VelocityX = 0;
503        /**
504         * The velocity in the Y direction, in m/s. Y is defined as to the left
505         * according to WPILib convention, so this determines how fast to travel to the
506         * left.
507         */
508        public double VelocityY = 0;
509        /**
510         * The angular rate to rotate at, in radians per second. Angular rate is defined
511         * as counterclockwise positive, so this determines how fast to turn
512         * counterclockwise.
513         */
514        public double RotationalRate = 0;
515        /**
516         * The allowable deadband of the request, in m/s.
517         */
518        public double Deadband = 0;
519        /**
520         * The rotational deadband of the request, in radians per second.
521         */
522        public double RotationalDeadband = 0;
523        /**
524         * The center of rotation the robot should rotate around. This is (0,0) by
525         * default, which will rotate around the center of the robot.
526         */
527        public Translation2d CenterOfRotation = new Translation2d();
528        /**
529         * The type of control request to use for the drive motor.
530         */
531        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
532        /**
533         * The type of control request to use for the drive motor.
534         */
535        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
536        /**
537         * Whether to desaturate wheel speeds before applying. For more information, see
538         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
539         */
540        public boolean DesaturateWheelSpeeds = true;
541        
542        /**
543         * Modifies the VelocityX parameter and returns itself.
544         * <p>
545         * The velocity in the X direction, in m/s. X is defined as forward according to
546         * WPILib convention, so this determines how fast to travel forward.
547         *
548         * @param newVelocityX Parameter to modify
549         * @return this object
550         */
551        public RobotCentric withVelocityX(double newVelocityX) {
552            this.VelocityX = newVelocityX;
553            return this;
554        }
555        
556        /**
557         * Modifies the VelocityX parameter and returns itself.
558         * <p>
559         * The velocity in the X direction, in m/s. X is defined as forward according to
560         * WPILib convention, so this determines how fast to travel forward.
561         *
562         * @param newVelocityX Parameter to modify
563         * @return this object
564         */
565        public RobotCentric withVelocityX(LinearVelocity newVelocityX) {
566            this.VelocityX = newVelocityX.in(MetersPerSecond);
567            return this;
568        }
569        
570        /**
571         * Modifies the VelocityY parameter and returns itself.
572         * <p>
573         * The velocity in the Y direction, in m/s. Y is defined as to the left
574         * according to WPILib convention, so this determines how fast to travel to the
575         * left.
576         *
577         * @param newVelocityY Parameter to modify
578         * @return this object
579         */
580        public RobotCentric withVelocityY(double newVelocityY) {
581            this.VelocityY = newVelocityY;
582            return this;
583        }
584        
585        /**
586         * Modifies the VelocityY parameter and returns itself.
587         * <p>
588         * The velocity in the Y direction, in m/s. Y is defined as to the left
589         * according to WPILib convention, so this determines how fast to travel to the
590         * left.
591         *
592         * @param newVelocityY Parameter to modify
593         * @return this object
594         */
595        public RobotCentric withVelocityY(LinearVelocity newVelocityY) {
596            this.VelocityY = newVelocityY.in(MetersPerSecond);
597            return this;
598        }
599        
600        /**
601         * Modifies the RotationalRate parameter and returns itself.
602         * <p>
603         * The angular rate to rotate at, in radians per second. Angular rate is defined
604         * as counterclockwise positive, so this determines how fast to turn
605         * counterclockwise.
606         *
607         * @param newRotationalRate Parameter to modify
608         * @return this object
609         */
610        public RobotCentric withRotationalRate(double newRotationalRate) {
611            this.RotationalRate = newRotationalRate;
612            return this;
613        }
614        
615        /**
616         * Modifies the RotationalRate parameter and returns itself.
617         * <p>
618         * The angular rate to rotate at, in radians per second. Angular rate is defined
619         * as counterclockwise positive, so this determines how fast to turn
620         * counterclockwise.
621         *
622         * @param newRotationalRate Parameter to modify
623         * @return this object
624         */
625        public RobotCentric withRotationalRate(AngularVelocity newRotationalRate) {
626            this.RotationalRate = newRotationalRate.in(RadiansPerSecond);
627            return this;
628        }
629        
630        /**
631         * Modifies the Deadband parameter and returns itself.
632         * <p>
633         * The allowable deadband of the request, in m/s.
634         *
635         * @param newDeadband Parameter to modify
636         * @return this object
637         */
638        public RobotCentric withDeadband(double newDeadband) {
639            this.Deadband = newDeadband;
640            return this;
641        }
642        
643        /**
644         * Modifies the Deadband parameter and returns itself.
645         * <p>
646         * The allowable deadband of the request, in m/s.
647         *
648         * @param newDeadband Parameter to modify
649         * @return this object
650         */
651        public RobotCentric withDeadband(LinearVelocity newDeadband) {
652            this.Deadband = newDeadband.in(MetersPerSecond);
653            return this;
654        }
655        
656        /**
657         * Modifies the RotationalDeadband parameter and returns itself.
658         * <p>
659         * The rotational deadband of the request, in radians per second.
660         *
661         * @param newRotationalDeadband Parameter to modify
662         * @return this object
663         */
664        public RobotCentric withRotationalDeadband(double newRotationalDeadband) {
665            this.RotationalDeadband = newRotationalDeadband;
666            return this;
667        }
668        
669        /**
670         * Modifies the RotationalDeadband parameter and returns itself.
671         * <p>
672         * The rotational deadband of the request, in radians per second.
673         *
674         * @param newRotationalDeadband Parameter to modify
675         * @return this object
676         */
677        public RobotCentric withRotationalDeadband(AngularVelocity newRotationalDeadband) {
678            this.RotationalDeadband = newRotationalDeadband.in(RadiansPerSecond);
679            return this;
680        }
681        
682        /**
683         * Modifies the CenterOfRotation parameter and returns itself.
684         * <p>
685         * The center of rotation the robot should rotate around. This is (0,0) by
686         * default, which will rotate around the center of the robot.
687         *
688         * @param newCenterOfRotation Parameter to modify
689         * @return this object
690         */
691        public RobotCentric withCenterOfRotation(Translation2d newCenterOfRotation) {
692            this.CenterOfRotation = newCenterOfRotation;
693            return this;
694        }
695        
696        /**
697         * Modifies the DriveRequestType parameter and returns itself.
698         * <p>
699         * The type of control request to use for the drive motor.
700         *
701         * @param newDriveRequestType Parameter to modify
702         * @return this object
703         */
704        public RobotCentric withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
705            this.DriveRequestType = newDriveRequestType;
706            return this;
707        }
708        
709        /**
710         * Modifies the SteerRequestType parameter and returns itself.
711         * <p>
712         * The type of control request to use for the drive motor.
713         *
714         * @param newSteerRequestType Parameter to modify
715         * @return this object
716         */
717        public RobotCentric withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
718            this.SteerRequestType = newSteerRequestType;
719            return this;
720        }
721        
722        /**
723         * Modifies the DesaturateWheelSpeeds parameter and returns itself.
724         * <p>
725         * Whether to desaturate wheel speeds before applying. For more information, see
726         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
727         *
728         * @param newDesaturateWheelSpeeds Parameter to modify
729         * @return this object
730         */
731        public RobotCentric withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) {
732            this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds;
733            return this;
734        }
735        
736        public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) {
737            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_RobotCentric(parameters.drivetrainId,
738                VelocityX,
739                VelocityY,
740                RotationalRate,
741                Deadband,
742                RotationalDeadband,
743                CenterOfRotation.getX(),
744                CenterOfRotation.getY(),
745                DriveRequestType.value,
746                SteerRequestType.value,
747                DesaturateWheelSpeeds));
748        }
749        
750        public void applyNative(int id) {
751            SwerveJNI.JNI_SetControl_RobotCentric(id,
752                VelocityX,
753                VelocityY,
754                RotationalRate,
755                Deadband,
756                RotationalDeadband,
757                CenterOfRotation.getX(),
758                CenterOfRotation.getY(),
759                DriveRequestType.value,
760                SteerRequestType.value,
761                DesaturateWheelSpeeds);
762        }
763    }
764    
765    /**
766     * Sets the swerve drive modules to point to a specified direction.
767     */
768    public static class PointWheelsAt implements NativeSwerveRequest {
769        /**
770         * The direction to point the modules toward. This direction is still optimized
771         * to what the module was previously at.
772         */
773        public Rotation2d ModuleDirection = new Rotation2d();
774        /**
775         * The type of control request to use for the drive motor.
776         */
777        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
778        /**
779         * The type of control request to use for the drive motor.
780         */
781        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
782        
783        /**
784         * Modifies the ModuleDirection parameter and returns itself.
785         * <p>
786         * The direction to point the modules toward. This direction is still optimized
787         * to what the module was previously at.
788         *
789         * @param newModuleDirection Parameter to modify
790         * @return this object
791         */
792        public PointWheelsAt withModuleDirection(Rotation2d newModuleDirection) {
793            this.ModuleDirection = newModuleDirection;
794            return this;
795        }
796        
797        /**
798         * Modifies the DriveRequestType parameter and returns itself.
799         * <p>
800         * The type of control request to use for the drive motor.
801         *
802         * @param newDriveRequestType Parameter to modify
803         * @return this object
804         */
805        public PointWheelsAt withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
806            this.DriveRequestType = newDriveRequestType;
807            return this;
808        }
809        
810        /**
811         * Modifies the SteerRequestType parameter and returns itself.
812         * <p>
813         * The type of control request to use for the drive motor.
814         *
815         * @param newSteerRequestType Parameter to modify
816         * @return this object
817         */
818        public PointWheelsAt withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
819            this.SteerRequestType = newSteerRequestType;
820            return this;
821        }
822        
823        public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) {
824            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_PointWheelsAt(parameters.drivetrainId,
825                ModuleDirection.getRadians(),
826                DriveRequestType.value,
827                SteerRequestType.value));
828        }
829        
830        public void applyNative(int id) {
831            SwerveJNI.JNI_SetControl_PointWheelsAt(id,
832                ModuleDirection.getRadians(),
833                DriveRequestType.value,
834                SteerRequestType.value);
835        }
836    }
837
838    /**
839     * Accepts a generic robot-centric ChassisSpeeds to apply to the drivetrain.
840     */
841    public static class ApplyRobotSpeeds implements NativeSwerveRequest {
842        /**
843         * The robot-centric chassis speeds to apply to the drivetrain.
844         */
845        public ChassisSpeeds Speeds = new ChassisSpeeds();
846        /**
847         * Robot-centric wheel force feedforwards to apply in the X direction, in
848         * newtons. X is defined as forward according to WPILib convention, so this
849         * determines the forward forces to apply.
850         * <p>
851         * These forces should include friction applied to the ground.
852         * <p>
853         * The order of the forces should match the order of the modules returned from
854         * SwerveDrivetrain.
855         */
856        public double[] WheelForceFeedforwardsX = {};
857        /**
858         * Robot-centric wheel force feedforwards to apply in the Y direction, in
859         * newtons. Y is defined as to the left according to WPILib convention, so this
860         * determines the forces to apply to the left.
861         * <p>
862         * These forces should include friction applied to the ground.
863         * <p>
864         * The order of the forces should match the order of the modules returned from
865         * SwerveDrivetrain.
866         */
867        public double[] WheelForceFeedforwardsY = {};
868        /**
869         * The center of rotation the robot should rotate around. This is (0,0) by
870         * default, which will rotate around the center of the robot.
871         */
872        public Translation2d CenterOfRotation = new Translation2d();
873        /**
874         * The type of control request to use for the drive motor.
875         */
876        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
877        /**
878         * The type of control request to use for the drive motor.
879         */
880        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
881        /**
882         * Whether to desaturate wheel speeds before applying. For more information, see
883         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
884         */
885        public boolean DesaturateWheelSpeeds = true;
886    
887        /**
888         * Modifies the Speeds parameter and returns itself.
889         * <p>
890         * The robot-centric chassis speeds to apply to the drivetrain.
891         *
892         * @param newSpeeds Parameter to modify
893         * @return this object
894         */
895        public ApplyRobotSpeeds withSpeeds(ChassisSpeeds newSpeeds) {
896            this.Speeds = newSpeeds;
897            return this;
898        }
899        
900        /**
901         * Modifies the WheelForceFeedforwardsX parameter and returns itself.
902         * <p>
903         * Robot-centric wheel force feedforwards to apply in the X direction, in
904         * newtons. X is defined as forward according to WPILib convention, so this
905         * determines the forward forces to apply.
906         * <p>
907         * These forces should include friction applied to the ground.
908         * <p>
909         * The order of the forces should match the order of the modules returned from
910         * SwerveDrivetrain.
911         *
912         * @param newWheelForceFeedforwardsX Parameter to modify
913         * @return this object
914         */
915        public ApplyRobotSpeeds withWheelForceFeedforwardsX(double[] newWheelForceFeedforwardsX) {
916            this.WheelForceFeedforwardsX = newWheelForceFeedforwardsX;
917            return this;
918        }
919        
920        /**
921         * Modifies the WheelForceFeedforwardsX parameter and returns itself.
922         * <p>
923         * Robot-centric wheel force feedforwards to apply in the X direction, in
924         * newtons. X is defined as forward according to WPILib convention, so this
925         * determines the forward forces to apply.
926         * <p>
927         * These forces should include friction applied to the ground.
928         * <p>
929         * The order of the forces should match the order of the modules returned from
930         * SwerveDrivetrain.
931         *
932         * @param newWheelForceFeedforwardsX Parameter to modify
933         * @return this object
934         */
935        public ApplyRobotSpeeds withWheelForceFeedforwardsX(Force[] newWheelForceFeedforwardsX) {
936            if (this.WheelForceFeedforwardsX.length != newWheelForceFeedforwardsX.length) {
937                this.WheelForceFeedforwardsX = new double[newWheelForceFeedforwardsX.length];
938            }
939            for (int i = 0; i < this.WheelForceFeedforwardsX.length; ++i) {
940                this.WheelForceFeedforwardsX[i] = newWheelForceFeedforwardsX[i].in(Newtons);
941            }
942
943            return this;
944        }
945        
946        /**
947         * Modifies the WheelForceFeedforwardsY parameter and returns itself.
948         * <p>
949         * Robot-centric wheel force feedforwards to apply in the Y direction, in
950         * newtons. Y is defined as to the left according to WPILib convention, so this
951         * determines the forces to apply to the left.
952         * <p>
953         * These forces should include friction applied to the ground.
954         * <p>
955         * The order of the forces should match the order of the modules returned from
956         * SwerveDrivetrain.
957         *
958         * @param newWheelForceFeedforwardsY Parameter to modify
959         * @return this object
960         */
961        public ApplyRobotSpeeds withWheelForceFeedforwardsY(double[] newWheelForceFeedforwardsY) {
962            this.WheelForceFeedforwardsY = newWheelForceFeedforwardsY;
963            return this;
964        }
965        
966        /**
967         * Modifies the WheelForceFeedforwardsY parameter and returns itself.
968         * <p>
969         * Robot-centric wheel force feedforwards to apply in the Y direction, in
970         * newtons. Y is defined as to the left according to WPILib convention, so this
971         * determines the forces to apply to the left.
972         * <p>
973         * These forces should include friction applied to the ground.
974         * <p>
975         * The order of the forces should match the order of the modules returned from
976         * SwerveDrivetrain.
977         *
978         * @param newWheelForceFeedforwardsY Parameter to modify
979         * @return this object
980         */
981        public ApplyRobotSpeeds withWheelForceFeedforwardsY(Force[] newWheelForceFeedforwardsY) {
982            if (this.WheelForceFeedforwardsY.length != newWheelForceFeedforwardsY.length) {
983                this.WheelForceFeedforwardsY = new double[newWheelForceFeedforwardsY.length];
984            }
985            for (int i = 0; i < this.WheelForceFeedforwardsY.length; ++i) {
986                this.WheelForceFeedforwardsY[i] = newWheelForceFeedforwardsY[i].in(Newtons);
987            }
988
989            return this;
990        }
991        
992        /**
993         * Modifies the CenterOfRotation parameter and returns itself.
994         * <p>
995         * The center of rotation the robot should rotate around. This is (0,0) by
996         * default, which will rotate around the center of the robot.
997         *
998         * @param newCenterOfRotation Parameter to modify
999         * @return this object
1000         */
1001        public ApplyRobotSpeeds withCenterOfRotation(Translation2d newCenterOfRotation) {
1002            this.CenterOfRotation = newCenterOfRotation;
1003            return this;
1004        }
1005        
1006        /**
1007         * Modifies the DriveRequestType parameter and returns itself.
1008         * <p>
1009         * The type of control request to use for the drive motor.
1010         *
1011         * @param newDriveRequestType Parameter to modify
1012         * @return this object
1013         */
1014        public ApplyRobotSpeeds withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
1015            this.DriveRequestType = newDriveRequestType;
1016            return this;
1017        }
1018        
1019        /**
1020         * Modifies the SteerRequestType parameter and returns itself.
1021         * <p>
1022         * The type of control request to use for the drive motor.
1023         *
1024         * @param newSteerRequestType Parameter to modify
1025         * @return this object
1026         */
1027        public ApplyRobotSpeeds withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
1028            this.SteerRequestType = newSteerRequestType;
1029            return this;
1030        }
1031        
1032        /**
1033         * Modifies the DesaturateWheelSpeeds parameter and returns itself.
1034         * <p>
1035         * Whether to desaturate wheel speeds before applying. For more information, see
1036         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
1037         *
1038         * @param newDesaturateWheelSpeeds Parameter to modify
1039         * @return this object
1040         */
1041        public ApplyRobotSpeeds withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) {
1042            this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds;
1043            return this;
1044        }
1045        
1046        public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) {
1047            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_ApplyRobotSpeeds(parameters.drivetrainId,
1048                Speeds.vxMetersPerSecond,
1049                Speeds.vyMetersPerSecond,
1050                Speeds.omegaRadiansPerSecond,
1051                WheelForceFeedforwardsX,
1052                WheelForceFeedforwardsY,
1053                CenterOfRotation.getX(),
1054                CenterOfRotation.getY(),
1055                DriveRequestType.value,
1056                SteerRequestType.value,
1057                DesaturateWheelSpeeds));
1058        }
1059        
1060        public void applyNative(int id) {
1061            SwerveJNI.JNI_SetControl_ApplyRobotSpeeds(id,
1062                Speeds.vxMetersPerSecond,
1063                Speeds.vyMetersPerSecond,
1064                Speeds.omegaRadiansPerSecond,
1065                WheelForceFeedforwardsX,
1066                WheelForceFeedforwardsY,
1067                CenterOfRotation.getX(),
1068                CenterOfRotation.getY(),
1069                DriveRequestType.value,
1070                SteerRequestType.value,
1071                DesaturateWheelSpeeds);
1072        }
1073    }
1074
1075    /**
1076     * Accepts a generic robot-centric ChassisSpeeds to apply to the drivetrain.
1077     *
1078     * @deprecated Use {@link ApplyRobotSpeeds} instead
1079     */
1080    @Deprecated(forRemoval = true, since = "2025")
1081    public static class ApplyChassisSpeeds extends ApplyRobotSpeeds {}
1082
1083    /**
1084     * Accepts a generic field-centric ChassisSpeeds to apply to the drivetrain.
1085     */
1086    public static class ApplyFieldSpeeds implements NativeSwerveRequest {
1087        /**
1088         * The field-centric chassis speeds to apply to the drivetrain.
1089         */
1090        public ChassisSpeeds Speeds = new ChassisSpeeds();
1091        /**
1092         * Field-centric wheel force feedforwards to apply in the X direction, in
1093         * newtons. X is defined as forward according to WPILib convention, so this
1094         * determines the forward forces to apply.
1095         * <p>
1096         * These forces should include friction applied to the ground.
1097         * <p>
1098         * The order of the forces should match the order of the modules returned from
1099         * SwerveDrivetrain.
1100         */
1101        public double[] WheelForceFeedforwardsX = {};
1102        /**
1103         * Field-centric wheel force feedforwards to apply in the Y direction, in
1104         * newtons. Y is defined as to the left according to WPILib convention, so this
1105         * determines the forces to apply to the left.
1106         * <p>
1107         * These forces should include friction applied to the ground.
1108         * <p>
1109         * The order of the forces should match the order of the modules returned from
1110         * SwerveDrivetrain.
1111         */
1112        public double[] WheelForceFeedforwardsY = {};
1113        /**
1114         * The center of rotation the robot should rotate around. This is (0,0) by
1115         * default, which will rotate around the center of the robot.
1116         */
1117        public Translation2d CenterOfRotation = new Translation2d();
1118        /**
1119         * The type of control request to use for the drive motor.
1120         */
1121        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
1122        /**
1123         * The type of control request to use for the drive motor.
1124         */
1125        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
1126        /**
1127         * Whether to desaturate wheel speeds before applying. For more information, see
1128         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
1129         */
1130        public boolean DesaturateWheelSpeeds = true;
1131        /**
1132         * The perspective to use when determining which direction is forward.
1133         */
1134        public ForwardPerspectiveValue ForwardPerspective = ForwardPerspectiveValue.BlueAlliance;
1135    
1136        /**
1137         * Modifies the Speeds parameter and returns itself.
1138         * <p>
1139         * The field-centric chassis speeds to apply to the drivetrain.
1140         *
1141         * @param newSpeeds Parameter to modify
1142         * @return this object
1143         */
1144        public ApplyFieldSpeeds withSpeeds(ChassisSpeeds newSpeeds) {
1145            this.Speeds = newSpeeds;
1146            return this;
1147        }
1148        
1149        /**
1150         * Modifies the WheelForceFeedforwardsX parameter and returns itself.
1151         * <p>
1152         * Field-centric wheel force feedforwards to apply in the X direction, in
1153         * newtons. X is defined as forward according to WPILib convention, so this
1154         * determines the forward forces to apply.
1155         * <p>
1156         * These forces should include friction applied to the ground.
1157         * <p>
1158         * The order of the forces should match the order of the modules returned from
1159         * SwerveDrivetrain.
1160         *
1161         * @param newWheelForceFeedforwardsX Parameter to modify
1162         * @return this object
1163         */
1164        public ApplyFieldSpeeds withWheelForceFeedforwardsX(double[] newWheelForceFeedforwardsX) {
1165            this.WheelForceFeedforwardsX = newWheelForceFeedforwardsX;
1166            return this;
1167        }
1168        
1169        /**
1170         * Modifies the WheelForceFeedforwardsX parameter and returns itself.
1171         * <p>
1172         * Field-centric wheel force feedforwards to apply in the X direction, in
1173         * newtons. X is defined as forward according to WPILib convention, so this
1174         * determines the forward forces to apply.
1175         * <p>
1176         * These forces should include friction applied to the ground.
1177         * <p>
1178         * The order of the forces should match the order of the modules returned from
1179         * SwerveDrivetrain.
1180         *
1181         * @param newWheelForceFeedforwardsX Parameter to modify
1182         * @return this object
1183         */
1184        public ApplyFieldSpeeds withWheelForceFeedforwardsX(Force[] newWheelForceFeedforwardsX) {
1185            if (this.WheelForceFeedforwardsX.length != newWheelForceFeedforwardsX.length) {
1186                this.WheelForceFeedforwardsX = new double[newWheelForceFeedforwardsX.length];
1187            }
1188            for (int i = 0; i < this.WheelForceFeedforwardsX.length; ++i) {
1189                this.WheelForceFeedforwardsX[i] = newWheelForceFeedforwardsX[i].in(Newtons);
1190            }
1191
1192            return this;
1193        }
1194        
1195        /**
1196         * Modifies the WheelForceFeedforwardsY parameter and returns itself.
1197         * <p>
1198         * Field-centric wheel force feedforwards to apply in the Y direction, in
1199         * newtons. Y is defined as to the left according to WPILib convention, so this
1200         * determines the forces to apply to the left.
1201         * <p>
1202         * These forces should include friction applied to the ground.
1203         * <p>
1204         * The order of the forces should match the order of the modules returned from
1205         * SwerveDrivetrain.
1206         *
1207         * @param newWheelForceFeedforwardsY Parameter to modify
1208         * @return this object
1209         */
1210        public ApplyFieldSpeeds withWheelForceFeedforwardsY(double[] newWheelForceFeedforwardsY) {
1211            this.WheelForceFeedforwardsY = newWheelForceFeedforwardsY;
1212            return this;
1213        }
1214        
1215        /**
1216         * Modifies the WheelForceFeedforwardsY parameter and returns itself.
1217         * <p>
1218         * Field-centric wheel force feedforwards to apply in the Y direction, in
1219         * newtons. Y is defined as to the left according to WPILib convention, so this
1220         * determines the forces to apply to the left.
1221         * <p>
1222         * These forces should include friction applied to the ground.
1223         * <p>
1224         * The order of the forces should match the order of the modules returned from
1225         * SwerveDrivetrain.
1226         *
1227         * @param newWheelForceFeedforwardsY Parameter to modify
1228         * @return this object
1229         */
1230        public ApplyFieldSpeeds withWheelForceFeedforwardsY(Force[] newWheelForceFeedforwardsY) {
1231            if (this.WheelForceFeedforwardsY.length != newWheelForceFeedforwardsY.length) {
1232                this.WheelForceFeedforwardsY = new double[newWheelForceFeedforwardsY.length];
1233            }
1234            for (int i = 0; i < this.WheelForceFeedforwardsY.length; ++i) {
1235                this.WheelForceFeedforwardsY[i] = newWheelForceFeedforwardsY[i].in(Newtons);
1236            }
1237
1238            return this;
1239        }
1240        
1241        /**
1242         * Modifies the CenterOfRotation parameter and returns itself.
1243         * <p>
1244         * The center of rotation the robot should rotate around. This is (0,0) by
1245         * default, which will rotate around the center of the robot.
1246         *
1247         * @param newCenterOfRotation Parameter to modify
1248         * @return this object
1249         */
1250        public ApplyFieldSpeeds withCenterOfRotation(Translation2d newCenterOfRotation) {
1251            this.CenterOfRotation = newCenterOfRotation;
1252            return this;
1253        }
1254        
1255        /**
1256         * Modifies the DriveRequestType parameter and returns itself.
1257         * <p>
1258         * The type of control request to use for the drive motor.
1259         *
1260         * @param newDriveRequestType Parameter to modify
1261         * @return this object
1262         */
1263        public ApplyFieldSpeeds withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
1264            this.DriveRequestType = newDriveRequestType;
1265            return this;
1266        }
1267        
1268        /**
1269         * Modifies the SteerRequestType parameter and returns itself.
1270         * <p>
1271         * The type of control request to use for the drive motor.
1272         *
1273         * @param newSteerRequestType Parameter to modify
1274         * @return this object
1275         */
1276        public ApplyFieldSpeeds withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
1277            this.SteerRequestType = newSteerRequestType;
1278            return this;
1279        }
1280        
1281        /**
1282         * Modifies the DesaturateWheelSpeeds parameter and returns itself.
1283         * <p>
1284         * Whether to desaturate wheel speeds before applying. For more information, see
1285         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
1286         *
1287         * @param newDesaturateWheelSpeeds Parameter to modify
1288         * @return this object
1289         */
1290        public ApplyFieldSpeeds withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) {
1291            this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds;
1292            return this;
1293        }
1294        
1295        /**
1296         * Modifies the ForwardPerspective parameter and returns itself.
1297         * <p>
1298         * The perspective to use when determining which direction is forward.
1299         *
1300         * @param newForwardPerspective Parameter to modify
1301         * @return this object
1302         */
1303        public ApplyFieldSpeeds withForwardPerspective(ForwardPerspectiveValue newForwardPerspective) {
1304            this.ForwardPerspective = newForwardPerspective;
1305            return this;
1306        }
1307        
1308        public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) {
1309            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_ApplyFieldSpeeds(parameters.drivetrainId,
1310                Speeds.vxMetersPerSecond,
1311                Speeds.vyMetersPerSecond,
1312                Speeds.omegaRadiansPerSecond,
1313                WheelForceFeedforwardsX,
1314                WheelForceFeedforwardsY,
1315                CenterOfRotation.getX(),
1316                CenterOfRotation.getY(),
1317                DriveRequestType.value,
1318                SteerRequestType.value,
1319                DesaturateWheelSpeeds,
1320                ForwardPerspective.value));
1321        }
1322        
1323        public void applyNative(int id) {
1324            SwerveJNI.JNI_SetControl_ApplyFieldSpeeds(id,
1325                Speeds.vxMetersPerSecond,
1326                Speeds.vyMetersPerSecond,
1327                Speeds.omegaRadiansPerSecond,
1328                WheelForceFeedforwardsX,
1329                WheelForceFeedforwardsY,
1330                CenterOfRotation.getX(),
1331                CenterOfRotation.getY(),
1332                DriveRequestType.value,
1333                SteerRequestType.value,
1334                DesaturateWheelSpeeds,
1335                ForwardPerspective.value);
1336        }
1337    }
1338
1339    /**
1340     * Drives the swerve drivetrain in a field-centric manner, maintaining a
1341     * specified heading angle to ensure the robot is facing the desired direction
1342     * <p>
1343     * When users use this request, they specify the direction the robot should
1344     * travel oriented against the field, and the direction the robot should be facing.
1345     * <p>
1346     * An example scenario is that the robot is oriented to the east, the VelocityX
1347     * is +5 m/s, VelocityY is 0 m/s, and TargetDirection is 180 degrees.
1348     * In this scenario, the robot would drive northward at 5 m/s and turn clockwise
1349     * to a target of 180 degrees.
1350     * <p>
1351     * This control request is especially useful for autonomous control, where the
1352     * robot should be facing a changing direction throughout the motion.
1353     */
1354    public static class FieldCentricFacingAngle implements SwerveRequest {
1355        /**
1356         * The velocity in the X direction, in m/s.
1357         * X is defined as forward according to WPILib convention,
1358         * so this determines how fast to travel forward.
1359         */
1360        public double VelocityX = 0;
1361        /**
1362         * The velocity in the Y direction, in m/s.
1363         * Y is defined as to the left according to WPILib convention,
1364         * so this determines how fast to travel to the left.
1365         */
1366        public double VelocityY = 0;
1367        /**
1368         * The desired direction to face.
1369         * 0 Degrees is defined as in the direction of the X axis.
1370         * As a result, a TargetDirection of 90 degrees will point along
1371         * the Y axis, or to the left.
1372         */
1373        public Rotation2d TargetDirection = new Rotation2d();
1374        /**
1375         * The rotational rate feedforward to add to the output of the heading
1376         * controller, in radians per second. When using a motion profile for the
1377         * target direction, this can be set to the current velocity reference of
1378         * the profile.
1379         */
1380        public double TargetRateFeedforward = 0;
1381
1382        /**
1383         * The allowable deadband of the request, in m/s.
1384         */
1385        public double Deadband = 0;
1386        /**
1387         * The rotational deadband of the request, in radians per second.
1388         */
1389        public double RotationalDeadband = 0;
1390        /**
1391         * The center of rotation the robot should rotate around.
1392         * This is (0,0) by default, which will rotate around the center of the robot.
1393         */
1394        public Translation2d CenterOfRotation = new Translation2d();
1395
1396        /**
1397         * The type of control request to use for the drive motor.
1398         */
1399        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
1400        /**
1401         * The type of control request to use for the steer motor.
1402         */
1403        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
1404        /**
1405         * Whether to desaturate wheel speeds before applying.
1406         * For more information, see the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
1407         */
1408        public boolean DesaturateWheelSpeeds = true;
1409
1410        /**
1411         * The perspective to use when determining which direction is forward.
1412         */
1413        public ForwardPerspectiveValue ForwardPerspective = ForwardPerspectiveValue.OperatorPerspective;
1414
1415        /**
1416         * The PID controller used to maintain the desired heading.
1417         * Users can specify the PID gains to change how aggressively to maintain
1418         * heading.
1419         * <p>
1420         * This PID controller operates on heading radians and outputs a target
1421         * rotational rate in radians per second. Note that continuous input should
1422         * be enabled on the range [-pi, pi].
1423         */
1424        public PhoenixPIDController HeadingController = new PhoenixPIDController(0, 0, 0);
1425
1426        private final FieldCentric m_fieldCentric = new FieldCentric();
1427
1428        public FieldCentricFacingAngle() {
1429            HeadingController.enableContinuousInput(-Math.PI, Math.PI);
1430        }
1431
1432        public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) {
1433            Rotation2d angleToFace = TargetDirection;
1434            if (ForwardPerspective == ForwardPerspectiveValue.OperatorPerspective) {
1435                /* If we're operator perspective, rotate the direction we want to face by the angle */
1436                angleToFace = angleToFace.rotateBy(parameters.operatorForwardDirection);
1437            }
1438
1439            double toApplyOmega = TargetRateFeedforward +
1440                HeadingController.calculate(
1441                    parameters.currentPose.getRotation().getRadians(),
1442                    angleToFace.getRadians(),
1443                    parameters.timestamp
1444                );
1445
1446            return m_fieldCentric
1447                .withVelocityX(VelocityX)
1448                .withVelocityY(VelocityY)
1449                .withRotationalRate(toApplyOmega)
1450                .withDeadband(Deadband)
1451                .withRotationalDeadband(RotationalDeadband)
1452                .withCenterOfRotation(CenterOfRotation)
1453                .withDriveRequestType(DriveRequestType)
1454                .withSteerRequestType(SteerRequestType)
1455                .withDesaturateWheelSpeeds(DesaturateWheelSpeeds)
1456                .withForwardPerspective(ForwardPerspective)
1457                .apply(parameters, modulesToApply);
1458        }
1459
1460        /**
1461         * Modifies the VelocityX parameter and returns itself.
1462         * <p>
1463         * The velocity in the X direction, in m/s. X is defined as forward according to
1464         * WPILib convention, so this determines how fast to travel forward.
1465         *
1466         * @param newVelocityX Parameter to modify
1467         * @return this object
1468         */
1469        public FieldCentricFacingAngle withVelocityX(double newVelocityX) {
1470            this.VelocityX = newVelocityX;
1471            return this;
1472        }
1473
1474        /**
1475         * Modifies the VelocityX parameter and returns itself.
1476         * <p>
1477         * The velocity in the X direction, in m/s. X is defined as forward according to
1478         * WPILib convention, so this determines how fast to travel forward.
1479         *
1480         * @param newVelocityX Parameter to modify
1481         * @return this object
1482         */
1483        public FieldCentricFacingAngle withVelocityX(LinearVelocity newVelocityX) {
1484            this.VelocityX = newVelocityX.in(MetersPerSecond);
1485            return this;
1486        }
1487
1488        /**
1489         * Modifies the VelocityY parameter and returns itself.
1490         * <p>
1491         * The velocity in the Y direction, in m/s. Y is defined as to the left
1492         * according to WPILib convention, so this determines how fast to travel to the
1493         * left.
1494         *
1495         * @param newVelocityY Parameter to modify
1496         * @return this object
1497         */
1498        public FieldCentricFacingAngle withVelocityY(double newVelocityY) {
1499            this.VelocityY = newVelocityY;
1500            return this;
1501        }
1502
1503        /**
1504         * Modifies the VelocityY parameter and returns itself.
1505         * <p>
1506         * The velocity in the Y direction, in m/s. Y is defined as to the left
1507         * according to WPILib convention, so this determines how fast to travel to the
1508         * left.
1509         *
1510         * @param newVelocityY Parameter to modify
1511         * @return this object
1512         */
1513        public FieldCentricFacingAngle withVelocityY(LinearVelocity newVelocityY) {
1514            this.VelocityY = newVelocityY.in(MetersPerSecond);
1515            return this;
1516        }
1517
1518        /**
1519         * Modifies the TargetDirection parameter and returns itself.
1520         * <p>
1521         * The desired direction to face. 0 Degrees is defined as in the direction of
1522         * the X axis. As a result, a TargetDirection of 90 degrees will point along
1523         * the Y axis, or to the left.
1524         *
1525         * @param newTargetDirection Parameter to modify
1526         * @return this object
1527         */
1528        public FieldCentricFacingAngle withTargetDirection(Rotation2d newTargetDirection) {
1529            this.TargetDirection = newTargetDirection;
1530            return this;
1531        }
1532
1533        /**
1534         * Modifies the TargetRateFeedforward parameter and returns itself.
1535         * <p>
1536         * The rotational rate feedforward to add to the output of the heading
1537         * controller, in radians per second. When using a motion profile for the
1538         * target direction, this can be set to the current velocity reference of
1539         * the profile.
1540         *
1541         * @param newTargetRateFeedforward Parameter to modify
1542         * @return this object
1543         */
1544        public FieldCentricFacingAngle withTargetRateFeedforward(double newTargetRateFeedforward) {
1545            this.TargetRateFeedforward = newTargetRateFeedforward;
1546            return this;
1547        }
1548        /**
1549         * Modifies the TargetRateFeedforward parameter and returns itself.
1550         * <p>
1551         * The rotational rate feedforward to add to the output of the heading
1552         * controller, in radians per second. When using a motion profile for the
1553         * target direction, this can be set to the current velocity reference of
1554         * the profile.
1555         *
1556         * @param newTargetRateFeedforward Parameter to modify
1557         * @return this object
1558         */
1559        public FieldCentricFacingAngle withTargetRateFeedforward(AngularVelocity newTargetRateFeedforward) {
1560            this.TargetRateFeedforward = newTargetRateFeedforward.in(RadiansPerSecond);
1561            return this;
1562        }
1563
1564        /**
1565         * Modifies the Deadband parameter and returns itself.
1566         * <p>
1567         * The allowable deadband of the request, in m/s.
1568         *
1569         * @param newDeadband Parameter to modify
1570         * @return this object
1571         */
1572        public FieldCentricFacingAngle withDeadband(double newDeadband) {
1573            this.Deadband = newDeadband;
1574            return this;
1575        }
1576
1577        /**
1578         * Modifies the Deadband parameter and returns itself.
1579         * <p>
1580         * The allowable deadband of the request, in m/s.
1581         *
1582         * @param newDeadband Parameter to modify
1583         * @return this object
1584         */
1585        public FieldCentricFacingAngle withDeadband(LinearVelocity newDeadband) {
1586            this.Deadband = newDeadband.in(MetersPerSecond);
1587            return this;
1588        }
1589
1590        /**
1591         * Modifies the RotationalDeadband parameter and returns itself.
1592         * <p>
1593         * The rotational deadband of the request, in radians per second.
1594         *
1595         * @param newRotationalDeadband Parameter to modify
1596         * @return this object
1597         */
1598        public FieldCentricFacingAngle withRotationalDeadband(double newRotationalDeadband) {
1599            this.RotationalDeadband = newRotationalDeadband;
1600            return this;
1601        }
1602
1603        /**
1604         * Modifies the RotationalDeadband parameter and returns itself.
1605         * <p>
1606         * The rotational deadband of the request, in radians per second.
1607         *
1608         * @param newRotationalDeadband Parameter to modify
1609         * @return this object
1610         */
1611        public FieldCentricFacingAngle withRotationalDeadband(AngularVelocity newRotationalDeadband) {
1612            this.RotationalDeadband = newRotationalDeadband.in(RadiansPerSecond);
1613            return this;
1614        }
1615
1616        /**
1617         * Modifies the CenterOfRotation parameter and returns itself.
1618         * <p>
1619         * The center of rotation the robot should rotate around. This is (0,0) by
1620         * default, which will rotate around the center of the robot.
1621         *
1622         * @param newCenterOfRotation Parameter to modify
1623         * @return this object
1624         */
1625        public FieldCentricFacingAngle withCenterOfRotation(Translation2d newCenterOfRotation) {
1626            this.CenterOfRotation = newCenterOfRotation;
1627            return this;
1628        }
1629
1630        /**
1631         * Modifies the DriveRequestType parameter and returns itself.
1632         * <p>
1633         * The type of control request to use for the drive motor.
1634         *
1635         * @param newDriveRequestType Parameter to modify
1636         * @return this object
1637         */
1638        public FieldCentricFacingAngle withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
1639            this.DriveRequestType = newDriveRequestType;
1640            return this;
1641        }
1642
1643        /**
1644         * Modifies the SteerRequestType parameter and returns itself.
1645         * <p>
1646         * The type of control request to use for the drive motor.
1647         *
1648         * @param newSteerRequestType Parameter to modify
1649         * @return this object
1650         */
1651        public FieldCentricFacingAngle withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
1652            this.SteerRequestType = newSteerRequestType;
1653            return this;
1654        }
1655
1656        /**
1657         * Modifies the DesaturateWheelSpeeds parameter and returns itself.
1658         * <p>
1659         * Whether to desaturate wheel speeds before applying. For more information, see
1660         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
1661         *
1662         * @param newDesaturateWheelSpeeds Parameter to modify
1663         * @return this object
1664         */
1665        public FieldCentricFacingAngle withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) {
1666            this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds;
1667            return this;
1668        }
1669
1670        /**
1671         * Modifies the ForwardPerspective parameter and returns itself.
1672         * <p>
1673         * The perspective to use when determining which direction is forward.
1674         *
1675         * @param newForwardPerspective Parameter to modify
1676         * @return this object
1677         */
1678        public FieldCentricFacingAngle withForwardPerspective(ForwardPerspectiveValue newForwardPerspective) {
1679            this.ForwardPerspective = newForwardPerspective;
1680            return this;
1681        }
1682    }
1683
1684    /**
1685     * SysId-specific SwerveRequest to characterize the translational
1686     * characteristics of a swerve drivetrain.
1687     */
1688    public static class SysIdSwerveTranslation implements SwerveRequest {
1689        /**
1690         * Voltage to apply to drive wheels.
1691         */
1692        public double VoltsToApply = 0;
1693
1694        /* Local reference to a voltage request for the drive motors */
1695        private final VoltageOut m_driveRequest = new VoltageOut(0);
1696        /* Local reference to a position request for the steer motors */
1697        private final PositionVoltage m_steerRequest = new PositionVoltage(0);
1698
1699        public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) {
1700            for (int i = 0; i < modulesToApply.length; ++i) {
1701                modulesToApply[i].apply(m_driveRequest.withOutput(VoltsToApply), m_steerRequest.withPosition(0));
1702            }
1703            return StatusCode.OK;
1704        }
1705
1706        /**
1707         * Sets the voltage to apply to the drive wheels.
1708         *
1709         * @param volts Voltage to apply
1710         * @return this request
1711         */
1712        public SysIdSwerveTranslation withVolts(double volts) {
1713            VoltsToApply = volts;
1714            return this;
1715        }
1716        /**
1717         * Sets the voltage to apply to the drive wheels.
1718         *
1719         * @param volts Voltage to apply
1720         * @return this request
1721         */
1722        public SysIdSwerveTranslation withVolts(Voltage volts) {
1723            VoltsToApply = volts.in(Volts);
1724            return this;
1725        }
1726    }
1727
1728    /**
1729     * SysId-specific SwerveRequest to characterize the rotational
1730     * characteristics of a swerve drivetrain. This is useful to
1731     * characterize the heading controller for FieldCentricFacingAngle.
1732     * <p>
1733     * The RotationalRate of this swerve request should be logged.
1734     * When importing the log to SysId, set the "voltage" to
1735     * RotationalRate, "position" to the Pigeon 2 Yaw, and "velocity"
1736     * to the Pigeon 2 AngularVelocityZWorld. Note that the position
1737     * and velocity will both need to be scaled by pi/180.
1738     * <p>
1739     * Alternatively, the MotorVoltage of one of the drive motors can
1740     * be loaded into the SysId "voltage" field, which can be useful
1741     * when determining the MOI of the robot.
1742     */
1743    public static class SysIdSwerveRotation implements SwerveRequest {
1744        /**
1745         * The angular rate to rotate at, in radians per second.
1746         */
1747        public double RotationalRate = 0;
1748
1749        /* Local reference to a voltage request for the drive motors */
1750        private final VoltageOut m_driveRequest = new VoltageOut(0);
1751        /* Local reference to a position request for the steer motors */
1752        private final PositionVoltage m_steerRequest = new PositionVoltage(0);
1753
1754        public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) {
1755            for (int i = 0; i < modulesToApply.length; ++i) {
1756                var speedMps = RotationalRate * parameters.moduleLocations[i].getNorm();
1757                var angle = parameters.moduleLocations[i].getAngle().plus(Rotation2d.kCCW_90deg);
1758                modulesToApply[i].apply(
1759                    m_driveRequest.withOutput(speedMps / parameters.kMaxSpeedMps * 12.0),
1760                    m_steerRequest.withPosition(angle.getRotations())
1761                );
1762            }
1763            return StatusCode.OK;
1764        }
1765
1766        /**
1767         * Update the angular rate to rotate at, in radians per second.
1768         *
1769         * @param newRotationalRate Angular rate to rotate at
1770         * @return this request
1771         */
1772        public SysIdSwerveRotation withRotationalRate(double newRotationalRate) {
1773            RotationalRate = newRotationalRate;
1774            return this;
1775        }
1776        /**
1777         * Update the angular rate to rotate at.
1778         *
1779         * @param newRotationalRate The angular rate to rotate at
1780         * @return this request
1781         */
1782        public SysIdSwerveRotation withRotationalRate(AngularVelocity newRotationalRate) {
1783            RotationalRate = newRotationalRate.in(RadiansPerSecond);
1784            return this;
1785        }
1786    }
1787
1788    /**
1789     * SysId-specific SwerveRequest to characterize the steer module
1790     * characteristics of a swerve drivetrain.
1791     */
1792    public static class SysIdSwerveSteerGains implements SwerveRequest {
1793        /**
1794         * Voltage to apply to steer motors.
1795         */
1796        public double VoltsToApply = 0;
1797
1798        /* Local reference to a coast request for the drive motors */
1799        private final CoastOut m_driveRequest = new CoastOut();
1800        /* Local reference to a voltage request for the steer motors */
1801        private final VoltageOut m_steerRequest = new VoltageOut(0);
1802
1803        public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) {
1804            for (int i = 0; i < modulesToApply.length; ++i) {
1805                modulesToApply[i].apply(m_driveRequest, m_steerRequest.withOutput(VoltsToApply));
1806            }
1807            return StatusCode.OK;
1808        }
1809
1810        /**
1811         * Sets the voltage to apply to the steer motors.
1812         *
1813         * @param volts Voltage to apply
1814         * @return this request
1815         */
1816        public SysIdSwerveSteerGains withVolts(double volts) {
1817            VoltsToApply = volts;
1818            return this;
1819        }
1820        /**
1821         * Sets the voltage to apply to the steer motors.
1822         *
1823         * @param volts Voltage to apply
1824         * @return this request
1825         */
1826        public SysIdSwerveSteerGains withVolts(Voltage volts) {
1827            VoltsToApply = volts.in(Volts);
1828            return this;
1829        }
1830    }
1831}