murray-bridge-bunyips / bunyipslib Goto Github PK

@TeleOp(name = "My OpMode")
public class MyOpMode extends BunyipsOpMode {
    private final MyBotConfig config = new MyBotConfig();

    @Override
    protected void onInit() {
        config.init(); // Instant initialisation defined by a common `config`
    }

    @Override
    protected void activeLoop() {
        // Runs repeatedly in the active loop
    }

    // Other virtual methods include: onInitLoop(), onInitDone(), onStart(), onStop()
}

@TeleOp(name = "My TeleOp")
public class MyTeleOp extends BunyipsOpMode {
    private final MyBotConfig config = new MyBotConfig();

    private MecanumDrive drive;
    private MyArm arm;
    private Cannon cannon;

    @Override
    protected void onInit() {
        config.init();
        drive = new DualDeadwheelMecanumDrive(
                config.driveConstants, config.mecanumCoefficients,
                hardwareMap.voltageSensor, config.imu, config.frontLeft, config.frontRight,
                config.backLeft, config.backRight, config.localizerCoefficients,
                config.parallelEncoder, config.perpendicularEncoder
        );

        // Custom gamepad objects which support feeding lambda functions to evaluate each
        // input or a group of inputs (for custom functions for inputs without having to change component code)
        gamepad1.set(Controls.AnalogGroup.STICKS, (v) -> v * 0.8f);

        cannon = new Cannon(/* ... */);
        arm = new MyArm(/* ... */);
    }

    @Override
    protected void activeLoop() {
        double x = gamepad1.left_stick_x;
        double y = gamepad1.left_stick_y;
        double r = gamepad1.right_stick_x;

        drive.setSpeedUsingController(x, y, r);
        if (gamepad2.y) {
            arm.faceClawToBoard();
        } else if (gamepad2.a) {
            arm.faceClawToGround();
        }

        // Custom gamepad objects allow for shorthand field names (rt == right_trigger)
        if (gamepad1.rt == 1.0) {
            cannon.fire();
        }
        if (gamepad1.back) {
            cannon.reset();
        }

        if (gamepad1.dpad_up) {
            arm.extendHook();
        } else if (gamepad1.dpad_down) {
            arm.retractHook();
        }

        drive.update();
        arm.update();
        cannon.update();
    }
}

@TeleOp(name = "Align To Pixel (Command Based)")
public class MyAlignToPixelTeleOp extends CommandBasedBunyipsOpMode {
    private final MyBotConfig config = new MyBotConfig();

    private MecanumDrive drive;
    private Vision vision;
    private MultiColourThreshold pixels;

    @Override
    protected void onInitialise() {
        config.init();

        drive = new DualDeadwheelMecanumDrive(/* ... */);
        vision = new Vision(/* ... */);

        pixels = new MultiColourThreshold(Pixels.createProcessors());
        // Choosable processors with full control methods
        vision.init(pixels, vision.raw);
        vision.start(pixels, vision.raw);

        // FtcDashboard integrations for individual processor stream previews
        // Vision previews work across multiple instances of Vision, allowing debugging across
        // multiple processors across multiple cameras within FtcDashboard.
        vision.startPreview();
        
        // Programmatic selection of previews exist as well, which will auto-switch the stream to
        // focus on this processor and camera.
        vision.setPreview(pixels);

        // The Scheduler will automatically manage all subsystems that are constructed, however
        // users can choose to update their own choice of subsystems for rapid debugging purposes.
        // More integrated solutions include calling .disable() on the subsystems in question
        // that should not be updated.
        useSubsystems(drive, vision); // Only update drive and vision
    }

    @Override
    protected void assignCommands() {
        // Running with default binds on gamepad1, without activating Field Centric drive (POV only)
        drive.setDefaultTask(new HolonomicDriveTask(gamepad1, drive, () -> false));

        // Calls reset yaw method every cycle while gamepad1.y is held
        driver().whenHeld(Controls.Y)
                .run(() -> drive.resetYaw());

        // Calls drive.stop() for 2 seconds after gamepad2.x is released for 2 seconds
        operator().whenReleased(Controls.X)
                .run(new RunForTask(Seconds.of(2), () -> drive.stop()))
                .in(Seconds.of(2));

        // Starts the AlignToContourTask on the drive subsystem when gamepad1.rb is pressed, cancelling on release
        driver().whenPressed(Controls.RIGHT_BUMPER)
                .run(new AlignToContourTask<>(gamepad1, drive, pixels, new PIDController(1, 0.25, 0.0)))
                .finishingIf(() -> !gamepad1.right_bumper);
    }
}

