Skip to content

L1 autoalign #106

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
SCool62 merged 5 commits into from
Apr 8, 2025
Merged

L1 autoalign #106

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
f0e1c66
Add target lines
SCool62 Mar 29, 2025
f731a09
Initial testing, nonworking
SCool62 Mar 29, 2025
6bffa4d
Make it mostly work
SCool62 Mar 29, 2025
c89a2a0
Allow driver to move along the line
SCool62 Mar 29, 2025
9738b0a
Merge branch 'main' into l1-autoalign
SCool62 Apr 8, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 35 additions & 1 deletion src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rectangle2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
Expand Down Expand Up @@ -79,6 +80,7 @@
import frc.robot.utils.autoaim.AutoAim;
import frc.robot.utils.autoaim.CageTargets;
import frc.robot.utils.autoaim.CoralTargets;
import frc.robot.utils.autoaim.L1Targets;
import java.util.HashMap;
import java.util.Optional;
import java.util.Set;
Expand Down Expand Up @@ -805,7 +807,7 @@ public Robot() {
driver
.rightBumper()
.or(driver.leftBumper())
.and(() -> superstructure.stateIsCoralAlike())
.and(() -> superstructure.stateIsCoralAlike() && currentTarget != ReefTarget.L1)
.whileTrue(
Commands.parallel(
AutoAim.autoAimWithIntermediatePose(
Expand All @@ -825,6 +827,32 @@ public Robot() {
AutoAim.INITIAL_REEF_KEEPOFF_DISTANCE_METERS, 0.0, Rotation2d.kZero)),
Commands.waitUntil(() -> AutoAim.isInToleranceCoral(swerve.getPose()))
.andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy())));

driver
.rightBumper()
.or(driver.leftBumper())
.and(() -> superstructure.stateIsCoralAlike() && currentTarget == ReefTarget.L1)
.whileTrue(
Commands.parallel(
AutoAim.alignToLine(
swerve,
() ->
modifyJoystick(driver.getLeftY())
* ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(),
() ->
modifyJoystick(driver.getLeftX())
* ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(),
() -> L1Targets.getNearestLine(swerve.getPose())),
Commands.waitUntil(
() ->
AutoAim.isInTolerance(
swerve.getPose(),
new Pose2d(
L1Targets.getNearestLine(swerve.getPose())
.nearest(swerve.getPose().getTranslation()),
L1Targets.getNearestLine(swerve.getPose()).getRotation())))
.andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy())));

driver
.rightBumper()
.and(
Expand Down Expand Up @@ -1093,6 +1121,12 @@ public Robot() {
Stream.of(CageTargets.values())
.map((target) -> target.getLocation())
.toArray(Pose2d[]::new));
if (Robot.ROBOT_TYPE != RobotType.REAL)
Logger.recordOutput(
"AutoAim/Targets/L1",
Stream.of(L1Targets.values())
.map((target) -> L1Targets.getRobotTargetLine(target.line))
.toArray(Rectangle2d[]::new));
if (Robot.ROBOT_TYPE != RobotType.REAL)
Logger.recordOutput(
"AutoAim/Targets/Algae",
Expand Down
88 changes: 88 additions & 0 deletions src/main/java/frc/robot/utils/autoaim/AutoAim.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rectangle2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
Expand Down Expand Up @@ -39,6 +40,8 @@ public class AutoAim {
public static Pose2d RED_PROCESSOR_POS = ChoreoAllianceFlipUtil.flip(BLUE_PROCESSOR_POS);
public static List<Pose2d> PROCESSOR_POSES = List.of(BLUE_PROCESSOR_POS, RED_PROCESSOR_POS);

public static final double L1_TROUGH_WIDTH_METERS = 0.935;

public static final double TRANSLATION_TOLERANCE_METERS = Units.inchesToMeters(2.0);
public static final double ROTATION_TOLERANCE_RADIANS = Units.degreesToRadians(2.0);
public static final double VELOCITY_TOLERANCE_METERSPERSECOND = 0.5;
Expand Down Expand Up @@ -223,6 +226,91 @@ public static Command translateToXCoord(
}));
}

