Backup.java

package org.firstinspires.ftc.teamcode;

import com.bylazar.configurables.annotations.Configurable;
import com.bylazar.telemetry.PanelsTelemetry;
import com.bylazar.telemetry.TelemetryManager;
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.HeadingInterpolator;
import com.pedropathing.paths.Path;
import com.pedropathing.paths.PathChain;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

import org.firstinspires.ftc.teamcode.pedroPathing.Constants;

import java.util.function.Supplier;

@Configurable
@TeleOp(name = "BackupTeleOp")
public class Backup extends OpMode {
    private Follower follower;
    public static Pose startingPose; //See ExampleAuto to understand how to use this
    private boolean automatedDrive;
    private Supplier<PathChain> pathChain;
    private TelemetryManager telemetryM;
    private boolean slowMode = false;
    final private double slowModeMultiplier = 0.7;
    final private double turnSpeedMultiplier = 0.5;
    DcMotor Intake;
    DcMotor Launcher;

    DcMotor Output1;
    DcMotor Output2;

    boolean rToggle = false;
    boolean rlastTriggeredPressed = false;
    boolean lToggle = false;
    boolean lLastTriggeredPressed = false;
    DualMotor outputMotors;




    @Override
    public void init() {
        follower = Constants.createFollower(hardwareMap);
        follower.setStartingPose(startingPose == null ? new Pose() : startingPose);
        follower.update();
        telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
        Intake = hardwareMap.get(DcMotorEx.class, "Intake");
        Launcher = hardwareMap.get(DcMotor.class, "Launcher");
        DcMotorEx Output1 = hardwareMap.get(DcMotorEx.class, "flywheel1");
        DcMotorEx Output2 = hardwareMap.get(DcMotorEx.class, "flywheel2");
        outputMotors = new DualMotor(Output1, Output2);
        Output1.setDirection(DcMotorSimple.Direction.REVERSE);
        Output2.setDirection(DcMotorSimple.Direction.FORWARD);




        pathChain = () -> follower.pathBuilder() //Lazy Curve Generation
            .addPath(new Path(new BezierLine(follower::getPose, new Pose(45, 98))))
            .setHeadingInterpolation(HeadingInterpolator.linearFromPoint(follower::getHeading, Math.toRadians(45), 0.8))
            .build();

    }

    @Override
    public void start() {
        //The parameter controls whether the Follower should use break mode on the motors (using it is recommended).
        //In order to use float mode, add .useBrakeModeInTeleOp(true); to your Drivetrain Constants in Constant.java (for Mecanum)
        //If you don't pass anything in, +it uses the default (false)
        follower.startTeleopDrive();
    }

    @Override
    public void loop() {
        //Call this once per loop
        follower.update();
        telemetryM.update();
        boolean RtriggeredPressed = gamepad1.right_trigger > 0.5;
        boolean LtriggeredPressed = gamepad1.left_trigger > 0.5;
        if (!automatedDrive) {
            //Make the last parameter false for field-centric
            //In case the drivers want to use a "slowMode" you can scale the vectors

            //This is the normal version to use in the TeleOp
            if (!slowMode) follower.setTeleOpDrive(
                -gamepad1.left_stick_y,
                -gamepad1.left_stick_x,
                -(gamepad1.right_stick_x*turnSpeedMultiplier),
                true // Robot Centric
            );
                //new comment test
                //This is how it looks with slowMode on
            else follower.setTeleOpDrive(
                -gamepad1.left_stick_y * slowModeMultiplier,
                -gamepad1.left_stick_x * slowModeMultiplier,
                ((-gamepad1.right_stick_x * slowModeMultiplier)*turnSpeedMultiplier),
                true // Robot Centric
            );
        }
        if (RtriggeredPressed && !rlastTriggeredPressed) {
            rToggle = !rToggle;
        }
        Intake.setPower(rToggle ? -1.0 : 0.0);
        rlastTriggeredPressed = RtriggeredPressed;

        if (LtriggeredPressed && !lLastTriggeredPressed) {
            lToggle = !lToggle;
        }
        outputMotors.setPower(lToggle ? 0.60 : 0.0);
        lLastTriggeredPressed = LtriggeredPressed;

        if (gamepad1.left_bumper) {
            Launcher.setPower(-0.75);
        } else {
            Launcher.setPower(0);
        }

        telemetryM.debug("position", follower.getPose());
        telemetryM.debug("velocity", follower.getVelocity());
        telemetryM.debug("automatedDrive", automatedDrive);
    }
}