Skip to content

Commit d125a1d

Browse files
jhhbrown1jhhbrown1
committed
parameters are turned for fast and accurate driving
1 parent 4355a1f commit d125a1d

File tree

5 files changed

+85
-66
lines changed

5 files changed

+85
-66
lines changed

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

Lines changed: 33 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
import java.util.Arrays;
1010
import java.util.LinkedList;
1111
import java.util.List;
12+
import java.util.function.Supplier;
1213

1314
import edu.wpi.first.math.geometry.Pose2d;
1415
import edu.wpi.first.math.geometry.Rotation2d;
@@ -129,14 +130,17 @@ distanceTolerance, angleTolerance, pose(0.0, 0, 0)).withTimeout(1),
129130
* describes how quickly the robot to move on the circle
130131
* @param distanceTolerance the distance error in meters which is tolerable
131132
* @param angleTolerance the angle error in degrees which is tolerable
132-
* @param intermediateToleranceRatio the ratio to apply to the distance and
133-
* angle tolerances for intermeidate target {@code Pose2d}s
133+
* @param intermediateDistanceTolerance the distance error in meters which is
134+
* tolerable for intermeidate target {@code Pose2d}s
135+
* @param intermediateAngleToleranceInDegrees the angle error in degrees which
136+
* is tolerable for intermeidate target {@code Pose2d}s
134137
* @param poseCount the number of {@code Pose2d}s on the circle
135138
*
136139
* @return a {@code Command} for moving the robot on a circle
137140
*/
138141
public static Command moveOnOval(double radius, double initialAngularIncrement, double finalAngularIncrement,
139-
double distanceTolerance, double angleTolerance, double intermediateToleranceRatio, int poseCount) {
142+
double distanceTolerance, double angleTolerance, double intermediateDistanceTolerance,
143+
double intermediateAngleToleranceInDegrees, int poseCount) {
140144
Rotation2d angle = Rotation2d.kZero;
141145
var l = new LinkedList<Pose2d>();
142146
for (double i = 0; i < poseCount; i++) {
@@ -146,8 +150,8 @@ public static Command moveOnOval(double radius, double initialAngularIncrement,
146150
double angularVelocity = progress * finalAngularIncrement + (1 - progress) * initialAngularIncrement;
147151
angle = angle.plus(rotation(angularVelocity));
148152
}
149-
return new PathDriveCommand(m_driveSubsystem, distanceTolerance, angleTolerance, intermediateToleranceRatio,
150-
l.toArray(new Pose2d[0]));
153+
return new PathDriveCommand(m_driveSubsystem, distanceTolerance, angleTolerance, intermediateDistanceTolerance,
154+
intermediateAngleToleranceInDegrees, l.toArray(new Pose2d[0]));
151155
}
152156

153157
/**
@@ -175,26 +179,27 @@ public static Command turnTowardTag(int tagID) {
175179
* @param tagID the ID of the {@code AprilTag}
176180
* @param distanceTolerance the distance error in meters which is tolerable
177181
* @param angleTolerance the angle error in degrees which is tolerable
178-
* @param intermediateToleranceRatio the ratio to apply to the distance and
179-
* angle tolerances for intermeidate target {@code Pose2d}s
180-
* @param robotToTag the {@code Pose2d}s of the {@code AprilTag} relative to the
181-
* center of the robot when the alignment is completed
182-
* @param robotToTagReady the {@code Pose2d}s of the {@code AprilTag} relative
183-
* to the center of the robot when the alignment ready step is completed
182+
* @param intermediateDistanceTolerance the distance error in meters which is
183+
* tolerable for intermeidate target {@code Pose2d}s
184+
* @param intermediateAngleToleranceInDegrees the angle error in degrees which
185+
* is tolerable for intermeidate target {@code Pose2d}s
186+
* @param robotToTagTransforms the {@code Pose2d}s of the {@code AprilTag}s
187+
* relative to the center of the robot to complete alignment
184188
*
185189
* @return a {@code Command} for aligning the robot to the specified
186190
* {@code AprilTag}
187191
*/
188192
public static Command alignToTag(int tagID, double distanceTolerance, double angleTolerance,
189-
double intermediateToleranceRatio, Transform2d robotToTag, Transform2d robotToTagReady) {
190-
return new PathDriveCommand(m_driveSubsystem, distanceTolerance, angleTolerance, intermediateToleranceRatio,
191-
List.of(
192-
() -> m_poseEstimationSubsystem
193-
.odometryCentricPose(kFieldLayout.getTagPose(tagID).get().toPose2d())
194-
.plus(robotToTagReady),
195-
() -> m_poseEstimationSubsystem
196-
.odometryCentricPose(kFieldLayout.getTagPose(tagID).get().toPose2d())
197-
.plus(robotToTag)));
193+
double intermediateDistanceTolerance, double intermediateAngleToleranceInDegrees,
194+
List<Transform2d> robotToTagTransforms) {
195+
return new PathDriveCommand(
196+
m_driveSubsystem, distanceTolerance, angleTolerance, intermediateDistanceTolerance,
197+
intermediateAngleToleranceInDegrees, robotToTagTransforms
198+
.stream().map(
199+
r -> (Supplier<Pose2d>) (() -> m_poseEstimationSubsystem
200+
.odometryCentricPose(kFieldLayout.getTagPose(tagID).get().toPose2d())
201+
.plus(r)))
202+
.toList());
198203
}
199204

200205
/**
@@ -203,8 +208,10 @@ public static Command alignToTag(int tagID, double distanceTolerance, double ang
203208
*
204209
* @param distanceTolerance the distance error in meters which is tolerable
205210
* @param angleTolerance the angle error in degrees which is tolerable
206-
* @param intermediateToleranceRatio the ratio to apply to the distance and
207-
* angle tolerances for intermeidate target {@code Pose2d}s
211+
* @param intermediateDistanceTolerance the distance error in meters which is
212+
* tolerable for intermeidate target {@code Pose2d}s
213+
* @param intermediateAngleToleranceInDegrees the angle error in degrees which
214+
* is tolerable for intermeidate target {@code Pose2d}s
208215
* @param robotToTag the {@code Pose2d}s of the {@code AprilTag} relative to the
209216
* center of the robot when the alignment is completed
210217
* @param robotToTagReady the {@code Pose2d}s of the {@code AprilTag} relative
@@ -215,10 +222,12 @@ public static Command alignToTag(int tagID, double distanceTolerance, double ang
215222
* {@code AprilTag}s
216223
*/
217224
public static Command alignToTags(double distanceTolerance, double angleTolerance,
218-
double intermediateToleranceRatio, Transform2d robotToTag, Transform2d robotToTagReady, int... tagIDs) {
225+
double intermediateDistanceTolerance, double intermediateAngleToleranceInDegrees,
226+
List<Transform2d> robotToTagTransforms, int... tagIDs) {
219227
List<Command> l = Arrays.stream(tagIDs).mapToObj(
220228
i -> alignToTag(
221-
i, distanceTolerance, angleTolerance, intermediateToleranceRatio, robotToTag, robotToTagReady))
229+
i, distanceTolerance, angleTolerance, intermediateDistanceTolerance,
230+
intermediateAngleToleranceInDegrees, robotToTagTransforms))
222231
.toList();
223232
return sequence(l.toArray(new Command[0]));
224233
}

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

