Skip to content

Commit

Permalink
Remove unused features
Browse files Browse the repository at this point in the history
superpenguin612 committed Jan 2, 2024
1 parent b6b45dc commit 46cfd4b
Showing 7 changed files with 66 additions and 429 deletions.
99 changes: 36 additions & 63 deletions src/main/java/com/techhounds/houndutil/houndauto/AutoManager.java
Original file line number Diff line number Diff line change
@@ -3,7 +3,6 @@
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Optional;
import java.util.function.Consumer;
import java.util.stream.Collectors;

@@ -31,13 +30,11 @@ public class AutoManager {
private AutoRoutine lastRoutine;
private SendableChooser<AutoRoutine> chooser = new SendableChooser<AutoRoutine>();
private Field2d field = new Field2d();
private Command scheduledCommand;
private Command baseCommand;
private Command currentCommand;
private Consumer<Pose2d> resetOdometryConsumer;
private HashMap<String, Command> eventMap = new HashMap<String, Command>();

private Command currentCommand;
private AutoPath currentAutoPath;

private Timer timer = new Timer();

/**
@@ -46,7 +43,8 @@ public class AutoManager {
* the chooser.
*/
private AutoManager() {
lastRoutine = chooser.getSelected();
// lastRoutine = chooser.getSelected();
chooser.setDefaultOption("None", new AutoRoutine("None", Commands.print("Waiting...")));
setupShuffleboardTab();
}

@@ -104,71 +102,44 @@ public void setResetOdometryConsumer(Consumer<Pose2d> consumer) {
* Create a Shuffleboard tab with the field and chooser objects.
*/
public void setupShuffleboardTab() {
LoggingManager.getInstance().addGroup("Autonomous",
LoggingManager.getInstance().addGroup("autonomous",
new LogGroup(
new DoubleLogItem("autoTimer", timer::get, LogType.NT),
new SendableLogger("field", field),
new SendableLogger("chooser", chooser),
new SendableLogger("generateRoute",
Commands.runOnce(this::generateSelectedRoutine)
.withName("Generate Routine")
.ignoringDisable(true))));
}

public void generateSelectedRoutine() {
AutoRoutine selectedRoutine = getSelectedRoutine();
if (resetOdometryConsumer != null) {
try {
resetOdometryConsumer.accept(selectedRoutine.getInitialPosition());
} catch (Exception e) {
DriverStation.reportError("Error resetting odometry: " + e.getMessage(), true);
}

Optional<AutoPath> autoPathOpt = selectedRoutine.getAutoPath();
if (autoPathOpt.isPresent()) {
currentAutoPath = autoPathOpt.orElseThrow();
try {
currentCommand = selectedRoutine.getCommand(currentAutoPath);
} catch (Exception e) {
DriverStation.reportError(
"[houndauto] Failure creating autonomous command! This is likely due to a configuration error. "
+ e.getMessage(),
true);
return;
}
displayAutoPath(currentAutoPath);
}
}
new SendableLogger("chooser", chooser)));
}

/**
* Updates the Shuffleboard visualization with the new selected auton path. This
* should be put in {@code disabledPeriodic()}.
*/
public void periodicUpdate(boolean ignoreLastTrajCheck) {
if (lastRoutine != getSelectedRoutine()) {
lastRoutine = getSelectedRoutine();
public void periodicUpdate() {
AutoRoutine selectedRoutine = getSelectedRoutine();
if (getSelectedRoutine() != lastRoutine) {
if (resetOdometryConsumer != null) {
resetOdometryConsumer.accept(selectedRoutine.getInitialPose());
}
if (selectedRoutine.getPathPlannerPaths() != null)
displayPaths(selectedRoutine.getPathPlannerPaths());
lastRoutine = selectedRoutine;
}
}

/**
* Display the selected routine's trajectories on the field object.
*/
public void displayAutoPath(AutoPath autoPath) {
if (getSelectedRoutine() != null) {
List<PathPlannerPath> paths = autoPath.getPaths();

ArrayList<Pose2d> poses = new ArrayList<Pose2d>();
for (PathPlannerPath path : paths) {
PathPlannerTrajectory trajectory = new PathPlannerTrajectory(path, new ChassisSpeeds());

poses.addAll(trajectory.getStates().stream()
.map((state) -> new Pose2d(state.positionMeters, state.targetHolonomicRotation))
.collect(Collectors.toList()));
}

field.getObject("Autonomous Routine").setPoses(poses);
public void displayPaths(List<PathPlannerPath> paths) {
ArrayList<Pose2d> poses = new ArrayList<Pose2d>();
for (PathPlannerPath path : paths) {
PathPlannerTrajectory trajectory = path.getTrajectory(new ChassisSpeeds(),
path.getStartingDifferentialPose().getRotation());

poses.addAll(trajectory.getStates().stream()
.map((state) -> new Pose2d(state.positionMeters, state.targetHolonomicRotation))
.collect(Collectors.toList()));
}

field.getObject("trajectory").setPoses(poses);
}

/**
@@ -184,19 +155,21 @@ public void runSelectedRoutine() {
if (this.getSelectedRoutine() == null) {
DriverStation.reportError("[houndauto] An auto routine must be chosen.", false);
} else {

// the line after this makes the autoroutine command a composition, and
// commands that are in a composition cannot be recomposed, which is what this
// would do if auto is run multiple times. this fixes it by removing the
// composition from the scheduler.
CommandScheduler.getInstance().removeComposedCommand(scheduledCommand);
CommandScheduler.getInstance().removeComposedCommand(baseCommand);

baseCommand = getSelectedRoutine().getCommand();

timer.reset();
timer.start();
Command toRun = currentCommand
.beforeStarting(() -> resetOdometryConsumer.accept(getSelectedRoutine().getInitialPosition()));
toRun.schedule();
new Trigger(toRun::isFinished).onTrue(Commands.runOnce(timer::stop).ignoringDisable(true));
currentCommand = baseCommand
.beforeStarting(() -> resetOdometryConsumer.accept(getSelectedRoutine().getInitialPose()))
.withName("auto");
currentCommand.schedule();
new Trigger(currentCommand::isFinished).onTrue(Commands.runOnce(timer::stop).ignoringDisable(true));
}
}

@@ -205,8 +178,8 @@ public void runSelectedRoutine() {
*/
public void endRoutine() {
timer.stop();
if (scheduledCommand != null) {
scheduledCommand.cancel();
if (currentCommand != null) {
currentCommand.cancel();
}
}

58 changes: 0 additions & 58 deletions src/main/java/com/techhounds/houndutil/houndauto/AutoPath.java

This file was deleted.

61 changes: 30 additions & 31 deletions src/main/java/com/techhounds/houndutil/houndauto/AutoRoutine.java
Original file line number Diff line number Diff line change
@@ -1,20 +1,16 @@
package com.techhounds.houndutil.houndauto;

import java.util.List;
import java.util.Optional;
import java.util.function.Function;
import java.util.function.Supplier;
import com.pathplanner.lib.path.PathPlannerPath;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.Command;

public class AutoRoutine {
private String name;
private List<AutoSetting> autoSettings;
private Supplier<AutoPath> autoPathSupplier;
private Supplier<Pose2d> blueInitialPoseSupplier;

private Function<AutoPath, Command> commandGetter;
private Command command;
private List<PathPlannerPath> pathPlannerPaths;
private Pose2d initialPose;

/**
* Initialize an AutoRoutine.
@@ -23,40 +19,43 @@ public class AutoRoutine {
* Shuffleboard
* @param commandSupplier the command to run, containing its trajectories
*/
public AutoRoutine(String name, List<AutoSetting> autoSettings,
Supplier<AutoPath> autoPathSupplier, Supplier<Pose2d> blueInitialPoseSupplier,
Function<AutoPath, Command> commandGetter) {
public AutoRoutine(String name, Command command) {
this.name = name;
this.commandGetter = commandGetter;
this.autoPathSupplier = autoPathSupplier;
this.autoSettings = autoSettings;
this.blueInitialPoseSupplier = blueInitialPoseSupplier;
// this.redInitialPoseSupplier = () ->
// TrajectoryReflector.reflectiveTransformPose(blueInitialPoseSupplier.get(),
// 16.54);
this.command = command;
}

public AutoRoutine(String name, Command command, List<PathPlannerPath> pathPlannerPaths, Pose2d initialPose) {
this.name = name;
this.command = command;
this.pathPlannerPaths = pathPlannerPaths;
this.initialPose = initialPose;
}

public String getName() {
return name;
}

public List<AutoSetting> getAutoSettings() {
return autoSettings;
public Command getCommand() {
return command;
}

public Optional<AutoPath> getAutoPath() {
return autoPathSupplier != null ? Optional.of(autoPathSupplier.get()) : Optional.empty();
public List<PathPlannerPath> getPathPlannerPaths() {
return pathPlannerPaths;
}

public Pose2d getInitialPosition() {
// return (DriverStation.getAlliance() == Alliance.Blue) ?
// blueInitialPoseSupplier.get()
// : redInitialPoseSupplier.get();
// TODO
return blueInitialPoseSupplier.get();
public Pose2d getInitialPose() {
if (pathPlannerPaths != null) {
// return (DriverStation.getAlliance() == Alliance.Blue) ?
// blueInitialPoseSupplier.get()
// : redInitialPoseSupplier.get();
// TODO
if (initialPose == null) {
return pathPlannerPaths.get(0).getPreviewStartingHolonomicPose();
} else {
return initialPose;
}
} else
return new Pose2d();
}

public Command getCommand(AutoPath autoPath) {
return commandGetter.apply(autoPath);
}
}
34 changes: 0 additions & 34 deletions src/main/java/com/techhounds/houndutil/houndauto/AutoSetting.java

This file was deleted.

This file was deleted.

This file was deleted.

This file was deleted.

0 comments on commit 46cfd4b

Please sign in to comment.