Skip to content

Commit 2fd5b06

Browse files
jhhbrown1jhhbrown1
committed
more sophisticated alginment mechanims added
1 parent d125a1d commit 2fd5b06

File tree

5 files changed

+102
-376
lines changed

5 files changed

+102
-376
lines changed

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

Lines changed: 25 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,6 @@
66
import static frc.robot.Constants.DriveConstants.*;
77
import static frc.robot.subsystems.PoseEstimationSubsystem.*;
88

9-
import java.util.Arrays;
109
import java.util.LinkedList;
1110
import java.util.List;
1211
import java.util.function.Supplier;
@@ -172,36 +171,6 @@ public static Command turnTowardTag(int tagID) {
172171
}).withTimeout(1);
173172
}
174173

175-
/**
176-
* Returns a {@code Command} for aligning the robot to the specified
177-
* {@code AprilTag}.
178-
*
179-
* @param tagID the ID of the {@code AprilTag}
180-
* @param distanceTolerance the distance error in meters which is tolerable
181-
* @param angleTolerance the angle error in degrees which is tolerable
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
188-
*
189-
* @return a {@code Command} for aligning the robot to the specified
190-
* {@code AprilTag}
191-
*/
192-
public static Command alignToTag(int tagID, double distanceTolerance, double angleTolerance,
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());
203-
}
204-
205174
/**
206175
* Returns a {@code Command} for aligning the robot to the specified
207176
* {@code AprilTag}s.
@@ -212,24 +181,37 @@ public static Command alignToTag(int tagID, double distanceTolerance, double ang
212181
* tolerable for intermeidate target {@code Pose2d}s
213182
* @param intermediateAngleToleranceInDegrees the angle error in degrees which
214183
* is tolerable for intermeidate target {@code Pose2d}s
215-
* @param robotToTag the {@code Pose2d}s of the {@code AprilTag} relative to the
216-
* center of the robot when the alignment is completed
217-
* @param robotToTagReady the {@code Pose2d}s of the {@code AprilTag} relative
218-
* to the center of the robot when the alignment ready step is completed
219-
* @param tagID the ID of the {@code AprilTag}
184+
* @param robotToTagTransforms the {@code Pose2d}s of the {@code AprilTag}
185+
* relative to the center of the robot when the robit is aligned to the
186+
* ready and alignment poses
187+
* @param robotToTagBackup the {@code Pose2d} of the {@code AprilTag} relative
188+
* to the center of the robot when the robit is aligned to the backup
189+
* pose
190+
* @param tagIDs the IDs of the {@code AprilTag}s
220191
*
221192
* @return a {@code Command} for aligning the robot to the specified
222193
* {@code AprilTag}s
223194
*/
224195
public static Command alignToTags(double distanceTolerance, double angleTolerance,
225196
double intermediateDistanceTolerance, double intermediateAngleToleranceInDegrees,
226-
List<Transform2d> robotToTagTransforms, int... tagIDs) {
227-
List<Command> l = Arrays.stream(tagIDs).mapToObj(
228-
i -> alignToTag(
229-
i, distanceTolerance, angleTolerance, intermediateDistanceTolerance,
230-
intermediateAngleToleranceInDegrees, robotToTagTransforms))
231-
.toList();
232-
return sequence(l.toArray(new Command[0]));
197+
List<Transform2d> robotToTagTransforms, Transform2d robotToTagBackup, int... tagIDs) {
198+
Pose2d previous = null;
199+
var commands = new LinkedList<Command>();
200+
for (int tagID : tagIDs) {
201+
var tagPose = kFieldLayout.getTagPose(tagID).get().toPose2d();
202+
var l = new LinkedList<Pose2d>();
203+
if (previous != null)
204+
l.add(previous);
205+
for (var r : robotToTagTransforms)
206+
l.add(tagPose.plus(r));
207+
previous = tagPose.plus(robotToTagBackup);
208+
var command = new PathDriveCommand(m_driveSubsystem, distanceTolerance, angleTolerance,
209+
intermediateDistanceTolerance, intermediateAngleToleranceInDegrees, l.stream()
210+
.map(p -> (Supplier<Pose2d>) (() -> m_poseEstimationSubsystem.odometryCentricPose(p)))
211+
.toList());
212+
commands.add(command);
213+
}
214+
return sequence(commands.toArray(new Command[0]));
233215
}
234216

