sa

package org.firstinspires.ftc.teamcode.drive.opmode;

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.teamcode.drive.mecanum.SampleMecanumDriveBase;
import org.firstinspires.ftc.teamcode.drive.mecanum.SampleMecanumDriveREV;

import kotlin.reflect.jvm.internal.impl.load.java.structure.LightClassOriginKind;

/*
 * This is a simple routine to test translational drive capabilities.
 */
@Config
@Autonomous(group = "drive")
public class Auto_RED extends LinearOpMode {
    public static double DISTANCE = 20;
    public static double LOCKED = 0.3;
    public static double UNLOCKED = 0.8;

    public static float S = 240;
    public static int R = 120;

    @Override
    public void runOpMode() throws InterruptedException {
        SampleMecanumDriveBase drive = new SampleMecanumDriveREV(hardwareMap);

        ((SampleMecanumDriveREV) drive).locked(UNLOCKED);

        Trajectory trajectory2 = drive.trajectoryBuilder()
                .back(38)
                .strafeRight(75)
                .build();

        Trajectory trajectory1 = drive.trajectoryBuilder()
                .strafeRight(23)
                .forward(72)
                .build();

        Trajectory trajectory3 = drive.trajectoryBuilder()
                .back(30)
                .strafeLeft(95)
                .back(5)
                .strafeLeft(5)
                .forward(90)
                .build();

        Trajectory trajectory4 = drive.trajectoryBuilder()
                .back(60)
                .strafeRight(120)
                .build();

        Trajectory trajectory5 = drive.trajectoryBuilder()
                .back(30)
                .strafeLeft(40)
                .forward(35)
                .build();


        waitForStart();


        drive.followTrajectorySync(trajectory1);
        ((SampleMecanumDriveREV) drive).locked(LOCKED);
        drive.followTrajectorySync(trajectory2);
        ((SampleMecanumDriveREV) drive).locked(UNLOCKED);
        drive.followTrajectorySync(trajectory3);
        ((SampleMecanumDriveREV) drive).locked(LOCKED);
        drive.followTrajectorySync(trajectory4);
        ((SampleMecanumDriveREV) drive).locked(UNLOCKED);
        drive.followTrajectorySync(trajectory5);

    }

}