Skip to content

Commit

Permalink
Use command factories on Drive.java to control motion planner
Browse files Browse the repository at this point in the history 
Fix autos for proper mirroring
  • Loading branch information
@suryatho
suryatho committed Jan 27, 2024
1 parent 9828672 commit 84d0260
Show file tree
Hide file tree
Showing 9 changed files with 169 additions and 6,577 deletions.
6,478 changes: 9 additions & 6,469 deletions src/main/Test.chor
View file

Large diffs are not rendered by default.

18 changes: 7 additions & 11 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,6 @@
import frc.robot.subsystems.superstructure.shooter.ShooterIO;
import frc.robot.subsystems.superstructure.shooter.ShooterIOSim;
import frc.robot.subsystems.superstructure.shooter.ShooterIOSparkMax;
import frc.robot.util.AllianceFlipUtil;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;

/**
Expand Down Expand Up @@ -167,27 +166,24 @@ public RobotContainer() {
*/
private void configureButtonBindings() {
drive
.getMotionPlanner()
.setDriveInputSpeeds(
.setDriveInput(
() ->
DriveCommands.getDriveInputSpeeds(
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
() -> -controller.getRightX()))
.schedule();
controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive));
controller
.b()
.onTrue(
Commands.runOnce(
() ->
RobotState.getInstance()
.resetPose(
new Pose2d(
robotState.getEstimatedPose().getTranslation(),
AllianceFlipUtil.apply(new Rotation2d()))),
drive)
robotState.resetPose(
new Pose2d(
robotState.getEstimatedPose().getTranslation(), new Rotation2d())))
.ignoringDisable(true));
controller.y().onTrue(Commands.runOnce(() -> RobotState.getInstance().resetPose(new Pose2d())));
controller.y().onTrue(Commands.runOnce(() -> robotState.resetPose(new Pose2d())));
}