235217
}

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

Lines changed: 3 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -14,27 +14,6 @@ public static final class ControllerConstants {
1414
public static final int kOperatorControllerPort = 1;
1515
public static final double kDeadzone = 0.05;
1616
public static final double kTriggerDeadzone = .05;
17-
18-
public static final class Button {
19-
/** Left middle button */
20-
public static final int kSquare = 1;
21-
/** Bottom button */
22-
public static final int kX = 2;
23-
/** Right middle button */
24-
public static final int kCircle = 3;
25-
/** Top button */
26-
public static final int kTriangle = 4;
27-
public static final int kLeftBumper = 5;
28-
public static final int kRightBumper = 6;
29-
public static final int kLeftTrigger = 7;
30-
public static final int kRightTrigger = 8;
31-
public static final int kShare = 9;
32-
public static final int kOptions = 10;
33-
public static final int kLeftStick = 11;
34-
public static final int kRightStick = 12;
35-
public static final int kPS = 13;
36-
public static final int kTrackpad = 14;
37-
}
3817
}
3918

4019
public static final class DriveConstants {
@@ -57,25 +36,16 @@ public static final class DriveConstants {
5736
public static final double kP = 0.03;
5837
public static final double kI = 0.0;
5938
public static final double kD = 0.1 * kP;
60-
// public static final double kD = 0;
6139
public static final double kS = 0;
62-
// public static final double kV = 0.11;
63-
// public static final double kA = 0.009;
64-
public static final double kV = 0.11 * 1 / 2;
65-
public static final double kA = 0.009 * 1 / 2;
66-
67-
// public static final double kA = 0.0001;
68-
69-
// public static final double kTeleopMaxVoltage = 12;
70-
// public static final double kTeleopMaxTurnVoltage = 7.2;
40+
public static final double kV = 0.11;
41+
public static final double kA = 0.009;
7142

7243
public static final double kTeleopDriveMaxSpeed = 5.0; // 5 meters per second
7344
public static final double kTeleopTurnMaxAngularSpeed = Math.toRadians(360); // 1 rotation per second
7445

7546
public static final double kDriveMaxSpeed = 5.0; // 5 meters per second
7647
public static final double kDriveMinSpeed = 0.4; // 0.4 meters per second
77-
public static final double kTurnMaxAngularSpeed = Math.toRadians(180); // 1/2 rotation per second
78-
public static final double kTurnMinAngularSpeed = Math.toRadians(0); // 0 degrees per second
48+
public static final double kTurnMaxAngularSpeed = Math.toRadians(360); // 1 rotation per second
7949
public static final double kDriveMaxVoltage = 12;
8050

8151
public static final double kDriveGearRatio = 6.12;

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

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

11+
import java.util.Arrays;
1112
import java.util.List;
1213
import java.util.Map;
1314
import java.util.function.DoubleSupplier;
15+
import java.util.function.Supplier;
1416

1517
import org.littletonrobotics.urcl.URCL;
1618
import org.photonvision.PhotonCamera;
1719

1820
import edu.wpi.first.math.geometry.Pose2d;
19-
import edu.wpi.first.math.geometry.Rotation2d;
2021
import edu.wpi.first.math.geometry.Transform2d;
2122
import edu.wpi.first.math.kinematics.ChassisSpeeds;
2223
import edu.wpi.first.math.util.Units;
@@ -31,8 +32,8 @@
3132
import edu.wpi.first.wpilibj2.command.CommandScheduler;
3233
import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
3334
import frc.robot.Constants.ControllerConstants;
34-
import frc.robot.Constants.ControllerConstants.Button;
3535
import frc.robot.commands.DriveCommand;
36+
import frc.robot.commands.PathDriveCommand;
3637
import frc.robot.subsystems.DriveSubsystem;
3738
import frc.robot.subsystems.PhotonCameraSimulator;
3839
import frc.robot.subsystems.PoseEstimationSubsystem;
@@ -87,7 +88,8 @@ public Robot() {
8788
CommandComposer.alignToTags(
8889
distanceTolerance, angleToleranceInDegrees, intermediateDistanceTolerance,
8990
intermediateAngleToleranceInDegrees,
90-
List.of(transform(2.7, 0, 180), transform(1.4, 0, 180), transform(.8, 0, 180)), 7, 6, 1,
91+
List.of(transform(1.5, 0, 180), transform(1.0, 0, 180), transform(.5, 0, 180)),
92+
transform(1.5, 0, 180), 7, 6, 1,
9193
6, 7, 8, 2, 8, 7));
9294
m_testSelector.addOption("Check All Subsystems in Pitt", CommandComposer.testAllSubsystems());
9395
m_testSelector.addOption("Check All Subsystems on Field", CommandComposer.testAllSubsystems());
@@ -123,14 +125,19 @@ public void bindDriveControls() {
123125
() -> -m_driverController.getLeftX(),
124126
() -> m_driverController.getL2Axis() - m_driverController.getR2Axis(),
125127
() -> !m_driverController.getHID().getSquareButton()));
126-
127-
m_driverController.button(Button.kSquare)
128-
.whileTrue(
129-
driveWithAlignmentCommand(
130-
() -> -m_driverController.getLeftY(),
131-
() -> -m_driverController.getLeftX(),
132-
() -> m_driverController.getL2Axis() - m_driverController.getR2Axis(),
133-
new Transform2d(0.5, 0, Rotation2d.fromDegrees(180)), 2, 0.03, 3));
128+
m_driverController.square().whileTrue(
129+
driveWithAlignmentCommand(
130+
() -> -m_driverController.getLeftY(),
131+
() -> -m_driverController.getLeftX(),
132+
() -> m_driverController.getL2Axis() - m_driverController.getR2Axis(), 2, 0.01, 1,
133+
transform(0.5, 0, 180)));
134+
m_driverController.cross().whileTrue(
135+
driveWithAlignmentCommand(
136+
() -> -m_driverController.getLeftY(),
137+
() -> -m_driverController.getLeftX(),
138+
() -> m_driverController.getL2Axis() - m_driverController.getR2Axis(), 2, 0.01, 1, 0.1, 5,
139+
transform(1.0, 0, 180),
140+
transform(0.5, 0, 180)));
134141
}
135142

