Skip to content

Commit

Permalink
Made AlignCommand more explanable
Browse files Browse the repository at this point in the history
  • Loading branch information
@jhhbrown1
jhhbrown1 committed Feb 2, 2025
1 parent 9fea1ff commit d74f656
Show file tree
Hide file tree
Showing 4 changed files with 91 additions and 48 deletions.
27 changes: 18 additions & 9 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
import static frc.robot.subsystems.PoseEstimationSubsystem.*;

import java.util.Arrays;
import java.util.List;
import java.util.Map;

import org.littletonrobotics.urcl.URCL;
Expand Down Expand Up @@ -88,41 +89,49 @@ public void bindDriveControls() {
DriveCommand.moveForward(m_driveSubsystem, 1.8, distanceTolerance, angleTolerance),
DriveCommand.moveForward(m_driveSubsystem, -1.8, distanceTolerance, angleTolerance)));

Transform2d robotToTarget = new Transform2d(1, 0, Rotation2d.fromDegrees(180));
m_driverController.button(Button.kTriangle)
.whileTrue(
AlignCommand.moveTo(
m_driveSubsystem, m_poseEstimationSubystem,
kFieldLayout.getTagPose(17).get().toPose2d().plus(robotToTarget),
distanceTolerance, angleTolerance));

double angleOfCoverageInDegrees = 90;
double distanceThresholdInMeters = 4;
m_driverController.button(Button.kTriangle)
m_driverController.button(Button.kLeftBumper)
.whileTrue(
AlignCommand.turnToClosestTag(
m_driveSubsystem, m_poseEstimationSubystem, angleOfCoverageInDegrees,
distanceThresholdInMeters,
distanceTolerance, angleTolerance));

Transform2d robotToTarget = new Transform2d(1.5, 0, Rotation2d.fromDegrees(180));
m_driverController.button(Button.kLeftBumper)
m_driverController.button(Button.kRightBumper)
.whileTrue(
AlignCommand.moveToClosestTag(
m_driveSubsystem, m_poseEstimationSubystem, angleOfCoverageInDegrees,
distanceThresholdInMeters, robotToTarget,
distanceTolerance, angleTolerance));

m_driverController.button(Button.kRightBumper)
m_driverController.button(Button.kLeftTrigger)
.whileTrue(
tourCommand(
m_driveSubsystem, m_poseEstimationSubystem, distanceTolerance, angleTolerance,
robotToTarget, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24));

robotToTarget, 12, 15, 14, 16, 17, 18, 19, 20, 21, 22));
}

Command tourCommand(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubystem,
double distanceTolerance, double angleTolerance, Transform2d robotToTarget, int... tagIDs) {
var s = Arrays.stream(tagIDs).mapToObj(i -> kFieldLayout.getTagPose(i)).filter(p -> p.isPresent())
List<Command> commands = Arrays.stream(tagIDs).mapToObj(i -> kFieldLayout.getTagPose(i))
.filter(p -> p.isPresent())
.map(p -> p.get())
.map(p -> p.toPose2d().plus(robotToTarget)).map(
p -> AlignCommand
.moveTo(
driveSubsystem, poseEstimationSubystem, p, distanceTolerance,
angleTolerance));
return sequence(s.toList().toArray(new Command[0]));
angleTolerance))
.toList();
return sequence(commands.toArray(new Command[0]));
}