public class RunArmFor extends Task {
    private final MyArm arm;
    private final MyScooper scooper;
    private final double power;

    public RunArmFor(Measure<Time> time, MyArm arm, MyScooper scooper, double power) {
        super(time); // Built-in timeout management with the full WPILib units conversion system
        // You may also wish to run this task on a subsystem with super(time, <subsystem>, <should override>),
        // this only applies to TeleOp OpModes, as Autonomous OpModes only run one task at a time.
        this.arm = arm;
        this.power = power;
        this.scooper = scooper;
    }

    @Override
    public void init() {
        arm.setPower(power);
        scooper.startScooping();
    }

    @Override
    public void periodic() {
        // Full telemetry utilities for FtcDashboard and Driver Station
        opMode.telemetry.add("Arm is at % ticks.", arm.getTicks());
        // Smaller set of an active loop to complete a task
        if (arm.isOverCurrent()) {
            scooper.pause();
        } else {
            scooper.resume();
        }
    }

    @Override
    public void onFinish() {
        arm.stopNow();
    }

    @Override
    public boolean isTaskFinished() {
        return arm.calculatePosition() > 400 && scooper.finishedScooping();
    }
}

@Autonomous(name = "Place a Pixel")
public class MyPlacePixelAuto extends AutonomousBunyipsOpMode implements RoadRunner {
    private final MyBotConfig config = new MyBotConfig();
    private DualDeadwheelMecanumDrive drive;

    private MyArm arm;
    private Vision vision;
    private PurplePixel purplePixelProcessor;
    private WhitePixel whitePixelProcessor;

    private YourInitTask initTask;

    @Override
    protected void onInitialise() {
        config.init();

        drive = new DualDeadwheelMecanumDrive(/* ... */);
        vision = new Vision(/* ... */);
        arm = new MyArm(/* ... */);

        vision.init(purplePixelProcessor, whitePixelProcessor);
        vision.start(whitePixelProcessor);

        // Optionally send all feeds to FtcDashboard, can switch between processor feeds for debugging
        // vision.startPreview();

        telemetry.add("Hello world!"); // Custom telemetry object to provide joint FtcDashboard/DS telemetry
        telemetry.addRetained("Greetings, world.");

        // Full support for any object to use as a selection, including enums
        // This will be automatically shown on the init-cycle.
        setOpModes(
                "PARK",
                "GO_TWICE",
                "SABOTAGE_ALLIANCE",
                "STOP"
        );

        // A task reference to dispatch after initialisation until the init-phase is done, or the task finishes
        setInitTask(initTask);
    }

    @NotNull
    @Override
    public MecanumDrive getDrive() {
        // Passes the drive to the RoadRunner interface to provide
        // utility methods for fast, reusable, and efficient trajectory generation (makeTrajectory() util)
        return drive;
    }

    @Override
    protected void onInitDone() {
        switch (initTask.getSpikeResult()) {
            case LEFT:
                // RoadRunner trajectory to get to the Spike Mark
                makeTrajectory(/* ... */);
                break;
            // ...
        }
        vision.stop(whitePixelProcessor);
    }

    @Override
    protected void onReady(@Nullable Reference<?> selectedOpMode, Controls selectedButton) {
        addTask(() -> vision.start(purplePixelProcessor));

        // Full RoadRunner support with utility methods such as makeTrajectory()
        makeTrajectory(new Pose2d(11.40, -62.00, Math.toRadians(180.00)))
                .lineToLinearHeading(new Pose2d(16.40, -48.10, Math.toRadians(90.00)))
                .lineToLinearHeading(new Pose2d(11.71, -34.52, Math.toRadians(90.00)))
                .addTask();

        // Interchangeable tasks between TeleOp commands and Autonomous tasks using overloads
        // This makes tasks reusable and easier to write, where you can define your own tasks just as quick
        addTask(new MoveToContourTask<>(Seconds.of(5), drive, purplePixel, new PIDController(1, 0.25, 0)));

        addTask(arm::drop);

        switch (selectedOpMode.toString()) {
            case "PARK":
                addTask(/* ... */);
                break;
            case "GO_TWICE":
                // Convenience task groupings for allocating multiple subsystem interactions
                // while only using a singular task. Useful in command-based OpModes.
                addTask(new SequentialTaskGroup(/* ..., ..., ... */));
                break;
            case "SABOTAGE_ALLIANCE":
                // BunyipsLib offers full automatic exception catching and debug logging
                // with support for exceptions that can emergency stop an OpMode as well
                throw new EmergencyStop("You have been banished for being a traitor.");
            default:
            case "STOP":
                break;
        }
    }
}