/**
Expand Down
36 changes: 16 additions & 20 deletions src/main/java/frc/robot/commands/DriveCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,26 +56,22 @@ public static ChassisSpeeds getDriveInputSpeeds(
AllianceFlipUtil.apply(RobotState.getInstance().getEstimatedPose().getRotation()));
}

public static Command toggleCalculateShotWhileMovingRotation(Drive drive) {
Supplier<Rotation2d> shotRotation =
() -> {
Twist2d fieldVel = RobotState.getInstance().fieldVelocity();
ShotCalculator.ShotData shotData =
ShotCalculator.calculate(
AllianceFlipUtil.apply(
FieldConstants.Speaker.centerSpeakerOpening.getTranslation()),
RobotState.getInstance().getEstimatedPose().getTranslation(),
new Translation2d(fieldVel.dx, fieldVel.dy));
return shotData.goalHeading();
};
private static final Supplier<Rotation2d> shotRotation =
() -> {
Twist2d fieldVel = RobotState.getInstance().fieldVelocity();
ShotCalculator.ShotData shotData =
ShotCalculator.calculate(
AllianceFlipUtil.apply(
FieldConstants.Speaker.centerSpeakerOpening.getTranslation()),
RobotState.getInstance().getEstimatedPose().getTranslation(),
new Translation2d(fieldVel.dx, fieldVel.dy));
return shotData.goalHeading();
};

return Commands.runOnce(
() -> {
if (drive.getMotionPlanner().isHeadingControlled()) {
drive.getMotionPlanner().disableHeadingSupplier();
} else {
drive.getMotionPlanner().setHeadingSupplier(shotRotation);
}
});
public static Command toggleCalculateShotWhileMovingRotation(Drive drive) {
return Commands.either(
drive.disableHeadingCommand(), // turn off
drive.setHeadingCommand(shotRotation), // turn on
drive.getMotionPlanner()::isHeadingControlled);
}
}
18 changes: 10 additions & 8 deletions src/main/java/frc/robot/commands/auto/AutoCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,17 +9,19 @@

public class AutoCommands {

public static boolean inRegion(Supplier<Region> region) {
return region.get().contains(RobotState.getInstance().getEstimatedPose().getTranslation());
}

public static Command waitForRegion(Supplier<Region> region) {
return Commands.waitUntil(
() -> region.get().contains(RobotState.getInstance().getEstimatedPose().getTranslation()));
return Commands.waitUntil(() -> inRegion(region));
}

public static Command intakeWhileInRegion(Intake intake, Region region) {
public static Command intakeWhileInRegion(Intake intake, Supplier<Region> region) {
return Commands.sequence(
waitForRegion(() -> region),
Commands.waitUntil(() -> inRegion(region)),
intake.runCommand(),
Commands.waitUntil(
() -> !region.contains(RobotState.getInstance().getEstimatedPose().getTranslation())),
Commands.waitUntil(() -> !inRegion(region)),
intake.stopCommand());
}

Expand All @@ -28,8 +30,8 @@ public interface Region {
}

public static class RectangularRegion implements Region {
private final Translation2d topLeft;
private final Translation2d bottomRight;
public final Translation2d topLeft;
public final Translation2d bottomRight;

public RectangularRegion(Translation2d topLeft, Translation2d bottomRight) {
this.topLeft = topLeft;
Expand Down
77 changes: 40 additions & 37 deletions src/main/java/frc/robot/commands/auto/TestAutos.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@
import frc.robot.RobotState;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.drive.DriveConstants;
import frc.robot.subsystems.drive.DriveMotionPlanner;
import frc.robot.subsystems.superstructure.ShotCalculator;
import frc.robot.subsystems.superstructure.intake.Intake;
import frc.robot.util.AllianceFlipUtil;
Expand All @@ -27,12 +26,12 @@ public class TestAutos {
"Auto/intakeDistance", DriveConstants.drivetrainConfig.trackwidthX() + 0.1);

// bottom to top
private static AutoCommands.Region spikeIntakeRegion(int i) {
private static AutoCommands.CircularRegion spikeIntakeRegion(int i) {
return new AutoCommands.CircularRegion(
FieldConstants.StagingLocations.spikeTranslations[i], intakeDistance.get());
}

private static AutoCommands.Region centerlineIntakeRegion(int i) {
private static AutoCommands.CircularRegion centerlineIntakeRegion(int i) {
return new AutoCommands.CircularRegion(
FieldConstants.StagingLocations.centerlineTranslations[i], intakeDistance.get());
}
Expand All @@ -41,13 +40,15 @@ private static AutoCommands.Region centerlineIntakeRegion(int i) {

public static Command davisAutoDefensive(Drive drive, Intake intake) {
Trajectory trajectory = getTrajectory("DavisAutoDefensive.traj");
DriveMotionPlanner driveMotionPlanner = drive.getMotionPlanner();

Command sequenceIntake =
Commands.sequence(
AutoCommands.intakeWhileInRegion(intake, spikeIntakeRegion(2)),
AutoCommands.intakeWhileInRegion(intake, centerlineIntakeRegion(4)),
AutoCommands.intakeWhileInRegion(intake, spikeIntakeRegion(1)));
AutoCommands.intakeWhileInRegion(
intake, () -> AllianceFlipUtil.apply(spikeIntakeRegion(2))),
AutoCommands.intakeWhileInRegion(
intake, () -> AllianceFlipUtil.apply(centerlineIntakeRegion(4))),
AutoCommands.intakeWhileInRegion(
intake, () -> AllianceFlipUtil.apply(spikeIntakeRegion(1))));

var secondShot =
new AutoCommands.RectangularRegion(
Expand All @@ -58,30 +59,32 @@ public static Command davisAutoDefensive(Drive drive, Intake intake) {

Command sequenceShots =
Commands.sequence(
moveWhileShooting(driveMotionPlanner),
AutoCommands.waitForRegion(() -> secondShot),
moveWhileShooting(driveMotionPlanner),
AutoCommands.waitForRegion(() -> thirdShot),
moveWhileShooting(driveMotionPlanner),
Commands.waitUntil(() -> !driveMotionPlanner.followingTrajectory()),
moveWhileShooting(driveMotionPlanner));
moveWhileShooting(drive),
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(secondShot)),
moveWhileShooting(drive),
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(thirdShot)),
moveWhileShooting(drive),
Commands.waitUntil(() -> !drive.getMotionPlanner().followingTrajectory()),
moveWhileShooting(drive));

return Commands.sequence(
reset(trajectory.getStartPose()),
resetPoseCommand(trajectory.getStartPose()),
Commands.waitSeconds(1.0), // Initial rev time for first shot
Commands.runOnce(() -> driveMotionPlanner.setTrajectory(trajectory)),
drive.followTrajectoryCommand(trajectory),
Commands.parallel(sequenceIntake, sequenceShots));
}

public static Command davisAuto(Drive drive, Intake intake) {
Trajectory trajectory = getTrajectory("DavisAutoOG.traj");
DriveMotionPlanner driveMotionPlanner = drive.getMotionPlanner();

Command sequenceIntake =
Commands.sequence(
AutoCommands.intakeWhileInRegion(intake, spikeIntakeRegion(2)),
AutoCommands.intakeWhileInRegion(intake, spikeIntakeRegion(1)),
AutoCommands.intakeWhileInRegion(intake, centerlineIntakeRegion(4)));
AutoCommands.intakeWhileInRegion(
intake, () -> AllianceFlipUtil.apply(spikeIntakeRegion(2))),
AutoCommands.intakeWhileInRegion(
intake, () -> AllianceFlipUtil.apply(spikeIntakeRegion(1))),
AutoCommands.intakeWhileInRegion(
intake, () -> AllianceFlipUtil.apply(centerlineIntakeRegion(4))));

var secondShot =
new AutoCommands.RectangularRegion(
Expand All @@ -95,25 +98,29 @@ public static Command davisAuto(Drive drive, Intake intake) {

Command sequenceShots =
Commands.sequence(
AutoCommands.waitForRegion(() -> secondShot),
moveWhileShooting(drive.getMotionPlanner()),
AutoCommands.waitForRegion(() -> thirdShot),
moveWhileShooting(drive.getMotionPlanner()),
AutoCommands.waitForRegion(() -> fourthShot),
moveWhileShooting(drive.getMotionPlanner()));
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(secondShot)),
moveWhileShooting(drive),
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(thirdShot)),
moveWhileShooting(drive),
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(fourthShot)),
moveWhileShooting(drive));

return Commands.sequence(
reset(trajectory.getStartPose()),
resetPoseCommand(trajectory.getStartPose()),
Commands.waitSeconds(1.2), // Preload shot
Commands.runOnce(() -> driveMotionPlanner.setTrajectory(trajectory)), // start trajectory
drive.followTrajectoryCommand(trajectory), // start trajectory
Commands.parallel(sequenceIntake, sequenceShots));
}

private static Command moveWhileShooting(DriveMotionPlanner driveMotionPlanner) {
return Commands.runOnce(() -> driveMotionPlanner.setHeadingSupplier(TestAutos::getShootHeading))
.andThen(
Commands.waitSeconds(0.7) // Estimated shoot time
.andThen(Commands.runOnce(driveMotionPlanner::disableHeadingSupplier)));
private static Command resetPoseCommand(Pose2d pose) {
return Commands.runOnce(() -> robotState.resetPose(AllianceFlipUtil.apply(pose)));
}

private static Command moveWhileShooting(Drive drive) {
return drive
.setHeadingCommand(TestAutos::getShootHeading)
.andThen(Commands.waitSeconds(0.7))
.andThen(drive.disableHeadingCommand());
}

private static Rotation2d getShootHeading() {
Expand All @@ -125,10 +132,6 @@ private static Rotation2d getShootHeading() {
.goalHeading();
}

private static Command reset(Pose2d pose) {
return Commands.runOnce(() -> robotState.resetPose(AllianceFlipUtil.apply(pose)));
}

private static Trajectory getTrajectory(String fileName) {
File trajFile = new File(Filesystem.getDeployDirectory(), "/choreo/" + fileName);
return generate(trajFile).orElseThrow();
Expand Down
31 changes: 30 additions & 1 deletion src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,16 @@
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.*;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.RobotState;
import frc.robot.util.trajectory.Trajectory;
import java.util.*;
import java.util.concurrent.ArrayBlockingQueue;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
import java.util.function.Supplier;
import java.util.stream.IntStream;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
Expand All @@ -36,7 +40,7 @@ public class Drive extends SubsystemBase {
private final GyroIO gyroIO;
private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
private final Module[] modules = new Module[4];
private final DriveMotionPlanner motionPlanner = new DriveMotionPlanner();
private final DriveMotionPlanner motionPlanner;
private Twist2d robotVelocity = new Twist2d();
private Twist2d fieldVelocity = new Twist2d();

Expand All @@ -49,6 +53,8 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br)
modules[1] = new Module(fr, 1);
modules[2] = new Module(bl, 2);
modules[3] = new Module(br, 3);

motionPlanner = new DriveMotionPlanner(DriveConstants.kinematics);
}

public void periodic() {
Expand Down Expand Up @@ -128,6 +134,8 @@ public void periodic() {

// Run robot
if (!characterizing) {
SwerveDriveKinematics.desaturateWheelSpeeds(
setpointStates, DriveConstants.drivetrainConfig.maxLinearVelocity());
for (int i = 0; i < modules.length; i++) {
// Optimize setpoints
optimizedSetpointStates[i] =
Expand Down Expand Up @@ -169,6 +177,27 @@ public double getCharacterizationVelocity() {
return driveVelocityAverage / 4.0;
}

// Command factories for motion planner commands
public Command setDriveInput(Supplier<ChassisSpeeds> driveInput) {
return Commands.runOnce(() -> motionPlanner.setDriveInputSpeeds(driveInput));
}

public Command followTrajectoryCommand(Trajectory trajectory) {
return Commands.runOnce(() -> motionPlanner.setTrajectory(trajectory));
}

public Command setHeadingCommand(Supplier<Rotation2d> heading) {
return Commands.runOnce(() -> motionPlanner.setHeadingSupplier(heading));
}

public Command disableHeadingCommand() {
return Commands.runOnce(motionPlanner::disableHeadingSupplier);
}

public Command orientModules(Rotation2d[] orientations) {
return Commands.runOnce(() -> motionPlanner.modulesOriented(orientations));
}

private SwerveDriveWheelPositions getWheelPositions() {
return new SwerveDriveWheelPositions(
Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new));
Expand Down
Loading

0 comments on commit 84d0260

Please sign in to comment.