Lines changed: 11 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -54,9 +54,9 @@ public static final class DriveConstants {
5454

5555
// Make sure these are tuned (can do with SysId)
5656
// public static final double kP = 0.09;
57-
public static final double kP = 0.03; // TODO: find a good value
57+
public static final double kP = 0.03;
5858
public static final double kI = 0.0;
59-
public static final double kD = 0.1 * kP; // TODO: find a good value
59+
public static final double kD = 0.1 * kP;
6060
// public static final double kD = 0;
6161
public static final double kS = 0;
6262
// public static final double kV = 0.11;
@@ -72,10 +72,8 @@ public static final class DriveConstants {
7272
public static final double kTeleopDriveMaxSpeed = 5.0; // 5 meters per second
7373
public static final double kTeleopTurnMaxAngularSpeed = Math.toRadians(360); // 1 rotation per second
7474

75-
// public static final double kDriveMaxSpeed = 5.0; // 5 meters per second //
76-
// TODO: find a good value
77-
public static final double kDriveMaxSpeed = 3.0; // 3 meters per second
78-
public static final double kDriveMinSpeed = 0.0; // 0 meters per second
75+
public static final double kDriveMaxSpeed = 5.0; // 5 meters per second
76+
public static final double kDriveMinSpeed = 0.4; // 0.4 meters per second
7977
public static final double kTurnMaxAngularSpeed = Math.toRadians(180); // 1/2 rotation per second
8078
public static final double kTurnMinAngularSpeed = Math.toRadians(0); // 0 degrees per second
8179
public static final double kDriveMaxVoltage = 12;
@@ -95,27 +93,24 @@ public static final class DriveConstants {
9593

9694
public static final int kEncoderDepth = 4;
9795
public static final int kEncoderMeasurementPeriod = 16;
98-
public static final int kDriveSmartCurrentLimit = 20; // TODO: find a good value
96+
public static final int kDriveSmartCurrentLimit = 20;
9997
public static final int kDrivePeakCurrentLimit = kDriveSmartCurrentLimit + 15;
100-
public static final int kSteerSmartCurrentLimit = 20; // TODO: find a good value
98+
public static final int kSteerSmartCurrentLimit = 20;
10199
public static final int kSteerPeakCurrentLimit = kSteerSmartCurrentLimit + 15;
102100
// The amount of time to go from 0 to full power in seconds
103101
public static final double kRampRate = .1;
104102

105103
// DriveCommand.java Constants
106-
public static final double kDriveP = 5; // TODO: find a good value
104+
public static final double kDriveP = 5;
107105
public static final double kDriveI = 0;
108106
public static final double kDriveD = 0;
109-
public static final double kDriveMaxAcceleration = 1 * kDriveMaxSpeed; // kDriveMaxSpeed in 1 sec
110-
// public static final double kDriveMaxAcceleration = 2 * kDriveMaxSpeed; //
111-
// kDriveMaxSpeed in 1/2 sec
107+
public static final double kDriveMaxAcceleration = 0.75 * kDriveMaxSpeed; // kDriveMaxSpeed in 1.5 sec
112108

113-
public static final double kTurnP = 5; // TODO: find a good value
109+
public static final double kTurnP = 5;
114110
public static final double kTurnI = 0;
115111
public static final double kTurnD = 0;
116-
public static final double kTurnMaxAcceleration = 1 * kTurnMaxAngularSpeed; // kTurnMaxAngularSpeed in 1 sec
117-
// public static final double kTurnMaxAcceleration = 2 * kTurnMaxAngularSpeed;
118-
// // kTurnMaxAngularSpeed in 1/2 sec
112+
public static final double kTurnMaxAcceleration = 0.75 * kTurnMaxAngularSpeed; // kTurnMaxAngularSpeed in 1.5
113+
// sec
119114

120115
}
121116

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

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
import static frc.robot.Constants.RobotConstants.*;
99
import static frc.robot.subsystems.PoseEstimationSubsystem.*;
1010

11+
import java.util.List;
1112
import java.util.Map;
1213
import java.util.function.DoubleSupplier;
1314

@@ -65,8 +66,10 @@ public Robot() {
6566
m_autoSelector.addOption("Test DriveSubsystem", m_driveSubsystem.testCommand());
6667
SmartDashboard.putData("Auto Selector", m_autoSelector);
6768

68-
double distanceTolerance = 0.03;
69-
double angleToleranceInDegrees = 2.0;
69+
double distanceTolerance = 0.01;
70+
double angleToleranceInDegrees = 1.0;
71+
double intermediateDistanceTolerance = 0.16;
72+
double intermediateAngleToleranceInDegrees = 16.0;
7073

7174
m_testSelector
7275
.addOption(
@@ -76,13 +79,16 @@ public Robot() {
7679
.addOption(
7780
"Check PID Constants for Driving (Oval with Max Radius of 1m)",
7881
CommandComposer.moveOnOval(
79-
1, 8, 16, distanceTolerance, angleToleranceInDegrees, 16, 360 / 12 * 3 / 2));
82+
1, 8, 16, distanceTolerance, angleToleranceInDegrees, intermediateDistanceTolerance,
83+
intermediateAngleToleranceInDegrees, 360 / 12 * 3 / 2));
8084
m_testSelector
8185
.addOption(
82-
"Quickly Align to AprilTags 7 and 8",
86+
"Quickly Align to AprilTags 1, 2, 6, 7, and 8",
8387
CommandComposer.alignToTags(
84-
distanceTolerance, angleToleranceInDegrees, 16, transform(0.8, 0, 180),
85-
transform(1.3, 0, 180), 7, 8, 7, 8, 7));
88+
distanceTolerance, angleToleranceInDegrees, intermediateDistanceTolerance,
89+
intermediateAngleToleranceInDegrees,
90+
List.of(transform(2.7, 0, 180), transform(1.4, 0, 180), transform(.8, 0, 180)), 7, 6, 1,
91+
6, 7, 8, 2, 8, 7));
8692
m_testSelector.addOption("Check All Subsystems in Pitt", CommandComposer.testAllSubsystems());
8793
m_testSelector.addOption("Check All Subsystems on Field", CommandComposer.testAllSubsystems());
8894
m_testSelector

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

Lines changed: 27 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -18,10 +18,16 @@
1818
public class PathDriveCommand extends DriveCommand {
1919

2020
/**
21-
* The ratio to apply to the distance and angle tolerances for intermeidate
22-
* target {@code Pose2d}s
21+
* The distance error in meters which is tolerable for intermeidate
22+
* target {@code Pose2d}s.
2323
*/
24-
protected double m_intermediateToleranceRatio;
24+
protected double m_intermediateDistanceTolerance;
25+
26+
/**
27+
* The angle error in radians which is tolerable for intermeidate
28+
* target {@code Pose2d}s.
29+
*/
30+
protected double m_intermediateAngleTolerance;
2531

2632
/**
2733
* Constructs a new {@code PathDriveCommand} whose purpose is to move
@@ -30,15 +36,16 @@ public class PathDriveCommand extends DriveCommand {
3036
* @param driveSubsystem the {@code DriveSubsystem} to use
3137
* @param distanceTolerance the distance error in meters which is tolerable
3238
* @param angleToleranceInDegrees the angle error in degrees which is tolerable
33-
* @param intermediateToleranceRatio the ratio to apply to the distance and
34-
* angle tolerances for intermeidate target {@code Pose2d}s
39+
* @param intermediateDistanceTolerance the distance error in meters which is
40+
* tolerable for intermeidate target {@code Pose2d}s
41+
* @param intermediateAngleToleranceInDegrees the angle error in degrees which
42+
* is tolerable for intermeidate target {@code Pose2d}s
3543
* @param targetPoses {@code Pose2d}s to which the robot should move
3644
*/
37-
public PathDriveCommand(DriveSubsystem driveSubsystem,
38-
double distanceTolerance,
39-
double angleToleranceInDegrees, double intermediateToleranceRatio,
40-
Pose2d... targetPoses) {
41-
this(driveSubsystem, distanceTolerance, angleToleranceInDegrees, intermediateToleranceRatio,
45+
public PathDriveCommand(DriveSubsystem driveSubsystem, double distanceTolerance, double angleToleranceInDegrees,
46+
double intermediateDistanceTolerance, double intermediateAngleToleranceInDegrees, Pose2d... targetPoses) {
47+
this(driveSubsystem, distanceTolerance, angleToleranceInDegrees, intermediateDistanceTolerance,
48+
intermediateAngleToleranceInDegrees,
4249
Arrays.stream(targetPoses).map(p -> (Supplier<Pose2d>) (() -> p)).toList());
4350
}
4451

@@ -49,18 +56,20 @@ public PathDriveCommand(DriveSubsystem driveSubsystem,
4956
* @param driveSubsystem the {@code DriveSubsystem} to use
5057
* @param distanceTolerance the distance error in meters which is tolerable
5158
* @param angleToleranceInDegrees the angle error in degrees which is tolerable
52-
* @param intermediateToleranceRatio the ratio to apply to the distance and
53-
* angle tolerances for intermeidate target {@code Pose2d}s
59+
* @param intermediateDistanceTolerance the distance error in meters which is
60+
* tolerable for intermeidate target {@code Pose2d}s
61+
* @param intermediateAngleToleranceInDegrees the angle error in degrees which
62+
* is tolerable for intermeidate target {@code Pose2d}s
5463
* @param targetPoseSuppliers {@code Supplier<Pose2d>}s that provide the
5564
* {@code Pose2d}s to which the robot should move
5665
*/
57-
public PathDriveCommand(DriveSubsystem driveSubsystem,
58-
double distanceTolerance,
59-
double angleToleranceInDegrees, double intermediateToleranceRatio,
66+
public PathDriveCommand(DriveSubsystem driveSubsystem, double distanceTolerance, double angleToleranceInDegrees,
67+
double intermediateDistanceTolerance, double intermediateAngleToleranceInDegrees,
6068
List<Supplier<Pose2d>> targetPoseSuppliers) {
6169
super(driveSubsystem, distanceTolerance, angleToleranceInDegrees,
6270
new IterativeTargetPoseSupplier(targetPoseSuppliers));
63-
m_intermediateToleranceRatio = intermediateToleranceRatio;
71+
m_intermediateDistanceTolerance = intermediateDistanceTolerance;
72+
m_intermediateAngleTolerance = Math.toRadians(intermediateAngleToleranceInDegrees);
6473
}
6574

6675
/**
@@ -90,8 +99,8 @@ protected void moveToNextTargetPose() {
9099
m_targetPose = pose;
91100
}
92101
if (targetPoseSupplier().hasNext()) {
93-
m_controllerXY.setTolerance(m_intermediateToleranceRatio * m_distanceTolerance);
94-
m_controllerYaw.setTolerance(m_intermediateToleranceRatio * m_angleTolerance);
102+
m_controllerXY.setTolerance(m_intermediateDistanceTolerance);
103+
m_controllerYaw.setTolerance(m_intermediateAngleTolerance);
95104
} else {
96105
m_controllerXY.setTolerance(m_distanceTolerance);
97106
m_controllerYaw.setTolerance(m_angleTolerance);

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22

33
import static frc.robot.Constants.*;
44

5-
import java.util.HashMap;
5+
import java.util.LinkedHashMap;
66
import java.util.Map;
77
import java.util.Map.Entry;
88
import java.util.Optional;
@@ -37,7 +37,7 @@ public class PoseEstimationSubsystem extends SubsystemBase {
3737
* The {@code PhotonCamera}s and {@code PhotonPoseEstimator}s used by this
3838
* {@code PoseEstimationSubsystem}.
3939
*/
40-
private final Map<PhotonCamera, PhotonPoseEstimator> m_cameras = new HashMap<PhotonCamera, PhotonPoseEstimator>();
40+
private final Map<PhotonCamera, PhotonPoseEstimator> m_cameras = new LinkedHashMap<PhotonCamera, PhotonPoseEstimator>();
4141

4242
/**
4343
* The {@code PhotonCamera} used by this {@code PoseEstimationSubsystem}.

0 commit comments

Comments
 (0)