136143
/**
@@ -143,19 +150,19 @@ public void bindDriveControls() {
143150
* go to the left (+Y direction).
144151
* @param rotation rotation speed supplier. Positive values make the
145152
* robot rotate CCW.
146-
* @param robotToTag the {@code Tranform2d} representing the pose of the
147-
* closest {@code AprilTag} relative to the robot when the robot is
148-
* aligned
149153
* @param distanceThresholdInMeters the maximum distance (in meters) within
150154
* which {@code AprilTag}s are considered
151155
* @param distanceTolerance the distance error in meters which is tolerable
152156
* @param angleToleranceInDegrees the angle error in degrees which is tolerable
157+
* @param robotToTag the {@code Tranform2d} representing the pose of the
158+
* closest {@code AprilTag} relative to the robot when the robot is
159+
* aligned
153160
* @return a {@code Command} to automatically align the robot to the closest tag
154161
* while driving the robot with joystick input
155162
*/
156-
Command driveWithAlignmentCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSpeed,
157-
DoubleSupplier rotation, Transform2d robotToTag, double distanceThresholdInMeters, double distanceTolerance,
158-
double angleToleranceInDegrees) {
163+
Command driveWithAlignmentCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSpeed, DoubleSupplier rotation,
164+
double distanceThresholdInMeters, double distanceTolerance, double angleToleranceInDegrees,
165+
Transform2d robotToTag) {
159166
return new DriveCommand(m_driveSubsystem, distanceTolerance, angleToleranceInDegrees, () -> {
160167
Pose2d closestTagPose = m_poseEstimationSubsystem.closestTagPose(180, distanceThresholdInMeters);
161168
if (closestTagPose == null)
@@ -172,6 +179,48 @@ public ChassisSpeeds chassisSpeeds() {
172179
};
173180
}
174181

182+
/**
183+
* Creates a {@code Command} to automatically align the robot to the closest
184+
* {@code AprilTag} while driving the robot with joystick input.
185+
*
186+
* @param forwardSpeed forward speed supplier. Positive values make the robot
187+
* go forward (+X direction).
188+
* @param strafeSpeed strafe speed supplier. Positive values make the robot
189+
* go to the left (+Y direction).
190+
* @param rotation rotation speed supplier. Positive values make the
191+
* robot rotate CCW.
192+
* @param distanceThresholdInMeters the maximum distance (in meters) within
193+
* which {@code AprilTag}s are considered
194+
* @param distanceTolerance the distance error in meters which is tolerable
195+
* @param angleToleranceInDegrees the angle error in degrees which is tolerable
196+
* @param robotToTags the {@code Tranform2d} representing the pose of the
197+
* closest {@code AprilTag} relative to the robot when the robot is
198+
* aligned
199+
* @return a {@code Command} to automatically align the robot to the closest tag
200+
* while driving the robot with joystick input
201+
*/
202+
Command driveWithAlignmentCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSpeed, DoubleSupplier rotation,
203+
double distanceThresholdInMeters, double distanceTolerance, double angleToleranceInDegrees,
204+
double intermedateDistanceTolerance, double intermediateAngleToleranceInDegrees,
205+
Transform2d... robotToTags) {
206+
var l = Arrays.stream(robotToTags).map(r -> (Supplier<Pose2d>) (() -> {
207+
Pose2d closestTagPose = m_poseEstimationSubsystem.closestTagPose(180, distanceThresholdInMeters);
208+
if (closestTagPose == null)
209+
return m_driveSubsystem.getPose();
210+
return m_poseEstimationSubsystem.odometryCentricPose(closestTagPose.plus(r));
211+
})).toList();
212+
return new PathDriveCommand(m_driveSubsystem, distanceTolerance, angleToleranceInDegrees,
213+
intermedateDistanceTolerance, intermediateAngleToleranceInDegrees, l) {
214+
215+
@Override
216+
public ChassisSpeeds chassisSpeeds() {
217+
ChassisSpeeds speeds = DriveSubsystem.chassisSpeeds(forwardSpeed, strafeSpeed, rotation);
218+
return speeds.plus(super.chassisSpeeds());
219+
}
220+
221+
};
222+
}
223+
175224
@Override
176225
public void robotPeriodic() {
177226
CommandScheduler.getInstance().run();

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

Lines changed: 8 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -146,15 +146,18 @@ public ChassisSpeeds chassisSpeeds() {
146146
double velocityX = 0, velocityY = 0;
147147
double distance = current2target.getNorm();
148148
double speed = Math.abs(m_controllerXY.calculate(distance, 0));
149-
if (speed > 1e-6) {
150-
speed = applyThreshold(speed, kDriveMinSpeed);
149+
if (speed > 1e-6) { // due to implementation of Translation2d#getAngle()
151150
var angle = current2target.getAngle();
152-
velocityX = speed * angle.getCos();
153-
velocityY = speed * angle.getSin();
151+
if (speed > kDriveMinSpeed) {
152+
velocityX = speed * angle.getCos();
153+
velocityY = speed * angle.getSin();
154+
} else {
155+
velocityX = kDriveMinSpeed * angle.getCos();
156+
velocityY = kDriveMinSpeed * angle.getSin();
157+
}
154158
}
155159
double angularVelocityRadiansPerSecond = m_controllerYaw
156160
.calculate(currentPose.getRotation().getRadians(), m_targetPose.getRotation().getRadians());
157-
angularVelocityRadiansPerSecond = applyThreshold(angularVelocityRadiansPerSecond, kTurnMinAngularSpeed);
158161
return new ChassisSpeeds(velocityX, velocityY, angularVelocityRadiansPerSecond);
159162
}
160163

@@ -181,17 +184,4 @@ public boolean isFinished() {
181184
return m_controllerXY.atGoal() && m_controllerYaw.atGoal();
182185
}
183186

184-
/**
185-
* Applies the specified threshold to the specified value.
186-
*
187-
* @param value the value to be thresholded
188-
* @param threshold the threshold limit
189-
* @return the original value if the absolute value of that value is greater or
190-
* equal to the threshold; the threshold with the original value's sign
191-
* otherwise
192-
*/
193-
public static double applyThreshold(double value, double threshold) {
194-
return Math.abs(value) < threshold ? Math.signum(value) * threshold : value;
195-
}
196-
197187
}

0 commit comments

Comments
 (0)