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.swerve;
008
009import static edu.wpi.first.units.Units.Volts;
010import static edu.wpi.first.units.MutableMeasure.mutable;
011
012import com.ctre.phoenix6.StatusCode;
013import com.ctre.phoenix6.controls.VoltageOut;
014import com.ctre.phoenix6.mechanisms.swerve.utility.PhoenixPIDController;
015
016import edu.wpi.first.math.geometry.Pose2d;
017import edu.wpi.first.math.geometry.Rotation2d;
018import edu.wpi.first.math.geometry.Translation2d;
019import edu.wpi.first.math.kinematics.ChassisSpeeds;
020import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
021import edu.wpi.first.math.kinematics.SwerveModuleState;
022import edu.wpi.first.units.Measure;
023import edu.wpi.first.units.MutableMeasure;
024import edu.wpi.first.units.Voltage;
025
026/**
027 * Container for all the Swerve Requests. Use this to find all applicable swerve
028 * drive requests.
029 * <p>
030 * This is also an interface common to all swerve drive control requests that
031 * allow the
032 * request to calculate the state to apply to the modules.
033 */
034public interface SwerveRequest {
035
036    /**
037     * The reference for "forward" is sometimes different if you're talking
038     * about field relative. This addresses which forward to use.
039     */
040    public enum ForwardReference {
041        /**
042         * This forward reference makes it so "forward" (positive X) is always towards the red alliance.
043         * This is important in situations such as path following where positive X is always towards the
044         * red alliance wall, regardless of where the operator physically are located.
045         */
046        RedAlliance,
047        /**
048         * This forward references makes it so "forward" (positive X) is determined from the operator's
049         * perspective. This is important for most teleop driven field-centric requests, where positive
050         * X really means to drive away from the operator.
051         * <p>
052         * <b>Important</b>: Users must specify the OperatorPerspective with {@link SwerveDrivetrain} object
053         */
054        OperatorPerspective
055    }
056
057    /*
058     * Contains everything the control requests need to calculate the module state.
059     */
060    public class SwerveControlRequestParameters {
061        public SwerveDriveKinematics kinematics;
062        public ChassisSpeeds currentChassisSpeed;
063        public Pose2d currentPose;
064        public double timestamp;
065        public Translation2d[] swervePositions;
066        public Rotation2d operatorForwardDirection;
067        public double updatePeriod;
068    }
069
070    /**
071     * Applies this swerve request to the given modules.
072     * This is typically called by the SwerveDrivetrain.
073     *
074     * @param parameters Parameters the control request needs to calculate the module state
075     * @param modulesToApply Modules to which the control request is applied
076     * @return Status code of sending the request
077     */
078    public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply);
079
080    /**
081     * Sets the swerve drive module states to point inward on the
082     * robot in an "X" fashion, creating a natural brake which will
083     * oppose any motion.
084     */
085    public class SwerveDriveBrake implements SwerveRequest {
086
087        /**
088         * The type of control request to use for the drive motor.
089         */
090        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
091        /**
092         * The type of control request to use for the steer motor.
093         */
094        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.MotionMagic;
095
096        public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) {
097
098            for (int i = 0; i < modulesToApply.length; ++i) {
099                SwerveModuleState state = new SwerveModuleState(0, parameters.swervePositions[i].getAngle());
100                modulesToApply[i].apply(state, DriveRequestType, SteerRequestType);
101            }
102
103            return StatusCode.OK;
104        }
105
106        /**
107         * Sets the type of control request to use for the drive motor.
108         *
109         * @param driveRequestType The type of control request to use for the drive motor
110         * @return this request
111         */
112        public SwerveDriveBrake withDriveRequestType(SwerveModule.DriveRequestType driveRequestType) {
113            this.DriveRequestType = driveRequestType;
114            return this;
115        }
116        /**
117         * Sets the type of control request to use for the steer motor.
118         *
119         * @param steerRequestType The type of control request to use for the steer motor
120         * @return this request
121         */
122        public SwerveDriveBrake withSteerRequestType(SwerveModule.SteerRequestType steerRequestType) {
123            this.SteerRequestType = steerRequestType;
124            return this;
125        }
126    }
127
128    /**
129     * Drives the swerve drivetrain in a field-centric manner.
130     * <p>
131     * When users use this request, they specify the direction the robot should
132     * travel
133     * oriented against the field, and the rate at which their robot should rotate
134     * about the center of the robot.
135     * <p>
136     * An example scenario is that the robot is oriented to the east,
137     * the VelocityX is +5 m/s, VelocityY is 0 m/s, and RotationRate is 0.5 rad/s.
138     * In this scenario, the robot would drive northward at 5 m/s and
139     * turn counterclockwise at 0.5 rad/s.
140     */
141    public class FieldCentric implements SwerveRequest {
142        /**
143         * The velocity in the X direction, in m/s.
144         * X is defined as forward according to WPILib convention,
145         * so this determines how fast to travel forward.
146         */
147        public double VelocityX = 0;
148        /**
149         * The velocity in the Y direction, in m/s.
150         * Y is defined as to the left according to WPILib convention,
151         * so this determines how fast to travel to the left.
152         */
153        public double VelocityY = 0;
154        /**
155         * The angular rate to rotate at, in radians per second.
156         * Angular rate is defined as counterclockwise positive,
157         * so this determines how fast to turn counterclockwise.
158         */
159        public double RotationalRate = 0;
160        /**
161         * The allowable deadband of the request.
162         */
163        public double Deadband = 0;
164        /**
165         * The rotational deadband of the request.
166         */
167        public double RotationalDeadband = 0;
168        /**
169         * The center of rotation the robot should rotate around.
170         * This is (0,0) by default, which will rotate around the center of the robot.
171         */
172        public Translation2d CenterOfRotation = new Translation2d();
173
174        /**
175         * The type of control request to use for the drive motor.
176         */
177        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
178        /**
179         * The type of control request to use for the steer motor.
180         */
181        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.MotionMagic;
182
183        /**
184         * The perspective to use when determining which direction is forward.
185         */
186        public ForwardReference ForwardReference = SwerveRequest.ForwardReference.OperatorPerspective;
187
188        /**
189         * The last applied state in case we don't have anything to drive.
190         */
191        protected SwerveModuleState[] m_lastAppliedState = null;
192
193        public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) {
194            double toApplyX = VelocityX;
195            double toApplyY = VelocityY;
196            if (ForwardReference == SwerveRequest.ForwardReference.OperatorPerspective) {
197                /* If we're operator perspective, modify the X/Y translation by the angle */
198                Translation2d tmp = new Translation2d(toApplyX, toApplyY);
199                tmp = tmp.rotateBy(parameters.operatorForwardDirection);
200                toApplyX = tmp.getX();
201                toApplyY = tmp.getY();
202            }
203            double toApplyOmega = RotationalRate;
204            if (Math.sqrt(toApplyX * toApplyX + toApplyY * toApplyY) < Deadband) {
205                toApplyX = 0;
206                toApplyY = 0;
207            }
208            if (Math.abs(toApplyOmega) < RotationalDeadband) {
209                toApplyOmega = 0;
210            }
211
212            ChassisSpeeds speeds = ChassisSpeeds.discretize(ChassisSpeeds.fromFieldRelativeSpeeds(toApplyX, toApplyY, toApplyOmega,
213                        parameters.currentPose.getRotation()), parameters.updatePeriod);
214
215            var states = parameters.kinematics.toSwerveModuleStates(speeds, CenterOfRotation);
216
217            for (int i = 0; i < modulesToApply.length; ++i) {
218                modulesToApply[i].apply(states[i], DriveRequestType, SteerRequestType);
219            }
220
221            return StatusCode.OK;
222        }
223
224        /**
225         * Sets the velocity in the X direction, in m/s.
226         * X is defined as forward according to WPILib convention,
227         * so this determines how fast to travel forward.
228         *
229         * @param velocityX Velocity in the X direction, in m/s
230         * @return this request
231         */
232        public FieldCentric withVelocityX(double velocityX) {
233            this.VelocityX = velocityX;
234            return this;
235        }
236
237        /**
238         * Sets the velocity in the Y direction, in m/s.
239         * Y is defined as to the left according to WPILib convention,
240         * so this determines how fast to travel to the left.
241         *
242         * @param velocityY Velocity in the Y direction, in m/s
243         * @return this request
244         */
245        public FieldCentric withVelocityY(double velocityY) {
246            this.VelocityY = velocityY;
247            return this;
248        }
249
250        /**
251         * The angular rate to rotate at, in radians per second.
252         * Angular rate is defined as counterclockwise positive,
253         * so this determines how fast to turn counterclockwise.
254         *
255         * @param rotationalRate Angular rate to rotate at, in radians per second
256         * @return this request
257         */
258        public FieldCentric withRotationalRate(double rotationalRate) {
259            this.RotationalRate = rotationalRate;
260            return this;
261        }
262
263        /**
264         * Sets the allowable deadband of the request.
265         *
266         * @param deadband Allowable deadband of the request
267         * @return this request
268         */
269        public FieldCentric withDeadband(double deadband) {
270            this.Deadband = deadband;
271            return this;
272        }
273        /**
274         * Sets the rotational deadband of the request.
275         *
276         * @param rotationalDeadband Rotational deadband of the request
277         * @return this request
278         */
279        public FieldCentric withRotationalDeadband(double rotationalDeadband) {
280            this.RotationalDeadband = rotationalDeadband;
281            return this;
282        }
283        /**
284         * Sets the center of rotation of the request
285         *
286         * @param centerOfRotation The center of rotation the robot should rotate around.
287         * @return this request
288         */
289        public FieldCentric withCenterOfRotation(Translation2d centerOfRotation) {
290            this.CenterOfRotation = centerOfRotation;
291            return this;
292        }
293
294        /**
295         * Sets the type of control request to use for the drive motor.
296         *
297         * @param driveRequestType The type of control request to use for the drive motor
298         * @return this request
299         */
300        public FieldCentric withDriveRequestType(SwerveModule.DriveRequestType driveRequestType) {
301            this.DriveRequestType = driveRequestType;
302            return this;
303        }
304        /**
305         * Sets the type of control request to use for the steer motor.
306         *
307         * @param steerRequestType The type of control request to use for the steer motor
308         * @return this request
309         */
310        public FieldCentric withSteerRequestType(SwerveModule.SteerRequestType steerRequestType) {
311            this.SteerRequestType = steerRequestType;
312            return this;
313        }
314    }
315
316    /**
317     * Drives the swerve drivetrain in a field-centric manner, maintaining a
318     * specified heading
319     * angle to ensure the robot is facing the desired direction
320     * <p>
321     * When users use this request, they specify the direction the robot should
322     * travel
323     * oriented against the field, and the direction the robot should be facing.
324     * <p>
325     * An example scenario is that the robot is oriented to the east,
326     * the VelocityX is +5 m/s, VelocityY is 0 m/s, and TargetDirection is 180
327     * degrees.
328     * In this scenario, the robot would drive northward at 5 m/s and
329     * turn clockwise to a target of 180 degrees.
330     * <p>
331     * This control request is especially useful for autonomous control, where the
332     * robot should be facing a changing direction throughout the motion.
333     */
334    public class FieldCentricFacingAngle implements SwerveRequest {
335        /**
336         * The velocity in the X direction, in m/s.
337         * X is defined as forward according to WPILib convention,
338         * so this determines how fast to travel forward.
339         */
340        public double VelocityX = 0;
341        /**
342         * The velocity in the Y direction, in m/s.
343         * Y is defined as to the left according to WPILib convention,
344         * so this determines how fast to travel to the left.
345         */
346        public double VelocityY = 0;
347        /**
348         * The desired direction to face.
349         * 0 Degrees is defined as in the direction of the X axis.
350         * As a result, a TargetDirection of 90 degrees will point along
351         * the Y axis, or to the left.
352         */
353        public Rotation2d TargetDirection = new Rotation2d();
354
355        /**
356         * The allowable deadband of the request.
357         */
358        public double Deadband = 0;
359        /**
360         * The rotational deadband of the request.
361         */
362        public double RotationalDeadband = 0;
363        /**
364         * The center of rotation the robot should rotate around.
365         * This is (0,0) by default, which will rotate around the center of the robot.
366         */
367        public Translation2d CenterOfRotation = new Translation2d();
368
369        /**
370         * The type of control request to use for the drive motor.
371         */
372        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
373        /**
374         * The type of control request to use for the steer motor.
375         */
376        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.MotionMagic;
377
378        /**
379         * The PID controller used to maintain the desired heading.
380         * Users can specify the PID gains to change how aggressively to maintain
381         * heading.
382         * <p>
383         * This PID controller operates on heading radians and outputs a target
384         * rotational rate in radians per second.
385         */
386        public PhoenixPIDController HeadingController = new PhoenixPIDController(0, 0, 0);
387
388        /**
389         * The perspective to use when determining which direction is forward.
390         */
391        public ForwardReference ForwardReference = SwerveRequest.ForwardReference.OperatorPerspective;
392
393        public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) {
394            double toApplyX = VelocityX;
395            double toApplyY = VelocityY;
396            Rotation2d angleToFace = TargetDirection;
397            if (ForwardReference == SwerveRequest.ForwardReference.OperatorPerspective) {
398                /* If we're operator perspective, modify the X/Y translation by the angle */
399                Translation2d tmp = new Translation2d(toApplyX, toApplyY);
400                tmp = tmp.rotateBy(parameters.operatorForwardDirection);
401                toApplyX = tmp.getX();
402                toApplyY = tmp.getY();
403                /* And rotate the direction we want to face by the angle */
404                angleToFace = angleToFace.rotateBy(parameters.operatorForwardDirection);
405            }
406
407            double rotationRate = HeadingController.calculate(parameters.currentPose.getRotation().getRadians(),
408                    angleToFace.getRadians(), parameters.timestamp);
409
410            double toApplyOmega = rotationRate;
411            if (Math.sqrt(toApplyX * toApplyX + toApplyY * toApplyY) < Deadband) {
412                toApplyX = 0;
413                toApplyY = 0;
414            }
415            if (Math.abs(toApplyOmega) < RotationalDeadband) {
416                toApplyOmega = 0;
417            }
418
419            ChassisSpeeds speeds = ChassisSpeeds.discretize(ChassisSpeeds.fromFieldRelativeSpeeds(toApplyX, toApplyY, toApplyOmega,
420                    parameters.currentPose.getRotation()), parameters.updatePeriod);
421
422            var states = parameters.kinematics.toSwerveModuleStates(speeds, CenterOfRotation);
423
424            for (int i = 0; i < modulesToApply.length; ++i) {
425                modulesToApply[i].apply(states[i], DriveRequestType, SteerRequestType);
426            }
427
428            return StatusCode.OK;
429        }
430
431        /**
432         * Sets the velocity in the X direction, in m/s.
433         * X is defined as forward according to WPILib convention,
434         * so this determines how fast to travel forward.
435         *
436         * @param velocityX Velocity in the X direction, in m/s
437         * @return this request
438         */
439        public FieldCentricFacingAngle withVelocityX(double velocityX) {
440            this.VelocityX = velocityX;
441            return this;
442        }
443
444        /**
445         * Sets the velocity in the Y direction, in m/s.
446         * Y is defined as to the left according to WPILib convention,
447         * so this determines how fast to travel to the left.
448         *
449         * @param velocityY Velocity in the Y direction, in m/s
450         * @return this request
451         */
452        public FieldCentricFacingAngle withVelocityY(double velocityY) {
453            this.VelocityY = velocityY;
454            return this;
455        }
456
457        /**
458         * Sets the desired direction to face.
459         * 0 Degrees is defined as in the direction of the X axis.
460         * As a result, a TargetDirection of 90 degrees will point along
461         * the Y axis, or to the left.
462         *
463         * @param targetDirection Desired direction to face
464         * @return this request
465         */
466        public FieldCentricFacingAngle withTargetDirection(Rotation2d targetDirection) {
467            this.TargetDirection = targetDirection;
468            return this;
469        }
470
471        /**
472         * Sets the allowable deadband of the request.
473         *
474         * @param deadband Allowable deadband of the request
475         * @return this request
476         */
477        public FieldCentricFacingAngle withDeadband(double deadband) {
478            this.Deadband = deadband;
479            return this;
480        }
481        /**
482         * Sets the rotational deadband of the request.
483         *
484         * @param rotationalDeadband Rotational deadband of the request
485         * @return this request
486         */
487        public FieldCentricFacingAngle withRotationalDeadband(double rotationalDeadband) {
488            this.RotationalDeadband = rotationalDeadband;
489            return this;
490        }
491        /**
492         * Sets the center of rotation of the request
493         *
494         * @param centerOfRotation The center of rotation the robot should rotate around.
495         * @return this request
496         */
497        public FieldCentricFacingAngle withCenterOfRotation(Translation2d centerOfRotation) {
498            this.CenterOfRotation = centerOfRotation;
499            return this;
500        }
501
502        /**
503         * Sets the type of control request to use for the drive motor.
504         *
505         * @param driveRequestType The type of control request to use for the drive motor
506         * @return this request
507         */
508        public FieldCentricFacingAngle withDriveRequestType(SwerveModule.DriveRequestType driveRequestType) {
509            this.DriveRequestType = driveRequestType;
510            return this;
511        }
512        /**
513         * Sets the type of control request to use for the steer motor.
514         *
515         * @param steerRequestType The type of control request to use for the steer motor
516         * @return this request
517         */
518        public FieldCentricFacingAngle withSteerRequestType(SwerveModule.SteerRequestType steerRequestType) {
519            this.SteerRequestType = steerRequestType;
520            return this;
521        }
522    }
523
524    /**
525     * Does nothing to the swerve module state. This is the default state of a newly
526     * created swerve drive mechanism.
527     */
528    public class Idle implements SwerveRequest {
529        public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) {
530            return StatusCode.OK;
531        }
532    }
533
534    /**
535     * Sets the swerve drive modules to point to a specified direction.
536     */
537    public class PointWheelsAt implements SwerveRequest {
538
539        /**
540         * The direction to point the modules toward.
541         * This direction is still optimized to what the module was previously at.
542         */
543        public Rotation2d ModuleDirection = new Rotation2d();
544        /**
545         * The type of control request to use for the drive motor.
546         */
547        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
548        /**
549         * The type of control request to use for the steer motor.
550         */
551        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.MotionMagic;
552
553        public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) {
554
555            for (int i = 0; i < modulesToApply.length; ++i) {
556                SwerveModuleState state = new SwerveModuleState(0, ModuleDirection);
557                modulesToApply[i].apply(state, DriveRequestType, SteerRequestType);
558            }
559
560            return StatusCode.OK;
561        }
562
563        /**
564         * Sets the direction to point the modules toward.
565         * This direction is still optimized to what the module was previously at.
566         *
567         * @param moduleDirection Direction to point the modules toward
568         * @return this request
569         */
570        public PointWheelsAt withModuleDirection(Rotation2d moduleDirection) {
571            this.ModuleDirection = moduleDirection;
572            return this;
573        }
574
575        /**
576         * Sets the type of control request to use for the drive motor.
577         *
578         * @param driveRequestType The type of control request to use for the drive motor
579         * @return this request
580         */
581        public PointWheelsAt withDriveRequestType(SwerveModule.DriveRequestType driveRequestType) {
582            this.DriveRequestType = driveRequestType;
583            return this;
584        }
585        /**
586         * Sets the type of control request to use for the steer motor.
587         *
588         * @param steerRequestType The type of control request to use for the steer motor
589         * @return this request
590         */
591        public PointWheelsAt withSteerRequestType(SwerveModule.SteerRequestType steerRequestType) {
592            this.SteerRequestType = steerRequestType;
593            return this;
594        }
595    }
596
597    /**
598     * Drives the swerve drivetrain in a robot-centric manner.
599     * <p>
600     * When users use this request, they specify the direction the robot should
601     * travel
602     * oriented against the robot itself, and the rate at which their
603     * robot should rotate about the center of the robot.
604     * <p>
605     * An example scenario is that the robot is oriented to the east,
606     * the VelocityX is +5 m/s, VelocityY is 0 m/s, and RotationRate is 0.5 rad/s.
607     * In this scenario, the robot would drive eastward at 5 m/s and
608     * turn counterclockwise at 0.5 rad/s.
609     */
610    public class RobotCentric implements SwerveRequest {
611        /**
612         * The velocity in the X direction, in m/s.
613         * X is defined as forward according to WPILib convention,
614         * so this determines how fast to travel forward.
615         */
616        public double VelocityX = 0;
617        /**
618         * The velocity in the Y direction, in m/s.
619         * Y is defined as to the left according to WPILib convention,
620         * so this determines how fast to travel to the left.
621         */
622        public double VelocityY = 0;
623        /**
624         * The angular rate to rotate at, in radians per second.
625         * Angular rate is defined as counterclockwise positive,
626         * so this determines how fast to turn counterclockwise.
627         */
628        public double RotationalRate = 0;
629
630        /**
631         * The allowable deadband of the request.
632         */
633        public double Deadband = 0;
634        /**
635         * The rotational deadband of the request.
636         */
637        public double RotationalDeadband = 0;
638        /**
639         * The center of rotation the robot should rotate around.
640         * This is (0,0) by default, which will rotate around the center of the robot.
641         */
642        public Translation2d CenterOfRotation = new Translation2d();
643
644        /**
645         * The type of control request to use for the drive motor.
646         */
647        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
648        /**
649         * The type of control request to use for the steer motor.
650         */
651        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.MotionMagic;
652
653        public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) {
654            double toApplyX = VelocityX;
655            double toApplyY = VelocityY;
656            double toApplyOmega = RotationalRate;
657            if (Math.sqrt(toApplyX * toApplyX + toApplyY * toApplyY) < Deadband) {
658                toApplyX = 0;
659                toApplyY = 0;
660            }
661            if (Math.abs(toApplyOmega) < RotationalDeadband) {
662                toApplyOmega = 0;
663            }
664            ChassisSpeeds speeds = new ChassisSpeeds(toApplyX, toApplyY, toApplyOmega);
665
666            var states = parameters.kinematics.toSwerveModuleStates(speeds, CenterOfRotation);
667
668            for (int i = 0; i < modulesToApply.length; ++i) {
669                modulesToApply[i].apply(states[i], DriveRequestType, SteerRequestType);
670            }
671
672            return StatusCode.OK;
673        }
674
675        /**
676         * Sets the velocity in the X direction, in m/s.
677         * X is defined as forward according to WPILib convention,
678         * so this determines how fast to travel forward.
679         *
680         * @param velocityX Velocity in the X direction, in m/s
681         * @return this request
682         */
683        public RobotCentric withVelocityX(double velocityX) {
684            this.VelocityX = velocityX;
685            return this;
686        }
687
688        /**
689         * Sets the velocity in the Y direction, in m/s.
690         * Y is defined as to the left according to WPILib convention,
691         * so this determines how fast to travel to the left.
692         *
693         * @param velocityY Velocity in the Y direction, in m/s
694         * @return this request
695         */
696        public RobotCentric withVelocityY(double velocityY) {
697            this.VelocityY = velocityY;
698            return this;
699        }
700
701        /**
702         * The angular rate to rotate at, in radians per second.
703         * Angular rate is defined as counterclockwise positive,
704         * so this determines how fast to turn counterclockwise.
705         *
706         * @param rotationalRate Angular rate to rotate at, in radians per second
707         * @return this request
708         */
709        public RobotCentric withRotationalRate(double rotationalRate) {
710            this.RotationalRate = rotationalRate;
711            return this;
712        }
713
714        /**
715         * Sets the allowable deadband of the request.
716         *
717         * @param deadband Allowable deadband of the request
718         * @return this request
719         */
720        public RobotCentric withDeadband(double deadband) {
721            this.Deadband = deadband;
722            return this;
723        }
724        /**
725         * Sets the rotational deadband of the request.
726         *
727         * @param rotationalDeadband Rotational deadband of the request
728         * @return this request
729         */
730        public RobotCentric withRotationalDeadband(double rotationalDeadband) {
731            this.RotationalDeadband = rotationalDeadband;
732            return this;
733        }
734        /**
735         * Sets the center of rotation of the request
736         *
737         * @param centerOfRotation The center of rotation the robot should rotate around.
738         * @return this request
739         */
740        public RobotCentric withCenterOfRotation(Translation2d centerOfRotation) {
741            this.CenterOfRotation = centerOfRotation;
742            return this;
743        }
744
745        /**
746         * Sets the type of control request to use for the drive motor.
747         *
748         * @param driveRequestType The type of control request to use for the drive motor
749         * @return this request
750         */
751        public RobotCentric withDriveRequestType(SwerveModule.DriveRequestType driveRequestType) {
752            this.DriveRequestType = driveRequestType;
753            return this;
754        }
755        /**
756         * Sets the type of control request to use for the steer motor.
757         *
758         * @param steerRequestType The type of control request to use for the steer motor
759         * @return this request
760         */
761        public RobotCentric withSteerRequestType(SwerveModule.SteerRequestType steerRequestType) {
762            this.SteerRequestType = steerRequestType;
763            return this;
764        }
765    }
766
767    /**
768     * Accepts a generic ChassisSpeeds to apply to the drivetrain.
769     */
770    public class ApplyChassisSpeeds implements SwerveRequest {
771
772        /**
773         * The chassis speeds to apply to the drivetrain.
774         */
775        public ChassisSpeeds Speeds = new ChassisSpeeds();
776        /**
777         * The center of rotation to rotate around.
778         */
779        public Translation2d CenterOfRotation = new Translation2d(0, 0);
780        /**
781         * The type of control request to use for the drive motor.
782         */
783        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
784        /**
785         * The type of control request to use for the steer motor.
786         */
787        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.MotionMagic;
788
789        public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) {
790            var states = parameters.kinematics.toSwerveModuleStates(Speeds, CenterOfRotation);
791            for (int i = 0; i < modulesToApply.length; ++i) {
792                modulesToApply[i].apply(states[i], DriveRequestType, SteerRequestType);
793            }
794
795            return StatusCode.OK;
796        }
797
798        /**
799         * Sets the chassis speeds to apply to the drivetrain.
800         *
801         * @param speeds Chassis speeds to apply to the drivetrain
802         * @return this request
803         */
804        public ApplyChassisSpeeds withSpeeds(ChassisSpeeds speeds) {
805            this.Speeds = speeds;
806            return this;
807        }
808        /**
809         * Sets the center of rotation to rotate around.
810         *
811         * @param centerOfRotation Center of rotation to rotate around
812         * @return this request
813         */
814        public ApplyChassisSpeeds withCenterOfRotation(Translation2d centerOfRotation) {
815            this.CenterOfRotation = centerOfRotation;
816            return this;
817        }
818
819        /**
820         * Sets the type of control request to use for the drive motor.
821         *
822         * @param driveRequestType The type of control request to use for the drive motor
823         * @return this request
824         */
825        public ApplyChassisSpeeds withDriveRequestType(SwerveModule.DriveRequestType driveRequestType) {
826            this.DriveRequestType = driveRequestType;
827            return this;
828        }
829        /**
830         * Sets the type of control request to use for the steer motor.
831         *
832         * @param steerRequestType The type of control request to use for the steer motor
833         * @return this request
834         */
835        public ApplyChassisSpeeds withSteerRequestType(SwerveModule.SteerRequestType steerRequestType) {
836            this.SteerRequestType = steerRequestType;
837            return this;
838        }
839    }
840
841    /**
842     * SysId-specific SwerveRequest to characterize the translational
843     * characteristics of a swerve drivetrain.
844     */
845    public class SysIdSwerveTranslation implements SwerveRequest {
846        /* Voltage to apply to drive wheels. This is final to enforce mutating the value */
847        public final MutableMeasure<Voltage> VoltsToApply = mutable(Volts.of(0));
848
849        /* Local reference to a voltage request to drive the motors with */
850        private VoltageOut m_voltRequest = new VoltageOut(0);
851
852        public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) {
853            for (int i = 0; i < modulesToApply.length; ++i) {
854                modulesToApply[i].applyCharacterization(Rotation2d.fromDegrees(0), m_voltRequest.withOutput(VoltsToApply.in(Volts)));
855            }
856            return StatusCode.OK;
857        }
858
859        /**
860         * Update the voltage to apply to the drive wheels.
861         *
862         * @param Volts Voltage to apply
863         * @return this request
864         */
865        public SysIdSwerveTranslation withVolts(Measure<Voltage> Volts) {
866            VoltsToApply.mut_replace(Volts);
867            return this;
868        }
869    }
870
871    /**
872     * SysId-specific SwerveRequest to characterize the rotational
873     * characteristics of a swerve drivetrain.
874     */
875    public class SysIdSwerveRotation implements SwerveRequest {
876        /* Voltage to apply to drive wheels. This is final to enforce mutating the value */
877        public final MutableMeasure<Voltage> VoltsToApply = mutable(Volts.of(0));
878
879        /* Local reference to a voltage request to drive the motors with */
880        private VoltageOut m_voltRequest = new VoltageOut(0);
881
882        public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) {
883            for (int i = 0; i < modulesToApply.length; ++i) {
884                modulesToApply[i].applyCharacterization(parameters.swervePositions[i].getAngle().plus(Rotation2d.fromDegrees(90)),
885                                                        m_voltRequest.withOutput(VoltsToApply.in(Volts)));
886            }
887            return StatusCode.OK;
888        }
889
890        /**
891         * Update the voltage to apply to the drive wheels.
892         *
893         * @param Volts Voltage to apply
894         * @return this request
895         */
896        public SysIdSwerveRotation withVolts(Measure<Voltage> Volts) {
897            VoltsToApply.mut_replace(Volts);
898            return this;
899        }
900    }
901
902    /**
903     * SysId-specific SwerveRequest to characterize the steer module
904     * characteristics of a swerve drivetrain.
905     */
906    public class SysIdSwerveSteerGains implements SwerveRequest {
907        /* Voltage to apply to drive wheels. This is final to enforce mutating the value */
908        public final MutableMeasure<Voltage> VoltsToApply = mutable(Volts.of(0));
909
910        /* Local reference to a voltage request to drive the motors with */
911        private VoltageOut m_voltRequest = new VoltageOut(0);
912
913        public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) {
914            for (int i = 0; i < modulesToApply.length; ++i) {
915                modulesToApply[i].getSteerMotor().setControl(m_voltRequest.withOutput(VoltsToApply.in(Volts)));
916                modulesToApply[i].getDriveMotor().setControl(m_voltRequest.withOutput(0));
917            }
918            return StatusCode.OK;
919        }
920
921        /**
922         * Update the voltage to apply to the drive wheels.
923         *
924         * @param Volts Voltage to apply
925         * @return this request
926         */
927        public SysIdSwerveSteerGains withVolts(Measure<Voltage> Volts) {
928            VoltsToApply.mut_replace(Volts);
929            return this;
930        }
931    }
932}