Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Added routine.trajectory() method #912

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Added routine.trajectory() method #912

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8f20160
Added java functionality
Daniel1464 Nov 7, 2024
4e21a72
Added missing docstrings
Daniel1464 Nov 7, 2024
601f774
Fixed circular dependencies and other issues
Daniel1464 Nov 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 13 additions & 2 deletions choreolib/src/main/java/choreo/auto/AutoChooser.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,14 +2,17 @@

package choreo.auto;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringArrayEntry;
import edu.wpi.first.networktables.StringEntry;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.IterativeRobotBase;
import edu.wpi.first.wpilibj2.command.Subsystem;
import java.util.HashMap;
import java.util.Map;
import java.util.Optional;
import java.util.function.Function;

/**
Expand All @@ -29,9 +32,17 @@
*/
public class AutoChooser {
/** A function that generates an {@link AutoRoutine} from an {@link AutoFactory}. */
public static interface AutoRoutineGenerator extends Function<AutoFactory, AutoRoutine> {
public interface AutoRoutineGenerator extends Function<AutoFactory, AutoRoutine> {
AutoFactory VOID_FACTORY = new AutoFactory(
Pose2d::new,
(pose2d, sampleType) -> {},
() -> false,
new Subsystem() {},
new AutoFactory.AutoBindings(),
Optional.empty()
);
/** A generator that returns an auto routine that does nothing */
static final AutoRoutineGenerator NONE = factory -> AutoFactory.VOID_ROUTINE;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why change the way this is done?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

voidRoutine has to be an instance method because AutoRoutine now depends on the AutoFactory.

AutoRoutineGenerator NONE = factory -> VOID_FACTORY.voidRoutine();
}

private static final String NONE_NAME = "Nothing";
Expand Down
149 changes: 37 additions & 112 deletions choreolib/src/main/java/choreo/auto/AutoFactory.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,19 +5,16 @@
import choreo.Choreo;
import choreo.Choreo.TrajectoryCache;
import choreo.Choreo.TrajectoryLogger;
import choreo.trajectory.SwerveSample;
import choreo.trajectory.Trajectory;
import choreo.trajectory.TrajectorySample;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.event.EventLoop;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import java.util.HashMap;
import java.util.List;
import java.util.Optional;
import java.util.function.BiConsumer;
import java.util.function.BooleanSupplier;
Expand Down Expand Up @@ -63,31 +60,30 @@
* </code></pre>
*/
public class AutoFactory {
static final AutoRoutine VOID_ROUTINE =
new AutoRoutine("VOID-ROUTINE") {
private final EventLoop loop = new EventLoop();
private final AutoRoutine voidRoutine = new AutoRoutine("VOID-ROUTINE", this) {
private final EventLoop loop = new EventLoop();

@Override
public Command cmd() {
return Commands.none().withName("VoidAutoRoutine");
}
@Override
public Command cmd() {
return Commands.none().withName("VoidAutoRoutine");
}

@Override
public Command cmd(BooleanSupplier _finishCondition) {
return cmd();
}
@Override
public Command cmd(BooleanSupplier _finishCondition) {
return cmd();
}

@Override
public void poll() {}
@Override
public void poll() {}

@Override
public void reset() {}
@Override
public void reset() {}

@Override
public Trigger running() {
return new Trigger(loop, () -> false);
}
};
@Override
public Trigger running() {
return new Trigger(loop, () -> false);
}
};

/** A class used to bind commands to events in all trajectories created by this factory. */
public static class AutoBindings {
Expand Down Expand Up @@ -125,13 +121,13 @@ HashMap<String, Command> getBindings() {
}
}

private final TrajectoryCache trajectoryCache = new TrajectoryCache();
private final Supplier<Pose2d> poseSupplier;
private final BiConsumer<Pose2d, ? extends TrajectorySample<?>> controller;
private final BooleanSupplier mirrorTrajectory;
private final Subsystem driveSubsystem;
private final AutoBindings bindings = new AutoBindings();
private final Optional<TrajectoryLogger<? extends TrajectorySample<?>>> trajectoryLogger;
final TrajectoryCache trajectoryCache = new TrajectoryCache();
final Supplier<Pose2d> poseSupplier;
final BiConsumer<Pose2d, ? extends TrajectorySample<?>> controller;
final BooleanSupplier mirrorTrajectory;
final Subsystem driveSubsystem;
final AutoBindings bindings = new AutoBindings();
final Optional<TrajectoryLogger<? extends TrajectorySample<?>>> trajectoryLogger;
Comment on lines +124 to +130
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this feels off to me; I don't get the benefit from breaking encapsulation like this

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I could pass them in as dependencies instead of passing in the AutoFactory itself. Overall though, it still doesn't break encapsulation from the library side(and was the only way I could do this)


/**
* Its recommended to use the {@link Choreo#createAutoFactory} to create a new instance of this
Expand Down Expand Up @@ -175,7 +171,7 @@ public AutoRoutine newRoutine(String name) {
clearCache();
}

return new AutoRoutine(name);
return new AutoRoutine(name, this);
}

/**
Expand All @@ -186,106 +182,35 @@ public AutoRoutine newRoutine(String name) {
* @see #newRoutine
*/
public AutoRoutine voidRoutine() {
return VOID_ROUTINE;
}

/**
* Creates a new auto trajectory to be used in an auto routine.
*
* @param trajectoryName The name of the trajectory to use.
* @param routine The {@link AutoRoutine} to register this trajectory under.
* @return A new auto trajectory.
*/
public AutoTrajectory trajectory(String trajectoryName, AutoRoutine routine) {
Optional<? extends Trajectory<?>> optTrajectory =
trajectoryCache.loadTrajectory(trajectoryName);
Trajectory<?> trajectory;
if (optTrajectory.isPresent()) {
trajectory = optTrajectory.get();
} else {
DriverStation.reportError("Could not load trajectory: " + trajectoryName, false);
trajectory = new Trajectory<SwerveSample>(trajectoryName, List.of(), List.of(), List.of());
}
return trajectory(trajectory, routine);
}

/**
* Creates a new auto trajectory to be used in an auto routine.
*
* @param trajectoryName The name of the trajectory to use.
* @param splitIndex The index of the split trajectory to use.
* @param routine The {@link AutoRoutine} to register this trajectory under.
* @return A new auto trajectory.
*/
public AutoTrajectory trajectory(
String trajectoryName, final int splitIndex, AutoRoutine routine) {
Optional<? extends Trajectory<?>> optTrajectory =
trajectoryCache.loadTrajectory(trajectoryName, splitIndex);
Trajectory<?> trajectory;
if (optTrajectory.isPresent()) {
trajectory = optTrajectory.get();
} else {
DriverStation.reportError("Could not load trajectory: " + trajectoryName, false);
trajectory = new Trajectory<SwerveSample>(trajectoryName, List.of(), List.of(), List.of());
}
return trajectory(trajectory, routine);
}

/**
* Creates a new auto trajectory to be used in an auto routine.
*
* @param <SampleType> The type of the trajectory samples.
* @param trajectory The trajectory to use.
* @param routine The {@link AutoRoutine} to register this trajectory under.
* @return A new auto trajectory.
*/
@SuppressWarnings("unchecked")
public <SampleType extends TrajectorySample<SampleType>> AutoTrajectory trajectory(
Trajectory<SampleType> trajectory, AutoRoutine routine) {
// type solidify everything
final Trajectory<SampleType> solidTrajectory = trajectory;
final BiConsumer<Pose2d, SampleType> solidController =
(BiConsumer<Pose2d, SampleType>) this.controller;
final Optional<TrajectoryLogger<SampleType>> solidLogger =
this.trajectoryLogger.map(logger -> (TrajectoryLogger<SampleType>) logger);
return new AutoTrajectory(
trajectory.name(),
solidTrajectory,
poseSupplier,
solidController,
mirrorTrajectory,
solidLogger,
driveSubsystem,
routine,
bindings);
return voidRoutine;
}

/**
* Creates a new auto trajectory command to be used in an auto routine.
*
* <p><b>Important </b>
*
* <p>{@link #trajectoryCommand} and {@link #trajectory} methods should not be mixed in the same
* <p>{@link #trajectoryCommand} and {@link AutoRoutine#trajectory} methods should not be mixed in the same
* auto routine. {@link #trajectoryCommand} is used as an escape hatch for teams that don't need
* the benefits of the {@link #trajectory} method and its {@link Trigger} API. {@link
* the benefits of the {@link AutoRoutine#trajectory} method and its {@link Trigger} API. {@link
* #trajectoryCommand} does not invoke bindings added via calling {@link #bind} or {@link
* AutoBindings} passed into the factory constructor.
*
* @param trajectoryName The name of the trajectory to use.
* @return A new auto trajectory.
*/
public Command trajectoryCommand(String trajectoryName) {
return trajectory(trajectoryName, VOID_ROUTINE).cmd();
return voidRoutine.trajectory(trajectoryName).cmd();
}

/**
* Creates a new auto trajectory command to be used in an auto routine.
*
* <p><b>Important </b>
*
* <p>{@link #trajectoryCommand} and {@link #trajectory} methods should not be mixed in the same
* <p>{@link #trajectoryCommand} and {@link AutoRoutine#trajectory} methods should not be mixed in the same
* auto routine. {@link #trajectoryCommand} is used as an escape hatch for teams that don't need
* the benefits of the {@link #trajectory} method and its {@link Trigger} API. {@link
* the benefits of the {@link AutoRoutine#trajectory} method and its {@link Trigger} API. {@link
* #trajectoryCommand} does not invoke bindings added via calling {@link #bind} or {@link
* AutoBindings} passed into the factory constructor.
*
Expand All @@ -294,17 +219,17 @@ public Command trajectoryCommand(String trajectoryName) {
* @return A new auto trajectory.
*/
public Command trajectoryCommand(String trajectoryName, final int splitIndex) {
return trajectory(trajectoryName, splitIndex, VOID_ROUTINE).cmd();
return voidRoutine.trajectory(trajectoryName, splitIndex).cmd();
}

/**
* Creates a new auto trajectory command to be used in an auto routine.
*
* <p><b>Important </b>
*
* <p>{@link #trajectoryCommand} and {@link #trajectory} methods should not be mixed in the same
* <p>{@link #trajectoryCommand} and {@link AutoRoutine#trajectory} methods should not be mixed in the same
* auto routine. {@link #trajectoryCommand} is used as an escape hatch for teams that don't need
* the benefits of the {@link #trajectory} method and its {@link Trigger} API. {@link
* the benefits of the {@link AutoRoutine#trajectory} method and its {@link Trigger} API. {@link
* #trajectoryCommand} does not invoke bindings added via calling {@link #bind} or {@link
* AutoBindings} passed into the factory constructor.
*
Expand All @@ -314,7 +239,7 @@ public Command trajectoryCommand(String trajectoryName, final int splitIndex) {
*/
public <SampleType extends TrajectorySample<SampleType>> Command trajectoryCommand(
Trajectory<SampleType> trajectory) {
return trajectory(trajectory, VOID_ROUTINE).cmd();
return voidRoutine.trajectory(trajectory).cmd();
}

/**
Expand Down
Loading
Loading