1+ package frc .robot .commands .drive ;
2+
3+ import static frc .robot .Constants .DriveConstants .*;
4+
5+ import java .util .List ;
6+ import java .util .function .Supplier ;
7+
8+ import edu .wpi .first .math .controller .ProfiledPIDController ;
9+ import edu .wpi .first .math .geometry .Pose2d ;
10+ import edu .wpi .first .math .geometry .Translation2d ;
11+ import edu .wpi .first .math .kinematics .ChassisSpeeds ;
12+ import edu .wpi .first .math .trajectory .TrapezoidProfile ;
13+ import edu .wpi .first .wpilibj2 .command .Command ;
14+ import frc .robot .subsystems .DriveSubsystem ;
15+
16+ /**
17+ * This {@code DriveCommand} aims to maneuver the robot to a certain
18+ * {@code Pose2d}. It utilizes two {@code ProfiledPIDController}s to precisely
19+ * control the robot in the x, y, and yaw dimensions.
20+ *
21+ * @author Jeong-Hyon Hwang ([email protected] ) 22+ * @author Andrew Hwang ([email protected] ) 23+ */
24+ public class DriveCommand extends Command {
25+
26+ /**
27+ * The {@code DriveSubsystem} used by this {@code DriveCommand}.
28+ */
29+ protected DriveSubsystem m_driveSubsystem ;
30+
31+ /**
32+ * The {@code ProfiledPIDController} for controlling the robot in the x and y
33+ * dimensions in meters (input: error in meters, output: velocity in meters per
34+ * second).
35+ */
36+ protected ProfiledPIDController m_controllerXY ;
37+
38+ /**
39+ * The {@code ProfiledPIDController} for controlling the robot in the yaw
40+ * dimension in radians (input: error in radians, output: velocity in radians
41+ * per second).
42+ */
43+ protected ProfiledPIDController m_controllerYaw ;
44+
45+ /**
46+ * The distance error in meters which is tolerable.
47+ */
48+ protected double m_distanceTolerance ;
49+
50+ /**
51+ * The angle error in radians which is tolerable.
52+ */
53+ protected double m_angleTolerance ;
54+
55+ /**
56+ * The {@code Pose2d} to which the robot should move.
57+ */
58+ protected Pose2d m_targetPose ;
59+
60+ /**
61+ * The {@code Supplier<Pose2d>}s that provide the {@code Pose2d}s to which the
62+ * robot should move.
63+ */
64+ protected List <Supplier <Pose2d >> m_targetPoseSuppliers ;
65+
66+ /**
67+ * Constructs a new {@code DriveCommand} whose purpose is to move
68+ * the robot to a certain {@code Pose2d}.
69+ *
70+ * @param driveSubsystem the {@code DriveSubsystem} to use
71+ * @param distanceTolerance the distance error in meters which is tolerable
72+ * @param angleToleranceInDegrees the angle error in degrees which is tolerable
73+ * @param targetPose a {@code Pose2d} to which the robot should move.
74+ */
75+ public DriveCommand (DriveSubsystem driveSubsystem , double distanceTolerance , double angleToleranceInDegrees ,
76+ Pose2d targetPose ) {
77+ this (driveSubsystem , distanceTolerance , angleToleranceInDegrees , () -> targetPose );
78+ }
79+
80+ /**
81+ * Constructs a new {@code DriveCommand} whose purpose is to move
82+ * the robot to a certain {@code Pose2d}.
83+ *
84+ * @param driveSubsystem the {@code DriveSubsystem} to use
85+ * @param distanceTolerance the distance error in meters which is tolerable
86+ * @param angleToleranceInDegrees the angle error in degrees which is tolerable
87+ * @param targetPoseSupplier a {@code Supplier<Pose2d>} that provides the
88+ * {@code Pose2d} to which the robot should move. This is used at the
89+ * commencement of the {@code DriveCommand} (i.e., when the scheduler
90+ * begins to periodically execute the {@code DriveCommand})
91+ */
92+ public DriveCommand (DriveSubsystem driveSubsystem , double distanceTolerance , double angleToleranceInDegrees ,
93+ Supplier <Pose2d > targetPoseSupplier ) {
94+ this (driveSubsystem , distanceTolerance , angleToleranceInDegrees , List .of (targetPoseSupplier ));
95+ }
96+
97+ /**
98+ * Constructs a new {@code DriveCommand} whose purpose is to move
99+ * the robot to a certain {@code Pose2d}.
100+ *
101+ * @param driveSubsystem the {@code DriveSubsystem} to use
102+ * @param distanceTolerance the distance error in meters which is tolerable
103+ * @param angleToleranceInDegrees the angle error in degrees which is tolerable
104+ * @param targetPoseSuppliers {@code Supplier<Pose2d>}s that provide the
105+ * {@code Pose2d}s to which the robot should move.
106+ */
107+ public DriveCommand (DriveSubsystem driveSubsystem , double distanceTolerance , double angleToleranceInDegrees ,
108+ List <Supplier <Pose2d >> targetPoseSuppliers ) {
109+ m_driveSubsystem = driveSubsystem ;
110+ m_distanceTolerance = distanceTolerance ;
111+ m_angleTolerance = Math .toRadians (angleToleranceInDegrees );
112+ m_controllerXY = new ProfiledPIDController (kDriveP , kDriveI , kDriveD ,
113+ new TrapezoidProfile .Constraints (kDriveMaxSpeed , kDriveMaxAcceleration ));
114+ m_controllerYaw = new ProfiledPIDController (kTurnP , kTurnI , kTurnD ,
115+ new TrapezoidProfile .Constraints (kTurnMaxAngularSpeed ,
116+ kTurnMaxAcceleration ));
117+ m_controllerYaw .enableContinuousInput (0 , 2 * Math .PI );
118+ m_targetPoseSuppliers = targetPoseSuppliers ;
119+ addRequirements (m_driveSubsystem );
120+ }
121+
122+ /**
123+ * Is invoked at the commencement of this {@code DriveCommand} (i.e,
124+ * when the scheduler begins to periodically execute this
125+ * {@code DriveCommand}).
126+ */
127+ @ Override
128+ public void initialize () {
129+ Pose2d pose = m_driveSubsystem .getPose ();
130+ m_targetPose = pose ;
131+ try {
132+ m_targetPose = m_targetPoseSuppliers .get (0 ).get ();
133+ } catch (Exception e ) {
134+ e .printStackTrace ();
135+ m_targetPose = pose ;
136+ }
137+ m_controllerXY .setTolerance (m_distanceTolerance );
138+ m_controllerYaw .setTolerance (m_angleTolerance );
139+ m_controllerXY .reset (m_targetPose .minus (pose ).getTranslation ().getNorm ());
140+ m_controllerYaw .reset (pose .getRotation ().getRadians ());
141+ }
142+
143+ /**
144+ * Is invoked periodically by the scheduler until this
145+ * {@code DriveCommand} is either ended or interrupted.
146+ */
147+ @ Override
148+ public void execute () {
149+ m_driveSubsystem .drive (chassisSpeeds (), true );
150+ }
151+
152+ /**
153+ * Calculates the {@code ChassisSpeeds} to move the robot toward the target.
154+ *
155+ * @return the calculated {@code ChassisSpeeds} to move the robot toward the
156+ * target
157+ */
158+ public ChassisSpeeds chassisSpeeds () {
159+ var currentPose = m_driveSubsystem .getPose ();
160+ Translation2d current2target = m_targetPose .getTranslation ()
161+ .minus (currentPose .getTranslation ());
162+ double velocityX = 0 , velocityY = 0 ;
163+ double distance = current2target .getNorm ();
164+ double speed = Math .abs (m_controllerXY .calculate (distance , 0 ));
165+ if (speed > 1e-6 ) {
166+ speed = applyThreshold (speed , kDriveMinSpeed );
167+ var angle = current2target .getAngle ();
168+ velocityX = speed * angle .getCos ();
169+ velocityY = speed * angle .getSin ();
170+ }
171+ double angularVelocityRadiansPerSecond = m_controllerYaw
172+ .calculate (currentPose .getRotation ().getRadians (), m_targetPose .getRotation ().getRadians ());
173+ angularVelocityRadiansPerSecond = applyThreshold (angularVelocityRadiansPerSecond , kTurnMinAngularSpeed );
174+ return new ChassisSpeeds (velocityX , velocityY , angularVelocityRadiansPerSecond );
175+ }
176+
177+ /**
178+ * Is invoked once this {@code DriveCommand} is either ended or
179+ * interrupted.
180+ *
181+ * @param interrupted indicates if this {@code DriveCommand} was
182+ * interrupted
183+ */
184+ @ Override
185+ public void end (boolean interrupted ) {
186+ m_driveSubsystem .drive (0 , 0 , 0 , true );
187+ }
188+
189+ /**
190+ * Determines whether or not this {@code DriveCommand} needs to end.
191+ *
192+ * @return {@code true} if this {@code DriveCommand} needs to end;
193+ * {@code false} otherwise
194+ */
195+ @ Override
196+ public boolean isFinished () {
197+ return m_controllerXY .atGoal () && m_controllerYaw .atGoal ();
198+ }
199+
200+ /**
201+ * Applies the specified threshold to the specified value.
202+ *
203+ * @param value the value to be thresholded
204+ * @param threshold the threshold limit
205+ * @return the original value if the absolute value of that value is greater or
206+ * equal to the threshold; the threshold with the original value's sign
207+ * otherwise
208+ */
209+ public static double applyThreshold (double value , double threshold ) {
210+ return Math .abs (value ) < threshold ? Math .signum (value ) * threshold : value ;
211+ }
212+
213+ }
0 commit comments