Skip to content

Commit 94f3a0c

Browse files
jhhbrown1jhhbrown1
committed
introduced PathDriveCommand
1 parent 9d0d775 commit 94f3a0c

File tree

4 files changed

+290
-73
lines changed

4 files changed

+290
-73
lines changed

‎src/main/java/frc/robot/CommandComposer.java‎

Lines changed: 21 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -5,14 +5,14 @@
55
import static frc.robot.Constants.DriveConstants.*;
66
import static frc.robot.subsystems.PoseEstimationSubsystem.*;
77

8-
import java.util.function.Supplier;
8+
import java.util.LinkedList;
99

1010
import edu.wpi.first.math.geometry.Pose2d;
1111
import edu.wpi.first.math.geometry.Rotation2d;
12-
import edu.wpi.first.wpilibj.Timer;
1312
import edu.wpi.first.wpilibj2.command.Command;
1413
import edu.wpi.first.wpilibj2.command.Commands;
1514
import frc.robot.commands.drive.DriveCommand2Controllers;
15+
import frc.robot.commands.drive.PathDriveCommand;
1616
import frc.robot.subsystems.DriveSubsystem;
1717
import frc.robot.subsystems.PoseEstimationSubsystem;
1818

@@ -143,43 +143,30 @@ public static Command moveOnSquare(double sideLength, double distanceTolerance,
143143
* Returns a {@code Command} for moving the robot on a circle.
144144
*
145145
* @param radius the radius of the circle in meters
146-
* @param initialAngularVelocity the initial angular velocity in degrees per
147-
* second which describes how quickly the robot is moving on the circle
148-
* @param finalAngularVelocity the final angular velocity in degrees per
149-
* second which describes how quickly the robot is moving on the circle
146+
* @param initialAngularIncrement the initial angular increment in degrees which
147+
* describes how quickly the robot to move on the circle
148+
* @param finalAngularIncrement the final angular increment in degrees which
149+
* describes how quickly the robot to move on the circle
150150
* @param distanceTolerance the distance error in meters which is tolerable
151151
* @param angleTolerance the angle error in degrees which is tolerable
152-
* @param timeout the maximum amount of the time given to the {@code Command}
152+
* @param intermediateToleranceRatio the ratio to apply to the distance and
153+
* angle tolerances for intermeidate target {@code Pose2d}s
154+
* @param poseCount the number of {@code Pose2d}s on the circle
153155
*
154156
* @return a {@code Command} for moving the robot on a circle
155157
*/
156-
public static Command moveOnCircle(double radius, double initialAngularVelocity, double finalAngularVelocity,
157-
double distanceTolerance, double angleTolerance, double timeout) {
158-
159-
Timer timer = new Timer();
160-
161-
Supplier<Pose2d> s = new Supplier<Pose2d>() {
162-
163-
Rotation2d angle = Rotation2d.kZero;
164-
165-
@Override
166-
public Pose2d get() {
167-
var p = new Pose2d(translation(radius, 0).rotateBy(angle), angle);
168-
double progress = timer.get() / timeout;
169-
double angularVelocity = progress * finalAngularVelocity + (1 - progress) * initialAngularVelocity;
170-
angle = angle.plus(rotation(angularVelocity));
171-
return p;
172-
}
173-
};
174-
return new DriveCommand2Controllers(m_driveSubsystem, s, true, distanceTolerance, angleTolerance) {
175-
176-
@Override
177-
public void initialize() {
178-
super.initialize();
179-
timer.restart();
180-
}
181-
182-
}.withTimeout(timeout);
158+
public static Command moveOnCircle(double radius, double initialAngularIncrement, double finalAngularIncrement,
159+
double distanceTolerance, double angleTolerance, double intermediateToleranceRatio, int poseCount) {
160+
Rotation2d angle = Rotation2d.kZero;
161+
var l = new LinkedList<Pose2d>();
162+
for (double i = 0; i < poseCount; i++) {
163+
l.add(new Pose2d(translation(radius, 0).rotateBy(angle), angle));
164+
double progress = i / poseCount;
165+
double angularVelocity = progress * finalAngularIncrement + (1 - progress) * initialAngularIncrement;
166+
angle = angle.plus(rotation(angularVelocity));
167+
}
168+
return new PathDriveCommand(m_driveSubsystem, distanceTolerance, angleTolerance, intermediateToleranceRatio,
169+
l.toArray(new Pose2d[0]));
183170
}
184171

185172
/**

‎src/main/java/frc/robot/Robot.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -87,7 +87,7 @@ public Robot() {
8787
m_testSelector
8888
.addOption(
8989
"Check PID Constants for Driving (Unit Circle)",
90-
CommandComposer.moveOnCircle(1, 1, 3, distanceTolerance, angleToleranceInDegrees, 10));
90+
CommandComposer.moveOnCircle(1, 8, 16, distanceTolerance, angleToleranceInDegrees, 10, 60));
9191
m_testSelector.addOption("Test Rotation", CommandComposer.testRotation());
9292
m_testSelector.addOption("Turn toward Tag 1", CommandComposer.turnTowardTag(1));
9393

@@ -150,7 +150,7 @@ Command driveWithAlignmentCommand(DoubleSupplier forwardSpeed, DoubleSupplier st
150150
if (closestTagPose == null)
151151
return m_driveSubsystem.getPose();
152152
return m_poseEstimationSubsystem.odometryCentricPose(closestTagPose.plus(robotToTag));
153-
}, false, distanceTolerance, angleToleranceInDegrees) {
153+
}, distanceTolerance, angleToleranceInDegrees) {
154154

155155
@Override
156156
public ChassisSpeeds chassisSpeeds() {

‎src/main/java/frc/robot/commands/drive/DriveCommand2Controllers.java‎

Lines changed: 12 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,8 @@
1414

1515
/**
1616
* This {@code DriveCommand2Controllers} aims to maneuver the robot to a certain
17-
* {@code Pose2d}.
18-
* It utilizes two {@code ProfiledPIDController}s to precisely control the
19-
* robot in the x, y, and yaw dimensions.
17+
* {@code Pose2d}. It utilizes two {@code ProfiledPIDController}s to precisely
18+
* control the robot in the x, y, and yaw dimensions.
2019
*
2120
* @author Jeong-Hyon Hwang (jhhbrown@gmail.com)
2221
* @author Andrew Hwang (u.andrew.h@gmail.com)
@@ -72,13 +71,6 @@ public class DriveCommand2Controllers extends Command {
7271
*/
7372
protected double m_angleTolerance;
7473

75-
/**
76-
* A boolean indicating whether or not the target needs to be determined
77-
* continuously or only once at the commencement of the
78-
* {@code DriveCommand2Controllers}.
79-
*/
80-
protected boolean m_continuousTargeting;
81-
8274
/**
8375
* Constructs a new {@code DriveCommand2Controllers} whose purpose is to move
8476
* the
@@ -91,7 +83,7 @@ public class DriveCommand2Controllers extends Command {
9183
*/
9284
public DriveCommand2Controllers(DriveSubsystem driveSubsystem, Pose2d targetPose, double distanceTolerance,
9385
double angleToleranceInDegrees) {
94-
this(driveSubsystem, () -> driveSubsystem.getPose(), () -> targetPose, false, distanceTolerance,
86+
this(driveSubsystem, () -> driveSubsystem.getPose(), () -> targetPose, distanceTolerance,
9587
angleToleranceInDegrees);
9688
}
9789

@@ -106,16 +98,12 @@ public DriveCommand2Controllers(DriveSubsystem driveSubsystem, Pose2d targetPose
10698
* {@code DriveCommand2Controllers} (i.e., when the scheduler
10799
* begins to periodically execute this
108100
* {@code DriveCommand2Controllers})
109-
* @param continuousTargeting a boolean value indicating whether or not the
110-
* targetPoseSupplier needs to be used continuously or only once at the
111-
* commencement of the {@code DriveCommand2Controllers}.
112101
* @param distanceTolerance the distance error in meters which is tolerable
113102
* @param angleToleranceInDegrees the angle error in degrees which is tolerable
114103
*/
115104
public DriveCommand2Controllers(DriveSubsystem driveSubsystem, Supplier<Pose2d> targetPoseSupplier,
116-
boolean continuousTargeting, double distanceTolerance, double angleToleranceInDegrees) {
117-
this(driveSubsystem, () -> driveSubsystem.getPose(), targetPoseSupplier, continuousTargeting,
118-
distanceTolerance,
105+
double distanceTolerance, double angleToleranceInDegrees) {
106+
this(driveSubsystem, () -> driveSubsystem.getPose(), targetPoseSupplier, distanceTolerance,
119107
angleToleranceInDegrees);
120108
}
121109

@@ -133,16 +121,12 @@ public DriveCommand2Controllers(DriveSubsystem driveSubsystem, Supplier<Pose2d>
133121
* {@code DriveCommand2Controllers} (i.e., when the scheduler
134122
* begins to periodically execute this
135123
* {@code DriveCommand2Controllers})
136-
* @param continuousTargeting a boolean value indicating whether or not the
137-
* targetPoseSupplier needs to be used continuously or only once at the
138-
* commencement of the {@code DriveCommand2Controllers}.
139124
* @param distanceTolerance the distance error in meters which is tolerable
140125
* @param angleToleranceInDegrees the angle error in degrees which is tolerable
141126
*/
142127
public DriveCommand2Controllers(DriveSubsystem driveSubsystem, Supplier<Pose2d> poseSupplier,
143-
Supplier<Pose2d> targetPoseSupplier, boolean continuousTargeting, double distanceTolerance,
144-
double angleToleranceInDegrees) {
145-
this(driveSubsystem, poseSupplier, targetPoseSupplier, continuousTargeting, distanceTolerance,
128+
Supplier<Pose2d> targetPoseSupplier, double distanceTolerance, double angleToleranceInDegrees) {
129+
this(driveSubsystem, poseSupplier, targetPoseSupplier, distanceTolerance,
146130
angleToleranceInDegrees,
147131
new ProfiledPIDController(kDriveP, kDriveI, kDriveD,
148132
new TrapezoidProfile.Constraints(kDriveMaxSpeed, kDriveMaxAcceleration)),
@@ -165,9 +149,6 @@ public DriveCommand2Controllers(DriveSubsystem driveSubsystem, Supplier<Pose2d>
165149
* {@code DriveCommand2Controllers} (i.e., when the scheduler
166150
* begins to periodically execute this
167151
* {@code DriveCommand2Controllers})
168-
* @param continuousTargeting a boolean value indicating whether or not the
169-
* targetPoseSupplier needs to be used continuously or only once at the
170-
* commencement of the {@code DriveCommand2Controllers}
171152
* @param distanceTolerance the distance error in meters which is tolerable
172153
* @param angleToleranceInDegrees the angle error in degrees which is tolerable
173154
* @param controllerXY the {@code ProfiledPIDController} for controlling the
@@ -177,12 +158,11 @@ public DriveCommand2Controllers(DriveSubsystem driveSubsystem, Supplier<Pose2d>
177158
*
178159
*/
179160
public DriveCommand2Controllers(DriveSubsystem driveSubsystem, Supplier<Pose2d> poseSupplier,
180-
Supplier<Pose2d> targetPoseSupplier, boolean continuousTargeting, double distanceTolerance,
161+
Supplier<Pose2d> targetPoseSupplier, double distanceTolerance,
181162
double angleToleranceInDegrees, ProfiledPIDController controllerXY, ProfiledPIDController controllerYaw) {
182163
m_driveSubsystem = driveSubsystem;
183164
m_poseSupplier = poseSupplier;
184165
m_targetPoseSupplier = targetPoseSupplier;
185-
m_continuousTargeting = continuousTargeting;
186166
m_distanceTolerance = distanceTolerance;
187167
m_angleTolerance = Math.toRadians(angleToleranceInDegrees);
188168
m_controllerXY = controllerXY;
@@ -228,22 +208,17 @@ public void execute() {
228208
*/
229209
public ChassisSpeeds chassisSpeeds() {
230210
var currentPose = m_poseSupplier.get();
231-
if (m_continuousTargeting)
232-
try {
233-
m_targetPose = m_targetPoseSupplier.get();
234-
} catch (Exception e) {
235-
// e.printStackTrace();
236-
}
237211
Translation2d current2target = m_targetPose.getTranslation()
238212
.minus(currentPose.getTranslation());
239213
double velocityX = 0, velocityY = 0;
240-
double distance = current2target.getNorm();
241-
double speed = Math.abs(m_controllerXY.calculate(distance, 0));
242-
if (distance > 0) {
214+
try {
215+
double distance = current2target.getNorm();
216+
double speed = Math.abs(m_controllerXY.calculate(distance, 0));
243217
speed = applyThreshold(speed, kDriveMinSpeed);
244218
var angle = current2target.getAngle();
245219
velocityX = speed * angle.getCos();
246220
velocityY = speed * angle.getSin();
221+
} catch (Exception e) {
247222
}
248223
double angularVelocityRadiansPerSecond = m_controllerYaw
249224
.calculate(currentPose.getRotation().getRadians(), m_targetPose.getRotation().getRadians());

0 commit comments

Comments
 (0)