Skip to content

Commit 1ac78dd

Browse files
jhhbrown1jhhbrown1
committed
introduced AlignCommand#follow
1 parent df8bc8b commit 1ac78dd

File tree

6 files changed

+103
-64
lines changed

6 files changed

+103
-64
lines changed

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

Lines changed: 31 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -4,9 +4,9 @@
44
import static frc.robot.Constants.*;
55

66
import java.util.Arrays;
7-
import java.util.LinkedList;
87
import java.util.List;
98

9+
import edu.wpi.first.math.geometry.Pose2d;
1010
import edu.wpi.first.math.geometry.Transform2d;
1111
import edu.wpi.first.wpilibj2.command.Command;
1212
import frc.robot.commands.AlignCommand;
@@ -44,7 +44,7 @@ public static Command testSubsystemsAndCommands() {
4444
* @param distanceTolerance the distance error in meters which is tolerable
4545
* @param angleTolerance the angle error in degrees which is tolerable
4646
*
47-
* @return a {@code Command} for moving 6 feet forward and then backward.
47+
* @return a {@code Command} for moving forward and then backward.
4848
*/
4949
public static Command moveForwardBackward(double distanceInFeet, double distanceTolerance,
5050
double angleTolerance) {
@@ -89,14 +89,12 @@ public static Command moveForwardBackward3Controllers(double distanceInFeet, dou
8989
*/
9090
public static Command visitTags(double distanceTolerance, double angleTolerance, Transform2d robotToTag,
9191
int... tagIDs) {
92-
List<DriveCommand> commands = Arrays.stream(tagIDs).mapToObj(i -> kFieldLayout.getTagPose(i))
93-
.filter(p -> p.isPresent())
94-
.map(p -> p.get())
95-
.map(p -> p.toPose2d().plus(robotToTag)).map(
96-
p -> AlignCommand
97-
.moveTo(
98-
m_driveSubsystem, m_poseEstimationSubsystem, p, distanceTolerance,
99-
angleTolerance))
92+
Pose2d[] poses = PoseEstimationSubsystem.tagPoses(tagIDs);
93+
var commands = Arrays.stream(poses).map(
94+
p -> AlignCommand
95+
.moveTo(
96+
m_driveSubsystem, m_poseEstimationSubsystem, p.plus(robotToTag), distanceTolerance,
97+
angleTolerance))
10098
.toList();
10199
return sequence(commands.toArray(new Command[0]));
102100
}
@@ -106,32 +104,21 @@ public static Command visitTags(double distanceTolerance, double angleTolerance,
106104
*
107105
* @param distanceTolerance the distance error in meters which is tolerable
108106
* @param angleTolerance the angle error in degrees which is tolerable
107+
* @param intermediateToleranceRatio the ratio of distance and angle tolerances
108+
* at intermediate {@code AprilTags} for faster movements
109109
* @param robotToTag the {@code Tranform2d} representing the pose of the
110110
* {@code AprilTag} relative to the robot when the robot is aligned
111111
* @param tagIDs the IDs of the {@code AprilTag}s
112112
* @return a {@code Command} for visiting all the specified {@code AprilTag}s
113113
*/
114-
public static Command visitTagsOptimized(double distanceTolerance, double angleTolerance, Transform2d robotToTarget,
114+
public static Command visitTagsOptimized(double distanceTolerance, double angleTolerance,
115+
double intermediateToleranceRatio, Transform2d robotToTag,
115116
int... tagIDs) {
116-
var l = Arrays.stream(tagIDs).mapToObj(i -> kFieldLayout.getTagPose(i))
117-
.filter(p -> p.isPresent())
118-
.map(p -> p.get())
119-
.map(p -> p.toPose2d().plus(robotToTarget)).toList();
120-
List<Command> commands = new LinkedList<Command>();
121-
DriveCommand previous = null;
122-
for (var p : l) {
123-
boolean last = p == l.get(l.size() - 1);
124-
DriveCommand c = previous == null ? AlignCommand.moveTo(
125-
m_driveSubsystem, m_poseEstimationSubsystem, p, last ? distanceTolerance : 3 * distanceTolerance,
126-
last ? angleTolerance : 3 * angleTolerance)
127-
: AlignCommand.moveTo(
128-
m_driveSubsystem, m_poseEstimationSubsystem, p,
129-
last ? distanceTolerance : 3 * distanceTolerance,
130-
last ? angleTolerance : 3 * angleTolerance, previous);
131-
commands.add(c);
132-
previous = c;
133-
}
134-
return sequence(commands.toArray(new Command[0]));
117+
var poses = PoseEstimationSubsystem.tagPoses(tagIDs);
118+
poses = Arrays.stream(poses).map(p -> p.plus(robotToTag)).toList().toArray(new Pose2d[0]);
119+
return AlignCommand.follow(
120+
m_driveSubsystem, m_poseEstimationSubsystem, distanceTolerance, angleTolerance,
121+
intermediateToleranceRatio, poses);
135122
}
136123

137124
/**
@@ -148,16 +135,17 @@ public static Command visitTagsOptimized(double distanceTolerance, double angleT
148135
* (2nd step)
149136
* @param distanceTolerance the distance error in meters which is tolerable
150137
* @param angleTolerance the angle error in degrees which is tolerable
138+
* @param intermediateToleranceRatio the ratio of distance and angle tolerances
139+
* at intermediate {@code AprilTags} for faster movements
151140
* @return a {@code Command} for moving to the specified {@code AprilTag} in two
152141
* steps.
153142
*/
154143
public static Command moveToTag(int tagID, Transform2d robotToTagReady, Transform2d robotToTag,
155-
double distanceTolerance,
156-
double angleTolerance) {
144+
double distanceTolerance, double angleTolerance, double intermediateToleranceRatio) {
157145
DriveCommand c1 = AlignCommand.moveTo(
158146
m_driveSubsystem, m_poseEstimationSubsystem,
159147
kFieldLayout.getTagPose(tagID).get().toPose2d().plus(robotToTagReady),
160-
distanceTolerance, angleTolerance);
148+
distanceTolerance * intermediateToleranceRatio, angleTolerance * intermediateToleranceRatio);
161149
DriveCommand c2 = AlignCommand.moveTo(
162150
m_driveSubsystem, m_poseEstimationSubsystem,
163151
kFieldLayout.getTagPose(tagID).get().toPose2d().plus(robotToTag),
@@ -166,14 +154,17 @@ public static Command moveToTag(int tagID, Transform2d robotToTagReady, Transfor
166154
c1, c2, AlignCommand.moveTo(
167155
m_driveSubsystem, m_poseEstimationSubsystem,
168156
kFieldLayout.getTagPose(tagID).get().toPose2d().plus(robotToTagReady),
169-
distanceTolerance, angleTolerance, c2));
157+
distanceTolerance * intermediateToleranceRatio, angleTolerance * intermediateToleranceRatio,
158+
c2));
170159
}
171160

172161
/**
173162
* Returns a {@code Command} for visiting all the specified {@code AprilTag}s.
174163
*
175164
* @param distanceTolerance the distance error in meters which is tolerable
176165
* @param angleTolerance the angle error in degrees which is tolerable
166+
* @param intermediateToleranceRatio the ratio of distance and angle tolerances
167+
* at intermediate {@code AprilTags} for faster movements
177168
* @param robotToTagReady the {@code Tranform2d} representing the pose of the
178169
* {@code AprilTag} relative to the robot when the robot is tentatively
179170
* aligned (1st step)
@@ -183,11 +174,15 @@ public static Command moveToTag(int tagID, Transform2d robotToTagReady, Transfor
183174
* @param tagIDs the IDs of the {@code AprilTag}s
184175
* @return a {@code Command} for visiting all the specified {@code AprilTag}s
185176
*/
186-
public static Command visitTags(double distanceTolerance, double angleTolerance, Transform2d robotToTagReady,
177+
public static Command visitTags(double distanceTolerance, double angleTolerance, double intermediateToleranceRatio,
178+
Transform2d robotToTagReady,
187179
Transform2d robotToTag,
188180
int... tagIDs) {
189181
List<Command> commands = Arrays.stream(tagIDs)
190-
.mapToObj(i -> moveToTag(i, robotToTagReady, robotToTag, distanceTolerance, angleTolerance))
182+
.mapToObj(
183+
i -> moveToTag(
184+
i, robotToTagReady, robotToTag, distanceTolerance, angleTolerance,
185+
intermediateToleranceRatio))
191186
.toList();
192187
return sequence(commands.toArray(new Command[0]));
193188
}

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

Lines changed: 4 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -73,24 +73,18 @@ public Robot() {
7373
CommandComposer.visitTags(0.03, 3, transform(1.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
7474
m_autoSelector.addOption(
7575
"Move around the Red Reef (Faster)",
76-
CommandComposer.visitTagsOptimized(0.03, 3, transform(1.5, 0, 180), 11, 6, 7, 8, 9, 10, 11));
76+
CommandComposer.visitTagsOptimized(0.03, 3, 20, transform(1.5, 0, 180), 11, 6, 7, 8, 9, 10, 11));
7777
m_autoSelector.addOption(
7878
"Move around the Blue Reef (Faster)",
79-
CommandComposer.visitTagsOptimized(0.03, 3, transform(1.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
80-
m_autoSelector.addOption(
81-
"Move around the Red Reef (Faster)",
82-
CommandComposer.visitTagsOptimized(0.03, 3, transform(1.5, 0, 180), 11, 6, 7, 8, 9, 10, 11));
83-
m_autoSelector.addOption(
84-
"Move around the Blue Reef (Faster)",
85-
CommandComposer.visitTagsOptimized(0.03, 3, transform(1.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
79+
CommandComposer.visitTagsOptimized(0.03, 3, 20, transform(1.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
8680
m_autoSelector.addOption(
8781
"Move around the Red Reef (Complex)",
8882
CommandComposer.visitTags(
89-
0.03, 3, transform(1.2, 0, 180), transform(0.5, 0, 180), 11, 6, 7, 8, 9, 10, 11));
83+
0.03, 3, 20, transform(1.2, 0, 180), transform(0.5, 0, 180), 11, 6, 7, 8, 9, 10, 11));
9084
m_autoSelector.addOption(
9185
"Move around the Blue Reef (Complex)",
9286
CommandComposer.visitTags(
93-
0.03, 3, transform(1.2, 0, 180), transform(0.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
87+
0.03, 3, 20, transform(1.2, 0, 180), transform(0.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
9488
SmartDashboard.putData(m_autoSelector);
9589

9690
SmartDashboard.putData(m_pdh);

‎src/main/java/frc/robot/commands/AlignCommand.java‎

Lines changed: 38 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,9 @@
11
package frc.robot.commands;
22

3+
import static edu.wpi.first.wpilibj2.command.Commands.*;
4+
5+
import java.util.LinkedList;
6+
import java.util.List;
37
import java.util.function.Supplier;
48

59
import edu.wpi.first.math.geometry.Pose2d;
@@ -103,6 +107,38 @@ public static DriveCommand driveCommand(DriveSubsystem driveSubsystem,
103107
}, distanceTolerance, angleTolerance, previous);
104108
}
105109

110+
/**
111+
* Constructs a {@code Command} for following the specified path.
112+
*
113+
* @param driveSubsystem the {@code DriveSubsystem} to use
114+
* @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
115+
* @param distanceTolerance the distance error in meters which is tolerable
116+
* @param angleTolerance the angle error in degrees which is tolerable
117+
* @param intermediateToleranceRatio the ratio of distance and angle tolerances
118+
* at intermediate {@code Pose2d}s for faster movements
119+
* @param poses the {@code Pose2d}s that constitute the path
120+
* @return a {@code Command} for following the specified path
121+
*/
122+
public static Command follow(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubsystem,
123+
double distanceTolerance, double angleTolerance, double intermediateToleranceRatio, Pose2d[] poses) {
124+
List<Command> commands = new LinkedList<Command>();
125+
DriveCommand previous = null;
126+
for (var p : poses) {
127+
boolean last = p == poses[poses.length - 1];
128+
DriveCommand c = previous == null ? AlignCommand.moveTo(
129+
driveSubsystem, poseEstimationSubsystem, p,
130+
last ? distanceTolerance : intermediateToleranceRatio * distanceTolerance,
131+
last ? angleTolerance : intermediateToleranceRatio * angleTolerance)
132+
: AlignCommand.moveTo(
133+
driveSubsystem, poseEstimationSubsystem, p,
134+
last ? distanceTolerance : intermediateToleranceRatio * distanceTolerance,
135+
last ? angleTolerance : intermediateToleranceRatio * angleTolerance, previous);
136+
commands.add(c);
137+
previous = c;
138+
}
139+
return sequence(commands.toArray(new Command[0]));
140+
}
141+
106142
/**
107143
* Constructs a {@code Command} for moving the robot to the specified
108144
* target.
@@ -130,7 +166,7 @@ public static DriveCommand moveTo(DriveSubsystem driveSubsystem,
130166
* @param targetPose the field-centric {@code Pose2d} of the target
131167
* @param distanceTolerance the distance error in meters which is tolerable
132168
* @param angleTolerance the angle error in degrees which is tolerable
133-
* @param previous the {@code DriveCommand2} right before the new
169+
* @param previous the {@code DriveCommand} right before the new
134170
* {@code Command}
135171
* @return a {@code Commmand} for moving the robot to the specified
136172
* target
@@ -179,7 +215,7 @@ public static DriveCommand moveTo(DriveSubsystem driveSubsystem,
179215
* {@code Command})
180216
* @param distanceTolerance the distance error in meters which is tolerable
181217
* @param angleTolerance the angle error in degrees which is tolerable
182-
* @param previous the {@code DriveCommand2} right before the new
218+
* @param previous the {@code DriveCommand} right before the new
183219
* {@code Command}
184220
* @return a {@code Commmand} for moving the robot to the specified
185221
* target

‎src/main/java/frc/robot/commands/DriveCommand.java‎

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -62,6 +62,16 @@ public class DriveCommand extends Command {
6262
*/
6363
protected Pose2d m_targetPose;
6464

65+
/**
66+
* The distance error in meters which is tolerable.
67+
*/
68+
protected double m_distanceTolerance;
69+
70+
/**
71+
* The angle error in degrees which is tolerable.
72+
*/
73+
protected double m_angleTolerance;
74+
6575
/**
6676
* Constructs a new {@code DriveCommand} whose purpose is to move the
6777
* robot to a certain {@code Pose2d}.
@@ -123,8 +133,6 @@ public DriveCommand(DriveSubsystem driveSubsystem, Supplier<Pose2d> poseSupplier
123133
new TrapezoidProfile.Constraints(Math.toDegrees(kTurnMaxAngularSpeed),
124134
Math.toDegrees(kTurnMaxAcceleration))));
125135
m_controllerYaw.enableContinuousInput(-180, 180);
126-
m_controllerXY.setTolerance(distanceTolerance);
127-
m_controllerYaw.setTolerance(angleTolerance);
128136
m_controllerXY.setGoal(0);
129137
m_controllerYaw.setGoal(0);
130138
}
@@ -157,6 +165,8 @@ public DriveCommand(DriveSubsystem driveSubsystem, Supplier<Pose2d> poseSupplier
157165
m_driveSubsystem = driveSubsystem;
158166
m_poseSupplier = poseSupplier;
159167
m_targetPoseSupplier = targetPoseSupplier;
168+
m_distanceTolerance = distanceTolerance;
169+
m_angleTolerance = angleTolerance;
160170
m_controllerXY = controllerXY;
161171
m_controllerYaw = controllerYaw;
162172
addRequirements(m_driveSubsystem);
@@ -178,6 +188,8 @@ public void initialize() {
178188
}
179189
m_controllerXY.reset(m_targetPose.getTranslation().minus(pose.getTranslation()).getNorm());
180190
m_controllerYaw.reset(m_targetPose.getRotation().minus(pose.getRotation()).getDegrees());
191+
m_controllerXY.setTolerance(m_distanceTolerance);
192+
m_controllerYaw.setTolerance(m_angleTolerance);
181193
}
182194

183195
/**

‎src/main/java/frc/robot/commands/SubsequentDriveCommand.java‎

Lines changed: 2 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -19,16 +19,6 @@
1919
*/
2020
public class SubsequentDriveCommand extends DriveCommand {
2121

22-
/**
23-
* The distance error in meters which is tolerable.
24-
*/
25-
private double m_distanceTolerance;
26-
27-
/**
28-
* The angle error in degrees which is tolerable.
29-
*/
30-
private double m_angleTolerance;
31-
3222
/**
3323
* Constructs a new {@code SubsequentDriveCommand} whose purpose is to move the
3424
* robot to a certain {@code Pose2d}.
@@ -37,7 +27,7 @@ public class SubsequentDriveCommand extends DriveCommand {
3727
* @param targetPose the {@code Pose2d} to which the robot needs to move
3828
* @param distanceTolerance the distance error in meters which is tolerable
3929
* @param angleTolerance the angle error in degrees which is tolerable
40-
* @param previous the {@code DriveCommand2} right before the new
30+
* @param previous the {@code DriveCommand} right before the new
4131
* {@code SubsequentDriveCommand}
4232
*/
4333
public SubsequentDriveCommand(DriveSubsystem driveSubsystem, Pose2d targetPose, double distanceTolerance,
@@ -61,7 +51,7 @@ public SubsequentDriveCommand(DriveSubsystem driveSubsystem, Pose2d targetPose,
6151
* {@code SubsequentDriveCommand})
6252
* @param distanceTolerance the distance error in meters which is tolerable
6353
* @param angleTolerance the angle error in degrees which is tolerable
64-
* @param previous the {@code DriveCommand2} right before the new
54+
* @param previous the {@code DriveCommand} right before the new
6555
* {@code SubsequentDriveCommand}
6656
*/
6757
public SubsequentDriveCommand(DriveSubsystem driveSubsystem, Supplier<Pose2d> poseSupplier,
@@ -70,8 +60,6 @@ public SubsequentDriveCommand(DriveSubsystem driveSubsystem, Supplier<Pose2d> po
7060
double angleTolerance, DriveCommand previous) {
7161
super(driveSubsystem, poseSupplier, targetPoseSupplier, distanceTolerance, angleTolerance,
7262
previous.m_controllerXY, previous.m_controllerYaw);
73-
m_distanceTolerance = distanceTolerance;
74-
m_angleTolerance = angleTolerance;
7563
}
7664

7765
/**

‎src/main/java/frc/robot/subsystems/PoseEstimationSubsystem.java‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
import static frc.robot.Constants.*;
44
import static frc.robot.Constants.DriveConstants.*;
55

6+
import java.util.Arrays;
67
import java.util.HashMap;
78
import java.util.Map;
89
import java.util.Map.Entry;
@@ -522,4 +523,17 @@ public static Rotation2d rotation(double angleInDegrees) {
522523
return Rotation2d.fromDegrees(angleInDegrees);
523524
}
524525

526+
/**
527+
* Returns the {@code Pose2d}s of the specified {@code AprilTag}s.
528+
*
529+
* @param tagIDs the IDs of the {@code AprilTag}s
530+
* @return the {@code Pose2d}s of the specified {@code AprilTag}s
531+
*/
532+
public static Pose2d[] tagPoses(int... tagIDs) {
533+
return Arrays.stream(tagIDs).mapToObj(i -> kFieldLayout.getTagPose(i))
534+
.filter(p -> p.isPresent())
535+
.map(p -> p.get())
536+
.map(p -> p.toPose2d()).toList().toArray(new Pose2d[0]);
537+
}
538+
525539
}

0 commit comments

Comments
 (0)