DONT.java

package org.firstinspires.ftc.teamcode;

import com.bylazar.configurables.annotations.Configurable;
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierCurve;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.PathChain;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.pedropathing.util.Timer;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
// this thing is going to explode if you run it
@Disabled
@Configurable
@Autonomous(name = "DONT")
public class DONT extends OpMode {
    private Follower follower;
    private Timer actionTimer;
    private int pathState = 0;
    private boolean pathStarted = false; // Tracks if path has been started

    private final Pose startPose = new Pose(20.187, 125.383, Math.toRadians(142));
    private Paths paths;

    private DualMotorFlywheelController flywheel;
    private DcMotor Intake;
    private DcMotor Launcher;


    public static double minDistance = 7;
    public static double maxDistance = 148;

    public static double minVelocity = 1200;   // rpm close & near–air-shot
    public static double maxVelocity = 3400;   // rpm long shot

    public static double goalX = 14.178;
    public static double goalY = 135.582;

    private double scale(double input, double inMin, double inMax, double outMin, double outMax) {
        input = Math.max(inMin, Math.min(input, inMax));
        return outMin + (outMax - outMin) * ((input - inMin) / (inMax - inMin));
    }

    public static class Paths {
        public PathChain ShootingPosition;
        public PathChain Pickup1;
        public PathChain Pickup1ToShootingPose;
        public PathChain Pickup2;
        public PathChain FinalPose;


        public Paths(Follower follower) {
            ShootingPosition = follower.pathBuilder()
                .addPath(new BezierLine(new Pose(20.187, 125.383), new Pose(43.738, 102.505)))
                .setLinearHeadingInterpolation(Math.toRadians(142), Math.toRadians(142))
                .build();

            Pickup1 = follower.pathBuilder()
                .addPath(new BezierCurve(new Pose(43.738, 102.505),
                    new Pose(63.925, 79.402),
                    new Pose(18.393, 83.664)))
                .setLinearHeadingInterpolation(Math.toRadians(142), Math.toRadians(180))
                .build();

            Pickup1ToShootingPose = follower.pathBuilder()
                .addPath(new BezierCurve(new Pose(18.393, 83.664),
                    new Pose(63.925, 79.402),
                    new Pose(43.738, 102.505)))
                .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(142))
                .build();

            Pickup2 = follower.pathBuilder()
                .addPath(new BezierCurve(new Pose(43.738, 102.505),
                    new Pose(105.869, 53.159),
                    new Pose(17.944, 59.888)))
                .setLinearHeadingInterpolation(Math.toRadians(142), Math.toRadians(180))
                .build();

            FinalPose = follower.pathBuilder()
                .addPath(new BezierCurve(new Pose(17.944, 59.888),
                    new Pose(105.869, 53.159),
                    new Pose(43.738, 102.505)))
                .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(142))
                .build();
        }
    }

    @Override
    public void init() {
        actionTimer = new Timer();
        Timer opmodeTimer = new Timer();
        opmodeTimer.resetTimer();

        Intake = hardwareMap.get(DcMotorEx.class, "Intake");
        Launcher = hardwareMap.get(DcMotorEx.class, "Launcher");

        follower = Constants.createFollower(hardwareMap);
        follower.setStartingPose(startPose);
        paths = new Paths(follower);

        flywheel = new DualMotorFlywheelController(hardwareMap, "flywheel1", "flywheel2");
        flywheel.setPIDF(0.003, 0.0, 0.0002, 0.00012); // tune these
    }

    @Override
    public void loop() {
        follower.update();

        // Compute distance to goal
        Pose pose = follower.getPose();
        double dx = goalX - pose.getX();
        double dy = goalY - pose.getY();
        double distance = Math.hypot(dx, dy);


        // Scale distance → flywheel velocity
        double targetRPM = scale(distance, minDistance, maxDistance, minVelocity, maxVelocity);
        flywheel.setTargetVelocity(targetRPM);
        flywheel.update(); // single update per loop
        telemetry.addData("path state", pathState);
        telemetry.addData("x", pose.getX());
        telemetry.addData("y", pose.getY());
        telemetry.addData("heading", Math.toDegrees(pose.getHeading()));
        telemetry.addData("flywheel target RPM", targetRPM);
        telemetry.addData("flywheel actual RPM", flywheel.getVelocity());
        telemetry.update();

        // Path-following state machine
        switch (pathState) {
            case 0: // Move to shooting position
                if (!pathStarted) {
                    follower.followPath(paths.ShootingPosition);
                    pathStarted = true;
                }
                if (!follower.isBusy()) {
                    pathState++;
                    pathStarted = false;
                    actionTimer.resetTimer();
                }
                break;

            case 1: // Shoot
                if (actionTimer.getElapsedTime() < 2.0) {
                    // TODO: trigger shooter servo/intake
                    Launcher.setPower(-1);
                } else {
                    pathState++;
                    actionTimer.resetTimer();
                }
                break;

            case 2: // Move to pickup 1
                if (!pathStarted) {
                    follower.followPath(paths.Pickup1);
                    pathStarted = true;
                }
                if (!follower.isBusy()) {
                    pathState++;
                    pathStarted = false;
                    actionTimer.resetTimer();
                }
                break;

            case 3: // Pick up first object
                if (actionTimer.getElapsedTime() < 1.5) {
                    // TODO: activate intake
                    Intake.setPower(-1);
                } else {
                    pathState++;
                    actionTimer.resetTimer();
                }
                break;

            case 4: // Return to shooting position
                if (!pathStarted) {
                    follower.followPath(paths.Pickup1ToShootingPose);
                    pathStarted = true;
                }
                if (!follower.isBusy()) {
                    pathState++;
                    pathStarted = false;
                    actionTimer.resetTimer();
                }
                break;

            case 5: // Shoot again
                if (actionTimer.getElapsedTime() < 2.0) {
                    // TODO: trigger shooter servo/intake
                    Launcher.setPower(-1);
                } else {
                    pathState++;
                    actionTimer.resetTimer();
                }
                break;

            case 6: // Move to pickup 2
                if (!pathStarted) {
                    follower.followPath(paths.Pickup2);
                    pathStarted = true;
                }
                if (!follower.isBusy()) {
                    pathState++;
                    pathStarted = false;
                    actionTimer.resetTimer();
                }
                break;

            case 7: // Pick up second object
                if (actionTimer.getElapsedTime() < 1.5) {
                    // TODO: activate intake
                    Intake.setPower(-1);
                } else {
                    pathState++;
                    actionTimer.resetTimer();
                }
                break;

            case 8: // Move to final pose
                if (!pathStarted) {
                    follower.followPath(paths.FinalPose);
                    pathStarted = true;
                    actionTimer.resetTimer(); // start shooting timer
                }
                // Once at final pose, activate launcher for 3 seconds
                if (!follower.isBusy()) {
                    if (actionTimer.getElapsedTime() < 3.0) {
                        // TODO: trigger shooter servo/intake here
                        Launcher.setPower(-1);
                    }
                }
                break;
        }

        // Telemetry

    }
}