Skip to content

Commit

Permalink
Merge pull request #89 from robototes/auto-field-improvements
Browse files Browse the repository at this point in the history 
Auto field improvements
  • Loading branch information
@kirbt
kirbt authored Mar 18, 2024
2 parents 8505150 + 36f0fa5 commit e12a6cc
Show file tree
Hide file tree
Showing 4 changed files with 236 additions and 126 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/team2412/robot/util/AutoLogic.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ public void registerCommands() {
"MidSpeakerCenterLineN5N4N3", AutoPaths.midSpeakerCenterLineN3N2N1);
NamedCommands.registerCommand(
"LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4N3);
NamedCommands.registerCommand("LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4);
NamedCommands.registerCommand("LowSpeakerCenterLineN5N4", AutoPaths.lowSpeakerCenterLineN5N4);
NamedCommands.registerCommand(
"TopSpeakerCenterLineN1N2AutoLine1", AutoPaths.TopSpeakerCenterLineN1N2AutoLine1);
NamedCommands.registerCommand(
Expand Down
225 changes: 131 additions & 94 deletions src/main/java/frc/team2412/robot/util/AutoPaths.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,115 +11,152 @@
import frc.team2412.robot.subsystems.LauncherSubsystem;

public class AutoPaths {
private static Command registerAuto(
String autoName, Command command, String... primaryPathNames) {
PathPlannerAutos.registerAuto(autoName, primaryPathNames);
return command.withName(autoName);
}

// Test Auto

public static SequentialCommandGroup testAuto =
new SequentialCommandGroup(
AutoLogic.getAutoCommand("TestPath"),
new ConditionalCommand(
AutoLogic.getAutoCommand("TestPathTrue"),
AutoLogic.getAutoCommand("TestPathFalse"),
() -> false));
public static Command testAuto =
registerAuto(
"AutoLogicTest",
new SequentialCommandGroup(
AutoLogic.getAutoCommand("TestPath"),
new ConditionalCommand(
AutoLogic.getAutoCommand("TestPathTrue"),
AutoLogic.getAutoCommand("TestPathFalse"),
() -> false)),
"TestPath",
"TestPathTrue");

// Complex Autos

public static Command TopSpeakerCenterLineN1N2AutoLine1 =
Commands.sequence(
SubwooferLaunchCommand(),
getAutoCommand("TopSpeakerQCenterLineN1"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN1LCenterLineN1"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN1QCenterLineN2")),
Commands.sequence(
getAutoCommand("QCenterLineN1QCenterLineN2"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN2LCenterLineN2"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN2LAutoLineN1")),
Commands.sequence(getAutoCommand("QCenterLineN2LAutoLineN1")),
() -> true)),
() -> true),
VisionLaunchCommand());
registerAuto(
"TopSpeakerCenterLineN1N2AutoLine1",
Commands.sequence(
SubwooferLaunchCommand(),
getAutoCommand("TopSpeakerQCenterLineN1"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN1LCenterLineN1"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN1QCenterLineN2")),
Commands.sequence(
getAutoCommand("QCenterLineN1QCenterLineN2"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN2LCenterLineN2"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN2LAutoLineN1")),
Commands.sequence(getAutoCommand("QCenterLineN2LAutoLineN1")),
() -> true)),
() -> true),
VisionLaunchCommand()),
"TopSpeakerQCenterLineN1",
"QCenterLineN1LCenterLineN1",
"LCenterLineN1QCenterLineN2");

