Skip to content

Commit d81099f

Browse files
jhhbrown1jhhbrown1
committed
introduced CommandComposer
1 parent f62e389 commit d81099f

File tree

4 files changed

+259
-120
lines changed

4 files changed

+259
-120
lines changed

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

Lines changed: 195 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,195 @@
1+
package frc.robot;
2+
3+
import static edu.wpi.first.wpilibj2.command.Commands.*;
4+
import static frc.robot.Constants.*;
5+
6+
import java.util.Arrays;
7+
import java.util.LinkedList;
8+
import java.util.List;
9+
10+
import edu.wpi.first.math.geometry.Transform2d;
11+
import edu.wpi.first.wpilibj2.command.Command;
12+
import frc.robot.commands.AlignCommand;
13+
import frc.robot.commands.DriveCommand;
14+
import frc.robot.commands.DriveCommandOld;
15+
import frc.robot.subsystems.DriveSubsystem;
16+
import frc.robot.subsystems.PoseEstimationSubsystem;
17+
18+
public class CommandComposer {
19+
private static DriveSubsystem m_driveSubsystem;
20+
private static PoseEstimationSubsystem m_poseEstimationSubsystem;
21+
22+
public static void setSubsystems(DriveSubsystem driveSubsystem,
23+
PoseEstimationSubsystem poseEstimationSubsystem) {
24+
m_driveSubsystem = driveSubsystem;
25+
m_poseEstimationSubsystem = poseEstimationSubsystem;
26+
}
27+
28+
/**
29+
* Returns a {@code Command} for testing subsystems and {@code Command}s.
30+
*
31+
* @return a {@code Command} for testing subsystems and {@code Command}s
32+
*/
33+
public static Command testSubsystemsAndCommands() {
34+
return sequence(
35+
m_driveSubsystem.testCommand(), // F, B, SL, SR, RL, RR
36+
DriveCommandOld.testCommand(m_driveSubsystem).withTimeout(2),
37+
DriveCommand.testCommand(m_driveSubsystem).withTimeout(2));
38+
}
39+
40+
/**
41+
* Returns a {@code Command} for moving forward and then backward.
42+
*
43+
* @param distanceInFeet the distance in feet
44+
* @param distanceTolerance the distance error in meters which is tolerable
45+
* @param angleTolerance the angle error in degrees which is tolerable
46+
*
47+
* @return a {@code Command} for moving 6 feet forward and then backward.
48+
*/
49+
public static Command moveForwardBackward(double distanceInFeet, double distanceTolerance,
50+
double angleTolerance) {
51+
return sequence(
52+
DriveCommand
53+
.moveForward(m_driveSubsystem, 0.0254 * 12 * distanceInFeet, distanceTolerance, angleTolerance),
54+
DriveCommand
55+
.moveForward(
56+
m_driveSubsystem, -0.0254 * 12 * distanceInFeet, distanceTolerance, angleTolerance));
57+
}
58+
59+
/**
60+
* Returns a {@code Command} for moving forward and then backward using 3 PID
61+
* controllers.
62+
*
63+
* @param distanceInFeet the distance in feet
64+
* @param distanceTolerance the distance error in meters which is tolerable
65+
* @param angleTolerance the angle error in degrees which is tolerable
66+
*
67+
* @return a {@code Command} for moving forward and then backward using 3 PID
68+
* controllers.
69+
*/
70+
public static Command moveForwardBackward3Controllers(double distanceInFeet, double distanceTolerance,
71+
double angleTolerance) {
72+
return sequence(
73+
DriveCommandOld
74+
.moveForward(m_driveSubsystem, 0.0254 * 12 * distanceInFeet, distanceTolerance, angleTolerance),
75+
DriveCommandOld
76+
.moveForward(
77+
m_driveSubsystem, -0.0254 * 12 * distanceInFeet, distanceTolerance, angleTolerance));
78+
}
79+
80+
/**
81+
* Returns a {@code Command} for visiting all the specified {@code AprilTag}s.
82+
*
83+
* @param distanceTolerance the distance error in meters which is tolerable
84+
* @param angleTolerance the angle error in degrees which is tolerable
85+
* @param robotToTag the {@code Tranform2d} representing the pose of the
86+
* {@code AprilTag} relative to the robot when the robot is aligned
87+
* @param tagIDs the IDs of the {@code AprilTag}s
88+
* @return a {@code Command} for visiting all the specified {@code AprilTag}s
89+
*/
90+
public static Command visitTags(double distanceTolerance, double angleTolerance, Transform2d robotToTag,
91+
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))
100+
.toList();
101+
return sequence(commands.toArray(new Command[0]));
102+
}
103+
104+
/**
105+
* Returns a {@code Command} for visiting all the specified {@code AprilTag}s.
106+
*
107+
* @param distanceTolerance the distance error in meters which is tolerable
108+
* @param angleTolerance the angle error in degrees which is tolerable
109+
* @param robotToTag the {@code Tranform2d} representing the pose of the
110+
* {@code AprilTag} relative to the robot when the robot is aligned
111+
* @param tagIDs the IDs of the {@code AprilTag}s
112+
* @return a {@code Command} for visiting all the specified {@code AprilTag}s
113+
*/
114+
public static Command visitTagsOptimized(double distanceTolerance, double angleTolerance, Transform2d robotToTarget,
115+
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]));
135+
}
136+
137+
/**
138+
* Returns a {@code Command} for moving to the specified {@code AprilTag} in two
139+
* steps.
140+
*
141+
* @param tagID the ID of the {@code AprilTag}
142+
*
143+
* @param robotToTagReady the {@code Tranform2d} representing the pose of the
144+
* {@code AprilTag} relative to the robot when the robot is tentatively
145+
* aligned (1st step)
146+
* @param robotToTag the {@code Tranform2d} representing the pose of the
147+
* {@code AprilTag} relative to the robot when the robot is fully aligned
148+
* (2nd step)
149+
* @param distanceTolerance the distance error in meters which is tolerable
150+
* @param angleTolerance the angle error in degrees which is tolerable
151+
* @return a {@code Command} for moving to the specified {@code AprilTag} in two
152+
* steps.
153+
*/
154+
public static Command moveToTag(int tagID, Transform2d robotToTagReady, Transform2d robotToTag,
155+
double distanceTolerance,
156+
double angleTolerance) {
157+
DriveCommand c1 = AlignCommand.moveTo(
158+
m_driveSubsystem, m_poseEstimationSubsystem,
159+
kFieldLayout.getTagPose(tagID).get().toPose2d().plus(robotToTagReady),
160+
distanceTolerance, angleTolerance);
161+
DriveCommand c2 = AlignCommand.moveTo(
162+
m_driveSubsystem, m_poseEstimationSubsystem,
163+
kFieldLayout.getTagPose(tagID).get().toPose2d().plus(robotToTag),
164+
distanceTolerance, angleTolerance, c1);
165+
return sequence(
166+
c1, c2, AlignCommand.moveTo(
167+
m_driveSubsystem, m_poseEstimationSubsystem,
168+
kFieldLayout.getTagPose(tagID).get().toPose2d().plus(robotToTagReady),
169+
distanceTolerance, angleTolerance, c2));
170+
}
171+
172+
/**
173+
* Returns a {@code Command} for visiting all the specified {@code AprilTag}s.
174+
*
175+
* @param distanceTolerance the distance error in meters which is tolerable
176+
* @param angleTolerance the angle error in degrees which is tolerable
177+
* @param robotToTagReady the {@code Tranform2d} representing the pose of the
178+
* {@code AprilTag} relative to the robot when the robot is tentatively
179+
* aligned (1st step)
180+
* @param robotToTag the {@code Tranform2d} representing the pose of the
181+
* {@code AprilTag} relative to the robot when the robot is fully aligned
182+
* (2nd step)
183+
* @param tagIDs the IDs of the {@code AprilTag}s
184+
* @return a {@code Command} for visiting all the specified {@code AprilTag}s
185+
*/
186+
public static Command visitTags(double distanceTolerance, double angleTolerance, Transform2d robotToTagReady,
187+
Transform2d robotToTag,
188+
int... tagIDs) {
189+
List<Command> commands = Arrays.stream(tagIDs)
190+
.mapToObj(i -> moveToTag(i, robotToTagReady, robotToTag, distanceTolerance, angleTolerance))
191+
.toList();
192+
return sequence(commands.toArray(new Command[0]));
193+
}
194+
195+
}

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -71,9 +71,9 @@ public static final class DriveConstants {
7171

7272
// public static final double kDriveMaxSpeed = 3.0; // 3 meters per second
7373
public static final double kDriveMaxSpeed = 5.0; // 5 meters per second
74-
public static final double kDriveMinSpeed = .3; // .3 meters per second // TODO: find a good value
74+
public static final double kDriveMinSpeed = .03; // .3 meters per second // TODO: find a good value
7575
public static final double kTurnMaxAngularSpeed = Math.toRadians(180); // 1/2 rotation per second
76-
public static final double kTurnMinAngularSpeed = Math.toRadians(5); // 5 degrees per second
76+
public static final double kTurnMinAngularSpeed = Math.toRadians(0); // 0 degrees per second
7777
public static final double kDriveMaxVoltage = 12;
7878

7979
public static final double kDriveGearRatio = 6.12;

0 commit comments

Comments
 (0)