1
1
package frc .robot .commands .swerve ;
2
2
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
+
3
10
import com .pathplanner .lib .auto .AutoBuilder ;
4
11
import com .pathplanner .lib .path .PathConstraints ;
5
12
import edu .wpi .first .math .geometry .Pose2d ;
6
13
import edu .wpi .first .math .util .Units ;
7
14
import edu .wpi .first .wpilibj2 .command .Command ;
8
15
import frc .robot .Constants .AutoAlignConstants ;
9
16
import frc .robot .subsystems .swerve .SwerveSubsystem ;
17
+ import frc .robot .util .Pose2dSupplier ;
18
+ import java .util .function .BooleanSupplier ;
10
19
11
20
/** Contains functions to create auto-alignment commands. */
12
21
public class AlignCommand {
@@ -38,31 +47,49 @@ public static Command getAlignCommand(Pose2d targetPose, SwerveSubsystem swerveS
38
47
* Creates a PathPlanner align command using PathPlanner's pathfindToPose() and autoBuilder.
39
48
*
40
49
* @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.
42
51
* @return Pathfinding Command that pathfinds and aligns the robot
43
52
*/
44
- public static Command getAmpAlignCommand (SwerveSubsystem swerveSubsystem , Boolean isRed ) {
53
+ public static Command getAmpAlignCommand (SwerveSubsystem swerveSubsystem , BooleanSupplier isRedSup ) {
45
54
46
55
Pose2d targetPose ;
47
56
48
- if (isRed ) {
57
+ if (isRedSup . getAsBoolean () ) {
49
58
targetPose = AutoAlignConstants .RED_AMP_POSE ;
50
59
} else {
51
60
targetPose = AutoAlignConstants .BLUE_AMP_POSE ;
52
61
}
62
+
63
+ return getAlignCommand (targetPose , swerveSubsystem );
64
+ }
53
65
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 ;
63
80
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 );
67
94
}
68
95
}
0 commit comments