@Override
Expand Down
72 changes: 51 additions & 21 deletions src/main/java/frc/robot/commands/AlignCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ public class AlignCommand {
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
* @param targetPoseSupplier a {@code Supplier<Pose2d>} that provides the
* target pose to which the robot should move.
* field-centric {@code Pose2d} to which the robot should move.
* This is used at the commencement of this
* {@code Command} (i.e., when the scheduler
* begins to periodically execute this
Expand All @@ -38,9 +38,36 @@ public static Command driveCommand(DriveSubsystem driveSubsystem, PoseEstimation
Supplier<Pose2d> targetPoseSupplier,
double distanceTolerance,
double angleTolerance) {
return new DriveCommand(driveSubsystem, () -> poseEstimationSubsystem.getEstimatedPose(), targetPoseSupplier,
distanceTolerance,
angleTolerance);
// Alternative 1
// return new DriveCommand(driveSubsystem, () ->
// poseEstimationSubsystem.getEstimatedPose(),
// targetPoseSupplier, distanceTolerance, angleTolerance);

// Alternative 2
// return new DriveCommand2(driveSubsystem, () ->
// poseEstimationSubsystem.getEstimatedPose(),
// targetPoseSupplier, distanceTolerance, angleTolerance);

// Alternative 3
// return new DriveCommand(driveSubsystem, () -> driveSubsystem.getPose(),
// () -> {
// Transform2d t =
// targetPoseSupplier.get().minus(poseEstimationSubsystem.getEstimatedPose());
// return driveSubsystem.getPose().plus(t); // odometry-centric pose of target
// }, distanceTolerance, angleTolerance);

// Alternative 4
// return new DriveCommand2(driveSubsystem, () -> driveSubsystem.getPose(),
// () -> {
// Transform2d t =
// targetPoseSupplier.get().minus(poseEstimationSubsystem.getEstimatedPose());
// return driveSubsystem.getPose().plus(t); // odometry-centric pose of target
// }, distanceTolerance, angleTolerance);
return new DriveCommand2(driveSubsystem, () -> driveSubsystem.getPose(),
() -> {
Transform2d t = targetPoseSupplier.get().minus(poseEstimationSubsystem.getEstimatedPose());
return driveSubsystem.getPose().plus(t); // odometry-centric pose of target
}, distanceTolerance, angleTolerance);
}

/**
Expand All @@ -49,7 +76,7 @@ public static Command driveCommand(DriveSubsystem driveSubsystem, PoseEstimation
*
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
* @param targetPose the {@code Pose2d} of the target
* @param targetPose the field-centric {@code Pose2d} of the target
* @param distanceTolerance the distance error in meters which is tolerable
* @param angleTolerance the angle error in degrees which is tolerable
* @return a {@code Commmand} for moving the robot to the specified
Expand All @@ -68,7 +95,7 @@ public static Command moveTo(DriveSubsystem driveSubsystem,
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
* @param targetPoseSupplier a {@code Supplier<Pose2d>} that provides the
* {@code Pose2d} of the target.
* field-centric {@code Pose2d} of the target.
* This is used at the commencement of the {@code Command} (i.e.,
* when the scheduler begins to periodically execute the
* {@code Command})
Expand All @@ -91,7 +118,7 @@ public static Command moveTo(DriveSubsystem driveSubsystem,
*
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
* @param targetPose the {@code Pose2d} of the target
* @param targetPose the field-centric {@code Pose2d} of the target
* @param distanceTolerance the distance error in meters which is tolerable
* @param angleTolerance the angle error in degrees which is tolerable
* @return a {@code Commmand} for turning the robot to the specified
Expand All @@ -110,7 +137,7 @@ public static Command turnTo(DriveSubsystem driveSubsystem,
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
* @param targetPoseSupplier a {@code Supplier<Pose2d>} that provides the
* {@code Pose2d} of the target.
* field-centric {@code Pose2d} of the target.
* This is used at the commencement of the {@code Command} (i.e.,
* when the scheduler begins to periodically execute the
* {@code Command})
Expand All @@ -125,9 +152,9 @@ public static Command turnTo(DriveSubsystem driveSubsystem,
return driveCommand(
driveSubsystem, poseEstimationSubsystem,
() -> {
var transform = new Transform2d(0, 0,
Transform2d t = new Transform2d(0, 0,
poseEstimationSubsystem.angularDisplacement(targetPoseSupplier.get()));
return poseEstimationSubsystem.getEstimatedPose().plus(transform);
return poseEstimationSubsystem.getEstimatedPose().plus(t);
},
distanceTolerance, angleTolerance);
}
Expand All @@ -139,8 +166,9 @@ public static Command turnTo(DriveSubsystem driveSubsystem,
*
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
* @param targetPosition a {@code Translation2d} (i.e., the position) of the
* target.
* @param targetPosition the field-centric {@code Translation2d} (i.e., the
* position) of the
* target
* @param distanceToTarget the desired distance between the robot and the
* target position
* @param distanceTolerance the distance error in meters which is tolerable
Expand All @@ -164,7 +192,8 @@ public static Command moveToward(DriveSubsystem driveSubsystem,
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
* @param targetPositionSupplier a {@code Supplier<Pose2d>} that provides the
* target position.
* field-centric {@code Translation2d} (i.e., the position) of the
* target.
* This is used at the commencement of the {@code Command} (i.e.,
* when the scheduler begins to periodically execute the
* {@code Command})
Expand All @@ -181,16 +210,16 @@ public static Command moveToward(DriveSubsystem driveSubsystem,
return driveCommand(
driveSubsystem, poseEstimationSubsystem,
() -> {
var transform = poseEstimationSubsystem
Transform2d t = poseEstimationSubsystem
.transformationToward(targetPositionSupplier.get(), distanceToTarget);
return poseEstimationSubsystem.getEstimatedPose().plus(transform);
return poseEstimationSubsystem.getEstimatedPose().plus(t);
},
distanceTolerance, angleTolerance);
}

/**
* Constructs a {@code Command} for turning the robot to the specified
* target.
* Constructs a {@code Command} for turning the robot to the closest
* {@code AprilTag} on the field.
*
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
Expand All @@ -213,17 +242,18 @@ public static Command turnToClosestTag(DriveSubsystem driveSubsystem,
}

/**
* Constructs a {@code Command} for moving the robot to the specified
* target.
* Constructs a {@code Command} for moving the robot to the closest
* {@code AprilTag} on the field.
*
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
* @param angleOfCoverageInDegrees the angular coverage (in degrees) within
* which {@code AprilTag}s are considered (maximum: 180)
* @param distanceThresholdInMeters the maximum distance (in meters) within
* which {@code AprilTag}s are considered
* @param robotToTarget the {@code Pose2d} of the target relative to the
* {@code Pose2d} of the robot for the {@code Command} to finish
* @param robotToTarget the {@code Pose2d} of the closest {@code AprilTag}
* relative to the {@code Pose2d} of the robot for the {@code Command} to
* finish
* @param distanceTolerance the distance error in meters which is tolerable
* @param angleTolerance the angle error in degrees which is tolerable
* @return a {@code Commmand} for moving the robot to the specified
Expand Down
14 changes: 7 additions & 7 deletions src/main/java/frc/robot/commands/DriveCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,8 @@
import frc.robot.subsystems.DriveSubsystem;

/**
* This {@code DriveCommand} aims to maneuver the robot from its current pose to
* a certain target pose.
* This {@code DriveCommand} aims to maneuver the robot to a certain
* {@code Pose2d}.
* It utilizes three {@code ProfiledPIDController}s to precisely control the
* robot in the x, y, and yaw dimensions.
*
Expand All @@ -34,7 +34,7 @@ public class DriveCommand extends Command {
private Supplier<Pose2d> m_poseSupplier;

/**
* The {@code Supplier<Pose2d>} that calculates the target pose to which the
* The {@code Supplier<Pose2d>} that provides the {@code Pose2d} to which the
* robot should move.
* This is used at the commencement of this {@code DriveCommand} (i.e.,
* when the scheduler begins to periodically execute this {@code
Expand Down Expand Up @@ -62,10 +62,10 @@ public class DriveCommand extends Command {

/**
* Constructs a new {@code DriveCommand} whose purpose is to move the
* robot to a certain target pose.
* robot to a certain {@code Pose2d}.
*
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param targetPose the target pose to which the robot needs to move
* @param targetPose the {@code Pose2d} to which the robot needs to move
* @param distanceTolerance the distance error in meters which is tolerable
* @param angleTolerance the angle error in degrees which is tolerable
*/
Expand Down Expand Up @@ -96,13 +96,13 @@ public static DriveCommand moveForward(DriveSubsystem driveSubsystem, double tra

/**
* Constructs a new {@code DriveCommand} whose purpose is to move the
* robot to a certain target pose.
* robot to a certain {@code Pose2d}.
*
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param poseSupplier the {@code Supplier} providing the current {@code Pose2d}
* of the robot
* @param targetPoseSupplier a {@code Supplier<Pose2d>} that provides the
* target pose to which the robot should move.
* {@code Pose2d} to which the robot should move.
* This is used at the commencement of this
* {@code DriveCommand} (i.e., when the scheduler
* begins to periodically execute this
Expand Down
26 changes: 15 additions & 11 deletions src/main/java/frc/robot/commands/DriveCommand2.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,14 +8,14 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.DriveSubsystem;

/**
* This {@code DriveCommand2} aims to maneuver the robot from its current pose
* to
* a certain target pose.
* This {@code DriveCommand2} aims to maneuver the robot to a certain
* {@code Pose2d}.
* It utilizes two {@code ProfiledPIDController}s to precisely control the
* robot in the x, y, and yaw dimensions.
*
Expand All @@ -35,7 +35,7 @@ public class DriveCommand2 extends Command {
private Supplier<Pose2d> m_poseSupplier;

/**
* The {@code Supplier<Pose2d>} that calculates the target pose to which the
* The {@code Supplier<Pose2d>} that provides the {@code Pose2d} to which the
* robot should move.
* This is used at the commencement of this {@code DriveCommand2} (i.e.,
* when the scheduler begins to periodically execute this {@code
Expand All @@ -55,14 +55,17 @@ public class DriveCommand2 extends Command {
*/
private ProfiledPIDController m_controllerYaw;

/**
* The {@code Pose2d} to which the robot should move.
*/
private Pose2d m_targetPose;

/**
* Constructs a new {@code DriveCommand2} whose purpose is to move the
* robot to a certain target pose.
* robot to a certain {@code Pose2d}.
*
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param targetPose the target pose to which the robot needs to move
* @param targetPose the {@code Pose2d} to which the robot needs to move
* @param distanceTolerance the distance error in meters which is tolerable
* @param angleTolerance the angle error in degrees which is tolerable
*/
Expand Down Expand Up @@ -93,13 +96,13 @@ public static DriveCommand2 moveForward(DriveSubsystem driveSubsystem, double tr

/**
* Constructs a new {@code DriveCommand2} whose purpose is to move the
* robot to a certain target pose.
* robot to a certain {@code Pose2d}.
*
* @param driveSubsystem the {@code DriveSubsystem} to use
* @param poseSupplier the {@code Supplier} providing the current {@code Pose2d}
* of the robot
* @param targetPoseSupplier a {@code Supplier<Pose2d>} that provides the
* target pose to which the robot should move.
* {@code Pose2d} to which the robot should move.
* This is used at the commencement of this
* {@code DriveCommand2} (i.e., when the scheduler
* begins to periodically execute this
Expand Down Expand Up @@ -137,8 +140,9 @@ public void initialize() {
try {
m_targetPose = m_targetPoseSupplier.get();
} catch (Exception e) {
e.printStackTrace();
}
var t = m_targetPose.getTranslation().minus(pose.getTranslation());
Translation2d t = m_targetPose.getTranslation().minus(pose.getTranslation());
m_controllerXY.reset(t.getNorm());
m_controllerYaw.reset(pose.getRotation().getDegrees());
m_controllerXY.setGoal(0);
Expand All @@ -152,9 +156,9 @@ public void initialize() {
@Override
public void execute() {
Pose2d pose = m_poseSupplier.get();
var t = m_targetPose.getTranslation().minus(pose.getTranslation());
Translation2d t = m_targetPose.getTranslation().minus(pose.getTranslation());
double speed = m_controllerXY.calculate(t.getNorm());
t = t.times(-speed / t.getNorm());
t = t.times(-speed / t.getNorm()); // translation toward target
double speedX = t.getX();
double speedY = t.getY();
// NEGATION needed if the robot rotates clockwise given positive turnSpeed
Expand Down

0 comments on commit d74f656

Please sign in to comment.