public static Command alignToLine(
SwerveSubsystem swerve,
DoubleSupplier xVel,
DoubleSupplier yVel,
Supplier<Rectangle2d> line) {
final Pose2d cachedTarget[] = {new Pose2d()};
final ProfiledPIDController headingController =
// assume we can accelerate to max in 2/3 of a second
new ProfiledPIDController(
Robot.ROBOT_HARDWARE.swerveConstants.getHeadingVelocityKP(),
0.0,
0.0,
new TrapezoidProfile.Constraints(MAX_ANGULAR_SPEED, MAX_ANGULAR_ACCELERATION));
headingController.enableContinuousInput(-Math.PI, Math.PI);
final ProfiledPIDController vxController =
new ProfiledPIDController(
10.0,
0.01,
0.02,
new TrapezoidProfile.Constraints(MAX_AUTOAIM_SPEED, MAX_AUTOAIM_ACCELERATION));
final ProfiledPIDController vyController =
new ProfiledPIDController(
10.0,
0.01,
0.02,
new TrapezoidProfile.Constraints(MAX_AUTOAIM_SPEED, MAX_AUTOAIM_ACCELERATION));
return Commands.runOnce(
() -> {
cachedTarget[0] =
new Pose2d(
line.get().nearest(swerve.getPose().getTranslation()),
line.get().getRotation());
if (Robot.ROBOT_TYPE != RobotType.REAL)
Logger.recordOutput("AutoAim/Cached Target", cachedTarget[0]);
headingController.reset(swerve.getPose().getRotation().getRadians(), 0.0);
vxController.reset(swerve.getPose().getX(), 0.0);
vyController.reset(swerve.getPose().getY(), 0.0);
})
.andThen(
swerve.driveVelocityFieldRelative(
() -> {
Supplier<Pose2d> target =
() ->
new Pose2d(
line.get().nearest(swerve.getPose().getTranslation()),
line.get().getRotation());
final var diff = swerve.getPose().minus(target.get());
var driverReqSpeedsRobotRelative =
ChassisSpeeds.fromFieldRelativeSpeeds(
xVel.getAsDouble(), yVel.getAsDouble(), 0.0, swerve.getRotation());
driverReqSpeedsRobotRelative.vxMetersPerSecond = 0;
final var speeds =
MathUtil.isNear(0.0, diff.getX(), Units.inchesToMeters(0.25))
&& MathUtil.isNear(0.0, diff.getY(), Units.inchesToMeters(0.25))
&& MathUtil.isNear(0.0, diff.getRotation().getDegrees(), 0.5)
? new ChassisSpeeds()
: new ChassisSpeeds(
vxController.calculate(
swerve.getPose().getX(), target.get().getX())
+ vxController.getSetpoint().velocity,
// Use the inputted y velocity target
vyController.calculate(
swerve.getPose().getY(), target.get().getY())
+ vyController.getSetpoint().velocity,
headingController.calculate(
swerve.getPose().getRotation().getRadians(),
target.get().getRotation().getRadians())
+ headingController.getSetpoint().velocity)
.plus(
ChassisSpeeds.fromRobotRelativeSpeeds(
driverReqSpeedsRobotRelative, swerve.getRotation())
.times(1.2));
if (Robot.ROBOT_TYPE != RobotType.REAL)
Logger.recordOutput(
"AutoAim/Target Pose",
new Pose2d(
vxController.getSetpoint().position,
vyController.getSetpoint().position,
Rotation2d.fromRadians(headingController.getSetpoint().position)));
if (Robot.ROBOT_TYPE != RobotType.REAL)
Logger.recordOutput("AutoAim/Target Speeds", speeds);
return speeds;
}));
}

public static Command approachAlgae(
SwerveSubsystem swerve, Supplier<Pose2d> target, double approachSpeed) {
// This feels like a horrible way of getting around lambda final requirements
Expand Down
100 changes: 100 additions & 0 deletions src/main/java/frc/robot/utils/autoaim/L1Targets.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,100 @@
package frc.robot.utils.autoaim;

import choreo.util.ChoreoAllianceFlipUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rectangle2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import frc.robot.Robot;
import java.util.Arrays;
import java.util.List;

public enum L1Targets {
BLUE_AB(
new Rectangle2d(
new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)),
0.0,
AutoAim.L1_TROUGH_WIDTH_METERS)),
BLUE_CD(
new Rectangle2d(
new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)),
0.0,
AutoAim.L1_TROUGH_WIDTH_METERS)),
BLUE_EF(
new Rectangle2d(
new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)),
0.0,
AutoAim.L1_TROUGH_WIDTH_METERS)),
BLUE_GH(
new Rectangle2d(
new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)),
BLUE_IJ(
new Rectangle2d(
new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)),
BLUE_KL(
new Rectangle2d(
new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)),
0.0,
AutoAim.L1_TROUGH_WIDTH_METERS)),

RED_AB(
new Rectangle2d(
ChoreoAllianceFlipUtil.flip(BLUE_AB.line.getCenter()),
0.0,
AutoAim.L1_TROUGH_WIDTH_METERS)),
RED_CD(
new Rectangle2d(
ChoreoAllianceFlipUtil.flip(BLUE_CD.line.getCenter()),
0.0,
AutoAim.L1_TROUGH_WIDTH_METERS)),
RED_EF(
new Rectangle2d(
ChoreoAllianceFlipUtil.flip(BLUE_EF.line.getCenter()),
0.0,
AutoAim.L1_TROUGH_WIDTH_METERS)),
RED_GH(
new Rectangle2d(
ChoreoAllianceFlipUtil.flip(BLUE_GH.line.getCenter()),
0.0,
AutoAim.L1_TROUGH_WIDTH_METERS)),
RED_IJ(
new Rectangle2d(
ChoreoAllianceFlipUtil.flip(BLUE_IJ.line.getCenter()),
0.0,
AutoAim.L1_TROUGH_WIDTH_METERS)),
RED_KL(
new Rectangle2d(
ChoreoAllianceFlipUtil.flip(BLUE_KL.line.getCenter()),
0.0,
AutoAim.L1_TROUGH_WIDTH_METERS));

public Rectangle2d line;

private L1Targets(Rectangle2d line) {
this.line = line;
}

private static final List<Rectangle2d> transformedLines =
Arrays.stream(values())
.map(
(L1Targets targets) -> {
return L1Targets.getRobotTargetLine(targets.line);
})
.toList();

public static Rectangle2d getRobotTargetLine(Rectangle2d original) {
return original.transformBy(
new Transform2d(
Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2,
0,
Rotation2d.fromDegrees(180.0)));
}

public static Rectangle2d getNearestLine(Pose2d pose) {
// It feels like there should be a better way to do this
return new Rectangle2d(
pose.nearest(transformedLines.stream().map(line -> line.getCenter()).toList()),
0.0,
AutoAim.L1_TROUGH_WIDTH_METERS);
}
}