public static Command TopSpeakerCenterLineN1N2N3 =
Commands.sequence(
SubwooferLaunchCommand(),
getAutoCommand("TopSpeakerQCenterLineN1"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN1LCenterLineN1"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN1QCenterLineN2")),
Commands.sequence(
getAutoCommand("QCenterLineN1QCenterLineN2"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN2LCenterLineN2"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN2LCenterLineN3")),
Commands.sequence(getAutoCommand("QCenterLineN2LCenterLineN3")),
() -> true)),
() -> true),
VisionLaunchCommand());
registerAuto(
"TopSpeakerCenterLineN1N2N3",
Commands.sequence(
SubwooferLaunchCommand(),
getAutoCommand("TopSpeakerQCenterLineN1"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN1LCenterLineN1"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN1QCenterLineN2")),
Commands.sequence(
getAutoCommand("QCenterLineN1QCenterLineN2"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN2LCenterLineN2"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN2LCenterLineN3")),
Commands.sequence(getAutoCommand("QCenterLineN2LCenterLineN3")),
() -> true)),
() -> true),
VisionLaunchCommand()),
"TopSpeakerQCenterLineN1",
"QCenterLineN1LCenterLineN1",
"LCenterLineN1QCenterLineN2");

public static Command midSpeakerCenterLineN3N2N1 =
Commands.sequence(
SubwooferLaunchCommand(),
getAutoCommand("MidSpeakerQCenterLineN3"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN3LCenterLineN3"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN3QCenterLineN2"),
Commands.either(
Commands.sequence(
getAutoCommand("N3QCenterLineN2LCenterLineN2"),
VisionLaunchCommand(),
getAutoCommand("N3LCenterLineN2LCenterLineN1")),
getAutoCommand("QCenterLineN2LCenterLineN1"),
() -> true)),
Commands.sequence(
getAutoCommand("QCenterLineN3QCenterLineN2"),
getAutoCommand("QCenterLineN2LCenterLineN2")),
() -> true),
VisionLaunchCommand());
registerAuto(
"MidSpeakerCenterLineN5N4N3",
Commands.sequence(
SubwooferLaunchCommand(),
getAutoCommand("MidSpeakerQCenterLineN3"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN3LCenterLineN3"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN3QCenterLineN2"),
Commands.either(
Commands.sequence(
getAutoCommand("N3QCenterLineN2LCenterLineN2"),
VisionLaunchCommand(),
getAutoCommand("N3LCenterLineN2LCenterLineN1")),
getAutoCommand("QCenterLineN2LCenterLineN1"),
() -> true)),
Commands.sequence(
getAutoCommand("QCenterLineN3QCenterLineN2"),
getAutoCommand("QCenterLineN2LCenterLineN2")),
() -> true),
VisionLaunchCommand()),
"MidSpeakerQCenterLineN3",
"QCenterLineN3LCenterLineN3",
"LCenterLineN3QCenterLineN2",
"N3QCenterLineN2LCenterLineN2",
"N3LCenterLineN2LCenterLineN1");

public static Command lowSpeakerCenterLineN5N4 =
Commands.sequence(
SubwooferLaunchCommand(),
getAutoCommand("LowSpeakerQCenterLineN5"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN5LCenterLineN5"),
Commands.waitSeconds(0.5),
getAutoCommand("LCenterLineN5LCenterLineN4")),
Commands.sequence(getAutoCommand("QCenterLineN5LCenterLineN4")),
() -> false));
registerAuto(
"LowSpeakerCenterLineN5N4",
Commands.sequence(
SubwooferLaunchCommand(),
getAutoCommand("LowSpeakerQCenterLineN5"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN5LCenterLineN5"),
Commands.waitSeconds(0.5),
getAutoCommand("LCenterLineN5LCenterLineN4")),
Commands.sequence(getAutoCommand("QCenterLineN5LCenterLineN4")),
() -> false)),
"LowSpeakerQCenterLineN5",
"QCenterLineN5LCenterLineN5",
"LCenterLineN5LCenterLineN4");

