Skip to content

Commit a13160c

Browse files
committed
Merge branch 'stage-align'
2 parents d8684a8 + 2abec6d commit a13160c

7 files changed

+130
-34
lines changed

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

+40-1
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
import edu.wpi.first.math.geometry.Quaternion;
99
import edu.wpi.first.math.geometry.Rotation2d;
1010
import edu.wpi.first.math.geometry.Rotation3d;
11+
import edu.wpi.first.math.geometry.Transform2d;
1112
import edu.wpi.first.math.geometry.Transform3d;
1213
import edu.wpi.first.math.geometry.Translation2d;
1314
import edu.wpi.first.math.geometry.Translation3d;
@@ -43,7 +44,7 @@ public static class ElevatorConstants {
4344
public static final double EXTENSION_D = 0;
4445
public static final double EXTENSION_TOLERANCE = 0.008;
4546

46-
public static final double POSITION_CONVERSION_FACTOR = 1 /26.357; // Units.inchesToMeters(30.)/63.5;
47+
public static final double POSITION_CONVERSION_FACTOR = 1 / 26.357; // Units.inchesToMeters(30.)/63.5;
4748
public static final double VELOCITY_CONVERSION_FACTOR = 1;
4849

4950
public static final double DOWN_POWER = -0.001; //the motor power to make the elevator move down slowly
@@ -184,6 +185,44 @@ public static class AutoAlignConstants {
184185
public static final Pose2d RED_AMP_POSE = new Pose2d(Units.inchesToMeters(578.77),
185186
Units.inchesToMeters(323.00 - robotRadius), Rotation2d.fromDegrees(90));
186187

188+
/* Stage poses such that the climb hooks are directly above the chains. Poses are defined in terms of the
189+
* corresponding apriltag's pose transformed by a constant distance to the chain. "Stage Left", in this case,
190+
* refers to the left third of the stage when facing the stage from the driver station. We are aware that this
191+
* naming scheme differs from the theatre convention. */
192+
private static final Transform2d STAGE_TAG_TO_ROBOT = new Transform2d(Units.inchesToMeters(17.0),
193+
Units.inchesToMeters(0),
194+
Rotation2d.fromDegrees(180));
195+
196+
public static final Pose2d BLUE_STAGE_BACK_POSE = new Pose2d(Units.inchesToMeters(209.48),
197+
Units.inchesToMeters(161.62),
198+
Rotation2d.fromDegrees(0))
199+
.transformBy(STAGE_TAG_TO_ROBOT);
200+
201+
public static final Pose2d BLUE_STAGE_LEFT_POSE = new Pose2d(Units.inchesToMeters(182.73),
202+
Units.inchesToMeters(177.10),
203+
Rotation2d.fromDegrees(120))
204+
.transformBy(STAGE_TAG_TO_ROBOT);
205+
206+
public static final Pose2d BLUE_STAGE_RIGHT_POSE = new Pose2d(Units.inchesToMeters(182.73),
207+
Units.inchesToMeters(146.19),
208+
Rotation2d.fromDegrees(240))
209+
.transformBy(STAGE_TAG_TO_ROBOT);
210+
211+
public static final Pose2d RED_STAGE_BACK_POSE = new Pose2d(Units.inchesToMeters(441.74),
212+
Units.inchesToMeters(161.62),
213+
Rotation2d.fromDegrees(180))
214+
.transformBy(STAGE_TAG_TO_ROBOT);
215+
216+
public static final Pose2d RED_STAGE_LEFT_POSE = new Pose2d(Units.inchesToMeters(468.69),
217+
Units.inchesToMeters(146.19),
218+
Rotation2d.fromDegrees(300))
219+
.transformBy(STAGE_TAG_TO_ROBOT);
220+
221+
public static final Pose2d RED_STAGE_RIGHT_POSE = new Pose2d(Units.inchesToMeters(468.69),
222+
Units.inchesToMeters(177.10),
223+
Rotation2d.fromDegrees(60))
224+
.transformBy(STAGE_TAG_TO_ROBOT);
225+
187226
}
188227

189228
/** Constants for auton. */

src/main/java/frc/robot/RobotContainer.java

+27-15
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,6 @@
99
import static frc.robot.Constants.VisionConstants.NOTE_CAMERA;
1010

1111
import java.util.EnumSet;
12-
import java.util.function.BooleanSupplier;
1312

1413
import com.choreo.lib.ChoreoTrajectory;
1514
import com.fasterxml.jackson.databind.util.Named;
@@ -224,11 +223,11 @@ public RobotContainer() {
224223
}
225224

226225
try {
227-
driverCamera = new UsbCamera("fisheye", 0);
228-
driverCamera.setVideoMode(PixelFormat.kMJPEG, 160, 120, 30);
229-
driverCamera.setExposureManual(40);
230-
driverCameraServer = new MjpegServer("m1", 1181);
231-
driverCameraServer.setSource(driverCamera);
226+
driverCamera = new UsbCamera("fisheye", 0);
227+
driverCamera.setVideoMode(PixelFormat.kMJPEG, 160, 120, 30);
228+
driverCamera.setExposureManual(40);
229+
driverCameraServer = new MjpegServer("m1", 1181);
230+
driverCameraServer.setSource(driverCamera);
232231
} catch (Exception e) {
233232
System.out.print(e);
234233
}
@@ -349,14 +348,14 @@ private void configureBindings() {
349348
/* Automatic Amping -- Pressing and holding the button will cause the robot to automatically path find to the
350349
* amp and deposit its note. Releasing the button will stop the robot (and the path finding). */
351350
// TODO: Bring back LED integration.
352-
driveController.getAutoAmp().onTrue(
353-
new AutoAmpSequence(fmsSubsystem,
354-
swerveSubsystem,
355-
elevatorSubsystem,
356-
intakePivotSubsystem,
357-
intakeRollerSubsystem)
358-
359-
.onlyWhile(driveController.getAutoAmp())
351+
driveController.getAutoAmp().whileTrue(
352+
Commands.deferredProxy(() -> new AutoAmpSequence(
353+
fmsSubsystem,
354+
swerveSubsystem,
355+
elevatorSubsystem,
356+
intakePivotSubsystem,
357+
intakeRollerSubsystem
358+
))
360359
);
361360

362361
// DEPRECATED AMP ALIGN
@@ -366,9 +365,22 @@ private void configureBindings() {
366365
// AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()),
367366
// new ConditionalWaitCommand(
368367
// () -> !driveController.getAmpAlign().getAsBoolean()))
369-
// ).andThen(new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1)))
368+
// ).andThen(
369+
// new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1))
370+
// )
370371
// );
371372

373+
/* Stage Align -- Pressing and holding the button will cause the robot to automatically pathfind such that its
374+
* climb hooks will end up directly above the center of the nearest chain. */
375+
driveController.getStageAlignButton().whileTrue(
376+
Commands.deferredProxy(() -> AlignCommand.getStageAlignCommand(
377+
swerveSubsystem,
378+
swerveSubsystem::getRobotPosition,
379+
fmsSubsystem::isRedAlliance
380+
))
381+
);
382+
383+
372384
/* Note align -- Auto-intakes the nearest visible note, leaving left power control to the driver. */
373385
driveController.getNoteAlign().onTrue(
374386
new AutoIntakeSequence(intakeRollerSubsystem, intakePivotSubsystem,

src/main/java/frc/robot/commands/auton/AutoAmpSequence.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ public AutoAmpSequence(FieldManagementSubsystem fms,
3030
/* Prepares the mechanisms for amping while path-finding the robot to its amping position. */
3131
Commands.parallel(
3232
new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem),
33-
AlignCommand.getAmpAlignCommand(swerveSubsystem, fms.isRedAlliance())
33+
AlignCommand.getAmpAlignCommand(swerveSubsystem, fms::isRedAlliance)
3434
),
3535

3636
/* Deposits the note into the amp. */

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

+42-15
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,21 @@
11
package frc.robot.commands.swerve;
22

3+
import static frc.robot.Constants.AutoAlignConstants.BLUE_STAGE_BACK_POSE;
4+
import static frc.robot.Constants.AutoAlignConstants.BLUE_STAGE_LEFT_POSE;
5+
import static frc.robot.Constants.AutoAlignConstants.BLUE_STAGE_RIGHT_POSE;
6+
import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_BACK_POSE;
7+
import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_LEFT_POSE;
8+
import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_RIGHT_POSE;
9+
310
import com.pathplanner.lib.auto.AutoBuilder;
411
import com.pathplanner.lib.path.PathConstraints;
512
import edu.wpi.first.math.geometry.Pose2d;
613
import edu.wpi.first.math.util.Units;
714
import edu.wpi.first.wpilibj2.command.Command;
815
import frc.robot.Constants.AutoAlignConstants;
916
import frc.robot.subsystems.swerve.SwerveSubsystem;
17+
import frc.robot.util.Pose2dSupplier;
18+
import java.util.function.BooleanSupplier;
1019

1120
/** Contains functions to create auto-alignment commands. */
1221
public class AlignCommand {
@@ -38,31 +47,49 @@ public static Command getAlignCommand(Pose2d targetPose, SwerveSubsystem swerveS
3847
* Creates a PathPlanner align command using PathPlanner's pathfindToPose() and autoBuilder.
3948
*
4049
* @param swerveSubsystem The robot swerve subsystem to control
41-
* @param isRed Whether or not we have a red alliance
50+
* @param isRedSup True if the robot is on the red alliance, false if on blue.
4251
* @return Pathfinding Command that pathfinds and aligns the robot
4352
*/
44-
public static Command getAmpAlignCommand(SwerveSubsystem swerveSubsystem, Boolean isRed) {
53+
public static Command getAmpAlignCommand(SwerveSubsystem swerveSubsystem, BooleanSupplier isRedSup) {
4554

4655
Pose2d targetPose;
4756

48-
if (isRed) {
57+
if (isRedSup.getAsBoolean()) {
4958
targetPose = AutoAlignConstants.RED_AMP_POSE;
5059
} else {
5160
targetPose = AutoAlignConstants.BLUE_AMP_POSE;
5261
}
62+
63+
return getAlignCommand(targetPose, swerveSubsystem);
64+
}
5365

54-
Command command = AutoBuilder.pathfindToPose(
55-
targetPose,
56-
new PathConstraints(
57-
2.0, 2.0,
58-
Units.degreesToRadians(720), Units.degreesToRadians(1080)
59-
),
60-
0,
61-
0.0
62-
);
66+
/**
67+
* Generates a holonomic path from the robot to the nearest stage face. The robot should end up with its climb hooks
68+
* aligned directly above the chain.
69+
*
70+
* @param swerveSubsystem The swerve drive to move the robot to its target.
71+
* @param currentPoseSup A function that returns the current pose of the robot.
72+
* @param isRedSup True if the robot is on the red alliance, false if on the blue alliance.
73+
* @return Pathfinding Command to bring the robot to its target position.
74+
*/
75+
public static Command getStageAlignCommand(SwerveSubsystem swerveSubsystem,
76+
Pose2dSupplier currentPoseSup, BooleanSupplier isRedSup) {
77+
Pose2d currentPose = currentPoseSup.getPose2d();
78+
Pose2d targetPose = new Pose2d();
79+
double distance = Double.MAX_VALUE;
6380

64-
command.addRequirements(swerveSubsystem);
65-
66-
return command;
81+
Pose2d[] possibleTargets = isRedSup.getAsBoolean()
82+
? new Pose2d[]{RED_STAGE_BACK_POSE, RED_STAGE_LEFT_POSE, RED_STAGE_RIGHT_POSE}
83+
: new Pose2d[]{BLUE_STAGE_BACK_POSE, BLUE_STAGE_LEFT_POSE, BLUE_STAGE_RIGHT_POSE};
84+
85+
for (Pose2d possibleTarget : possibleTargets) {
86+
double possibleDistance = currentPose.getTranslation().getDistance(possibleTarget.getTranslation());
87+
if (possibleDistance < distance) {
88+
distance = possibleDistance;
89+
targetPose = possibleTarget;
90+
}
91+
}
92+
93+
return getAlignCommand(targetPose, swerveSubsystem);
6794
}
6895
}

src/main/java/frc/robot/controllers/BaseDriveController.java

+3
Original file line numberDiff line numberDiff line change
@@ -88,4 +88,7 @@ public abstract class BaseDriveController {
8888
* Gets the button to aim the shooter at the speaker.
8989
*/
9090
public abstract JoystickButton getShooterAimButton();
91+
92+
/** Returns the button to auto-align to the nearest stage face. */
93+
public abstract JoystickButton getStageAlignButton();
9194
}

src/main/java/frc/robot/controllers/DualJoystickDriveController.java

+7-1
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ public class DualJoystickDriveController extends BaseDriveController {
1515
private final JoystickButton leftMiddleButton = new JoystickButton(leftJoystick, 2);
1616
private final JoystickButton leftTopLeftButton = new JoystickButton(leftJoystick, 3);
1717
private final JoystickButton leftTopRightButton = new JoystickButton(leftJoystick, 4);
18+
private final JoystickButton leftBottomLeftButton = new JoystickButton(leftJoystick, 5);
1819

1920
private final Joystick rightJoystick = new Joystick(1);
2021
private final JoystickButton rightTrigger = new JoystickButton(rightJoystick, 1);
@@ -100,5 +101,10 @@ public Boolean getSwerveAimMode() {
100101
@Override
101102
public JoystickButton getShooterAimButton() {
102103
return leftTopRightButton;
103-
}
104+
}
105+
106+
@Override
107+
public JoystickButton getStageAlignButton() {
108+
return leftBottomLeftButton;
109+
}
104110
}

src/main/java/frc/robot/controllers/XboxDriveController.java

+10-1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,9 @@ public class XboxDriveController extends BaseDriveController {
2121
private final JoystickButton driveLStickButton = new JoystickButton(
2222
driveController, XboxController.Button.kLeftStick.value
2323
);
24+
private final JoystickButton driveRStickButton = new JoystickButton(
25+
driveController, XboxController.Button.kRightStick.value
26+
);
2427

2528
@Override
2629
public double getForwardPower() {
@@ -77,7 +80,13 @@ public Boolean getSwerveAimMode() {
7780
return driveLStickButton.getAsBoolean();
7881
}
7982

80-
public JoystickButton getShooterAimButton(){
83+
@Override
84+
public JoystickButton getShooterAimButton() {
8185
return driveLStickButton;
8286
}
87+
88+
@Override
89+
public JoystickButton getStageAlignButton() {
90+
return driveRStickButton;
91+
}
8392
}

0 commit comments

Comments
 (0)