Skip to content

Commit

Permalink
DavisAuto ready for testing
Browse files Browse the repository at this point in the history 
Added AutoCommands.java
  • Loading branch information
@suryatho
suryatho committed Feb 17, 2024
1 parent 1b5b226 commit afdc715
Show file tree
Hide file tree
Showing 5 changed files with 81 additions and 27 deletions.
2 changes: 1 addition & 1 deletion src/main/java/org/littletonrobotics/frc2024/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
*/
public final class Constants {
public static final double loopPeriodSecs = 0.02;
private static RobotType robotType = RobotType.SIMBOT;
private static RobotType robotType = RobotType.DEVBOT;
public static final boolean tuningMode = true;

public static RobotType getRobot() {
Expand Down
7 changes: 7 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOKrakenFOC;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOSim;
import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
import org.littletonrobotics.frc2024.util.NoteVisualizer;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;

/**
Expand Down Expand Up @@ -173,6 +174,10 @@ public RobotContainer() {
}
superstructure = new Superstructure(arm);

// Configure NoteVisualizer
NoteVisualizer.setRobotPoseSupplier(() -> RobotState.getInstance().getEstimatedPose());
NoteVisualizer.setArmAngleSupplier(arm::getSetpoint);

// Configure autos and buttons
configureAutos();
configureButtonBindings();
Expand All @@ -196,6 +201,8 @@ private void configureAutos() {
flywheels::getCharacterizationVelocity));

autoChooser.addOption("Davis Ethical Auto", autoBuilder.davisEthicalAuto());
autoChooser.addOption("N5_S1_C234", autoBuilder.N5_S1_C234());
autoChooser.addOption("N5_S0_C012", autoBuilder.N5_S0_C012());
}

/**
Expand Down
55 changes: 38 additions & 17 deletions src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels;
import org.littletonrobotics.frc2024.subsystems.rollers.Rollers;
import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure;
import org.littletonrobotics.frc2024.util.NoteVisualizer;

public class AutoBuilder {
private final Drive drive;
Expand All @@ -44,19 +45,20 @@ public Command davisEthicalAuto() {
HolonomicTrajectory driveToCenterline0Trajectory =
new HolonomicTrajectory("davisEthicalAuto_driveToCenterline0");

Timer autoTimer = new Timer();
return sequence(
runOnce(autoTimer::restart),
runOnce(() -> flywheels.setIdleMode(Flywheels.IdleMode.AUTO)),
// Shoot preloaded note
resetPose(driveToPodiumTrajectory),
shoot(drive, superstructure, flywheels, rollers),
print("First shot at " + Timer.getFPGATimestamp()),
runOnce(() -> System.out.printf("First shot at %.2f seconds.", autoTimer.get())),

// Drive to podium note while intaking and shoot
followTrajectory(drive, driveToPodiumTrajectory)
.deadlineWith(
intake(superstructure, rollers)), // TODO: change back to alongWith for real
shoot(drive, superstructure, flywheels, rollers),
print("Second shot at " + Timer.getFPGATimestamp()),
.deadlineWith(intakeIntoShot(drive, superstructure, flywheels, rollers)), // uh oh 👀
NoteVisualizer.shoot(),
runOnce(() -> System.out.printf("Second shot at %.2f seconds.", autoTimer.get())),

// Drive to centerline 2 note making sure to only intake after crossed stage
followTrajectory(drive, driveToCenterline2Trajectory)
Expand All @@ -68,10 +70,10 @@ public Command davisEthicalAuto() {
true),
intake(superstructure, rollers).withTimeout(0.8),
// Wait until we are close enough to shot to start arm aiming
waitUntilXCrossed(FieldConstants.Stage.podiumLeg.getX() + 0.2, false),
waitUntilXCrossed(FieldConstants.Stage.podiumLeg.getX() + 0.5, false),
superstructure.aim())),
shoot(drive, superstructure, flywheels, rollers),
print("Third shot at " + Timer.getFPGATimestamp()),
runOnce(() -> System.out.printf("Third shot at %.2f seconds.", autoTimer.get())),

// Drive back to centerline 1 while intaking
followTrajectory(drive, driveToCenterline1Trajectory)
Expand All @@ -83,7 +85,7 @@ public Command davisEthicalAuto() {
intake(superstructure, rollers).withTimeout(1.0),
superstructure.aim())),
shoot(drive, superstructure, flywheels, rollers),
print("Fourth shot at " + Timer.getFPGATimestamp()),
runOnce(() -> System.out.printf("Fourth shot at %.2f seconds.", autoTimer.get())),

// Drive back to centerline 0 and then shoot
followTrajectory(drive, driveToCenterline0Trajectory)
Expand All @@ -95,19 +97,38 @@ public Command davisEthicalAuto() {
intake(superstructure, rollers).withTimeout(1.0),
superstructure.aim())),
shoot(drive, superstructure, flywheels, rollers),
print("Fifth shot at " + Timer.getFPGATimestamp()),
runOnce(() -> System.out.printf("Fifth shot at %.2f seconds.", autoTimer.get())),
// Revert to teleop idle mode
runOnce(() -> flywheels.setIdleMode(Flywheels.IdleMode.TELEOP)));
}

// public Command N5_S1_C234() {
// return sequence(
// reset("N5-S1-C234_driveToS1"),
// path("N5-S1-C234_driveToS1"),
// path("N5-S1-C234_driveToC2"),
// path("N5-S1-C234_driveToC3"),
// path("N5-S1-C234_driveToC4"));
// }
public Command N5_S1_C234() {
HolonomicTrajectory driveToS1 = new HolonomicTrajectory("N5-S1-C234_driveToS1");
HolonomicTrajectory driveToC2 = new HolonomicTrajectory("N5-S1-C234_driveToC2");
HolonomicTrajectory driveToC3 = new HolonomicTrajectory("N5-S1-C234_driveToC3");
HolonomicTrajectory driveToC4 = new HolonomicTrajectory("N5-S1-C234_driveToC4");

return sequence(
resetPose(driveToS1),
followTrajectory(drive, driveToS1),
followTrajectory(drive, driveToC2),
followTrajectory(drive, driveToC3),
followTrajectory(drive, driveToC4));
}

public Command N5_S0_C012() {
HolonomicTrajectory driveToS0 = new HolonomicTrajectory("N5-S0-C0123_driveToS0");
HolonomicTrajectory driveToC0 = new HolonomicTrajectory("N5-S0-C0123_driveToC0");
HolonomicTrajectory driveToC1 = new HolonomicTrajectory("N5-S0-C0123_driveToC1");
HolonomicTrajectory driveToC2 = new HolonomicTrajectory("N5-S0-C0123_driveToC2");

return sequence(
resetPose(driveToS0),
followTrajectory(drive, driveToS0),
followTrajectory(drive, driveToC0),
followTrajectory(drive, driveToC1),
followTrajectory(drive, driveToC2));
}

// public Command N5_S0_C012() {
// return sequence(
Expand Down
33 changes: 25 additions & 8 deletions src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,10 @@
import static edu.wpi.first.wpilibj2.command.Commands.*;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import java.util.function.Supplier;
import org.littletonrobotics.frc2024.FieldConstants;
import org.littletonrobotics.frc2024.RobotState;
import org.littletonrobotics.frc2024.subsystems.drive.Drive;
Expand All @@ -23,7 +25,7 @@
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;

public class AutoCommands {
private static final LoggedTunableNumber shootTimeoutSecs =
public static final LoggedTunableNumber shootTimeoutSecs =
new LoggedTunableNumber("Auto/ShotTimeoutSecs", 0.5);

/**
Expand All @@ -40,8 +42,8 @@ public static Command resetPose(HolonomicTrajectory trajectory) {

/** Creates a command that follows a trajectory, command ends when the trajectory is finished */
public static Command followTrajectory(Drive drive, HolonomicTrajectory trajectory) {
return startEnd(() -> drive.setTrajectoryGoal(trajectory), drive::clearTrajectoryGoal)
.until(drive::isTrajectoryGoalCompleted);
return startEnd(() -> drive.setTrajectory(trajectory), drive::clearTrajectory)
.until(drive::isTrajectoryCompleted);
}

/**
Expand Down Expand Up @@ -83,18 +85,33 @@ public static Command intake(Superstructure superstructure, Rollers rollers) {
/** Shoots note, ending after rollers have spun */
public static Command shoot(
Drive drive, Superstructure superstructure, Flywheels flywheels, Rollers rollers) {
Supplier<Rotation2d> aimHeadingSupplier =
() -> RobotState.getInstance().getAimingParameters().driveHeading();
return parallel(
// Aim and spin up flywheels
startEnd(drive::setAutoAimGoal, drive::clearAutoAimGoal),
startEnd(() -> drive.setHeadingGoal(aimHeadingSupplier), drive::clearHeadingGoal),
superstructure.aim(),
flywheels.shootCommand())
// End command when ready to shoot and rollers have spun
.raceWith(
Commands.waitUntil(
() ->
drive.isAutoAimGoalCompleted()
&& superstructure.atGoal()
&& flywheels.atGoal())
() -> drive.atHeadingGoal() && superstructure.atGoal() && flywheels.atGoal())
.andThen(rollers.feedShooter().withTimeout(shootTimeoutSecs.get())));
}

/**
* Runs intake and feeder while aiming at speaker essentially shooting immediately after acquiring
* game piece. Command does not end.
*/
public static Command intakeIntoShot(
Drive drive, Superstructure superstructure, Flywheels flywheels, Rollers rollers) {
Supplier<Rotation2d> aimHeadingSupplier =
() -> RobotState.getInstance().getAimingParameters().driveHeading();
return parallel(
// Aim and spin up flywheels
startEnd(() -> drive.setHeadingGoal(aimHeadingSupplier), drive::clearHeadingGoal),
superstructure.aim(),
flywheels.shootCommand(),
rollers.quickFeed());
}
}
11 changes: 10 additions & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,8 @@ public class ArmIOSim implements ArmIO {
private boolean controllerNeedsReset = false;
private boolean closedLoop = true;

private boolean wasNotAuto = true;

public ArmIOSim() {
controller = new PIDController(0.0, 0.0, 0.0);
sim.setState(Math.PI / 2.0, 0.0);
Expand All @@ -47,7 +49,14 @@ public void updateInputs(ArmIOInputs inputs) {
if (DriverStation.isDisabled()) {
controllerNeedsReset = true;
}

// Assume starting at ~80 degrees
if (wasNotAuto && DriverStation.isAutonomousEnabled()) {
sim.setState(0.9 * Math.PI / 2.0, 0.0);
wasNotAuto = false;
}
if (!DriverStation.isAutonomousEnabled()) {
wasNotAuto = true;
}
sim.update(Constants.loopPeriodSecs);

inputs.armPositionRads = sim.getAngleRads() + positionOffset;
Expand Down

0 comments on commit afdc715

Please sign in to comment.