public static Command lowSpeakerCenterLineN5N4N3 =
Commands.sequence(
SubwooferLaunchCommand(),
getAutoCommand("LowSpeakerQCenterLineN5"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN5QCenterLineN4"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN4LCenterLineN4"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN4LCenterLineN3")),
Commands.sequence(getAutoCommand("QCenterLineN4LCenterLineN3")),
() -> true)),
Commands.sequence(
getAutoCommand("QCenterLineN5LCenterLineN4"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN4LCenterLineN3")),
() -> false),
VisionLaunchCommand());
registerAuto(
"LowSpeakerCenterLineN5N4N3",
Commands.sequence(
SubwooferLaunchCommand(),
getAutoCommand("LowSpeakerQCenterLineN5"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN5QCenterLineN4"),
Commands.either(
Commands.sequence(
getAutoCommand("QCenterLineN4LCenterLineN4"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN4LCenterLineN3")),
Commands.sequence(getAutoCommand("QCenterLineN4LCenterLineN3")),
() -> true)),
Commands.sequence(
getAutoCommand("QCenterLineN5LCenterLineN4"),
VisionLaunchCommand(),
getAutoCommand("LCenterLineN4LCenterLineN3")),
() -> false),
VisionLaunchCommand()),
"LowSpeakerQCenterLineN5",
"QCenterLineN5QCenterLineN4",
"QCenterLineN4LCenterLineN4",
"LCenterLineN4LCenterLineN3");

// new command getters

Expand Down
30 changes: 27 additions & 3 deletions src/main/java/frc/team2412/robot/util/AutonomousField.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,9 @@
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
Expand Down Expand Up @@ -39,7 +41,19 @@ public static void configureShuffleboardTab(
.subscribe("");
var autonomousField =
new AutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED));
addPeriodic.accept(() -> autonomousField.update(activeAutoSub.get()), UPDATE_RATE);
var watchdog =
new Watchdog(0.001, () -> DriverStation.reportWarning("auto field loop overrun", false));
addPeriodic.accept(
() -> {
watchdog.reset();
autonomousField.update(activeAutoSub.get());
watchdog.addEpoch("AutonomousField.update()");
watchdog.disable();
if (watchdog.isExpired()) {
watchdog.printEpochs();
}
},
UPDATE_RATE);
tab.add("Selected auto", autonomousField.getField())
.withPosition(columnIndex, rowIndex)
.withSize(2, 2);
Expand All @@ -49,6 +63,7 @@ public static void configureShuffleboardTab(
private final Field2d field = new Field2d();

// Keeping track of the current trajectory
private PathPlannerAutos.Auto auto;
private List<PathPlannerTrajectory> trajectories;
private int trajectoryIndex = 0;

Expand Down Expand Up @@ -92,12 +107,16 @@ public Pose2d getUpdatedPose(String autoName) {
double fpgaTime = Timer.getFPGATimestamp();
if (lastName.isEmpty() || !lastName.get().equals(autoName)) {
lastName = Optional.of(autoName);
trajectories = PathPlannerAutos.getAutoTrajectories(autoName);
auto = PathPlannerAutos.getAuto(autoName);
trajectories = auto.trajectories;
trajectoryIndex = 0;
lastFPGATime = fpgaTime;
lastTrajectoryTimeOffset = 0;
}
if (trajectories.isEmpty()) {
if (auto.startingPose != null) {
return auto.startingPose;
}
return new Pose2d();
}
lastTrajectoryTimeOffset += (fpgaTime - lastFPGATime) * speed;
Expand All @@ -115,11 +134,16 @@ public Pose2d getUpdatedPose(String autoName) {
}

/**
* Updates the {@link Field2d} robot pose.
* Updates the {@link Field2d} robot pose. If the robot is enabled, does nothing and the
* trajectory will restart when the robot is disabled.
*
* @param autoName The name of the selected PathPlanner autonomous routine.
*/
public void update(String autoName) {
if (DriverStation.isEnabled()) {
lastName = Optional.empty();
return;
}
field.setRobotPose(getUpdatedPose(autoName));
}
}
Loading

0 comments on commit e12a6cc

Please sign in to comment.