Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -15,35 +15,78 @@
import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive;
import org.firstinspires.ftc.teamcode.subsystems.Indexer;

/**
* FarLeave is a minimal "leave the starting zone" autonomous OpMode.
*
* <p>This is the simplest autonomous program: the robot simply drives straight
* forward to the parking zone and stops. It is used when the field-side
* ("far") starting position is used and there is no time to implement a more
* complex autonomous routine, or as a guaranteed scoring fallback when more
* complex autos are unreliable.
*
* <p>The robot starts at field position (0, 0) facing 0° (right), then
* drives straight to ({@link #PARK_X}, {@link #PARK_Y}). Both coordinates
* are tunable at runtime via FTC Dashboard (the class is annotated with
* {@link Config}).
*
* <p>Registered as {@code "Far Leave Auto"} in the {@code "Autonomous"} group.
*/
@Config
@Autonomous(name = "Far Leave Auto", group = "Autonomous")
public class FarLeave extends LinearOpMode {

/**
* Target X-coordinate (inches) for the parking position.
* Positive X moves the robot forward (away from the driver station wall).
* Default of 26 inches is enough to cross the field boundary line.
*/
public static double PARK_X = 26;

/**
* Target Y-coordinate (inches) for the parking position.
* Zero keeps the robot on the same lateral track as the starting position.
* Adjust if the robot needs to dodge field elements while leaving.
*/
public static double PARK_Y = 0;

/**
* Entry point called by the FTC runtime.
* Builds the park trajectory, waits for start, then executes it.
*
* @throws InterruptedException (implicitly) if the OpMode is interrupted
*/
@Override
public void runOpMode() {
// Define the starting pose: origin (0,0) facing 0° (pointing in +X direction).
Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0));

// Construct all hardware subsystems via the Hardware container.
// Hardware also builds the MecanumDrive and BotActions.
Hardware hardware = new Hardware(hardwareMap, telemetry, this, startPose);
BotActions botActions = hardware.actions;
MecanumDrive drive = hardware.mecanumDrive;
BotActions botActions = hardware.actions; // action factory (unused here but available)
MecanumDrive drive = hardware.mecanumDrive; // Road Runner drive for trajectory building

// The destination pose: (PARK_X, PARK_Y) still facing 0°.
Pose2d parkPose = new Pose2d(PARK_X, PARK_Y, Math.toRadians(0));

// Build the single-segment trajectory: strafe straight from startPose to parkPose.
// strafeTo drives along a straight line while maintaining the current robot heading.
Action toPark = drive.actionBuilder(startPose)
.strafeTo(
parkPose.position
parkPose.position // target x,y; heading is held constant at 0°
)
.build();

waitForStart();
if (isStopRequested()) return;
waitForStart(); // block here until the referee presses PLAY
if (isStopRequested()) return; // abort immediately if stop was pressed during init

// Execute the parking trajectory.
// ParallelAction with a single child is equivalent to running that child directly;
// the wrapper is kept here for structural consistency with other autonomous OpModes.
Actions.runBlocking(
new ParallelAction(
new SequentialAction(
toPark
toPark // drive forward to the parking position
)
)
);
Expand Down

Large diffs are not rendered by default.

Large diffs are not rendered by default.

Large diffs are not rendered by default.

Large diffs are not rendered by default.

Large diffs are not rendered by default.

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
Expand Up @@ -3,43 +3,129 @@
import java.util.ArrayList;
import java.util.List;

/**
* Action is a lightweight sequential-task runner for use in autonomous
* routines that do not use the full Road Runner Action framework.
*
* <p>An {@code Action} holds an ordered list of {@link Runnable} steps.
* Calling {@link #run()} executes every step in sequence, blocking until
* all steps complete.
*
* <p>Instances are constructed exclusively through the nested {@link Builder}
* to enforce a clear, readable construction pattern:
* <pre>
* Action myAction = new Action.Builder()
* .add(() -> subsystem.doSomething())
* .moveTo(() -> drive.moveTo(targetPose))
* .add(() -> shooter.fire())
* .build();
* myAction.run();
* </pre>
*
* <p><b>Note:</b> This class predates the Road Runner-based
* {@link com.acmerobotics.roadrunner.Action} pipeline and is used in
* helper utilities that need a simple imperative list of operations.
*/
public class Action {

// -----------------------------------------------------------------------
// Steps
// -----------------------------------------------------------------------

/**
* The ordered list of {@link Runnable} steps to execute when {@link #run()}
* is called. Populated only by the {@link Builder}.
*/
private final List<Runnable> steps;

// -----------------------------------------------------------------------
// Constructor (private – use Builder)
// -----------------------------------------------------------------------

/**
* Private constructor; only the {@link Builder} may create Action instances.
*
* @param steps the ordered list of steps to execute
*/
// Constructor is private; only Builder can build Actions
private Action(List<Runnable> steps) {
this.steps = steps;
this.steps = steps; // store the immutable-from-outside step list
}

// -----------------------------------------------------------------------
// Execution
// -----------------------------------------------------------------------

/**
* Executes all steps in order, blocking until each step completes.
*
* <p>Steps are run synchronously on the calling thread; there is no
* timeout or interruption mechanism.
*/
// Run all steps in order
public void run() {
for (Runnable step : steps) {
step.run();
step.run(); // execute each step sequentially
}
}

// -----------------------------------------------------------------------
// Builder
// -----------------------------------------------------------------------

/**
* Fluent builder for constructing {@link Action} instances.
*
* <p>Steps can be added in any order using {@link #add(Runnable)} or
* {@link #moveTo(Runnable)}. Call {@link #build()} to produce the
* immutable {@link Action}.
*/
// Builder nested class
public static class Builder {

/** Accumulates steps in insertion order. */
private final List<Runnable> steps = new ArrayList<>();

/**
* Creates an empty Builder.
*/
public Builder() {}

/**
* Appends a custom step to the action sequence.
*
* @param step any {@link Runnable} to execute at this position in the sequence
* @return this Builder (enables method chaining)
*/
// Add a custom step
public Builder add(Runnable step) {
steps.add(step);
return this; // Allows chaining
steps.add(step); // append to the ordered step list
return this; // return this for chaining
}

/**
* Appends a movement step to the action sequence.
* Semantically equivalent to {@link #add(Runnable)}; provided as a
* named convenience to make movement steps visually distinct in code.
*
* @param movement the movement command to execute (e.g., a drive-to-pose call)
* @return this Builder (enables method chaining)
*/
// Example: move to a position
public Builder moveTo(Runnable movement) {
steps.add(movement);
steps.add(movement); // append the movement command like any other step
return this;
}

/**
* Finalises the builder and returns the constructed {@link Action}.
* The returned Action's step list is a snapshot of all steps added so far.
*
* @return a new {@link Action} containing all added steps in order
*/
// Build the org.firstinspires.ftc.teamcode.auto.utils.Action
public Action build() {
return new Action(steps);
return new Action(steps); // construct the Action from the accumulated steps
}
}
}
Expand Down
Loading