package org.firstinspires.ftc.teamcode.LoadZone;

import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.Servo;
import java.util.Iterator;
import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;

import org.firstinspires.ftc.teamcode.DriveClass;

@Autonomous(name=" BLUE (Park): Skybridge Park", group="chad")
@Disabled

public class LOAD_Blue_Skybridge extends LinearOpMode {
    DriveClass dt;
    DcMotor intake1, intake2;
    Servo found1, found2;

    public void runOpMode(){
        dt = new DriveClass(hardwareMap, this);

        intake1 = hardwareMap.get(DcMotor.class, "intakeLeft");
        intake2 = hardwareMap.get(DcMotor.class, "intakeRight");

        found1 = hardwareMap.get(Servo.class, "foundLeft");
        found2 = hardwareMap.get(Servo.class, "foundRight");
        /* SOME GOOD THINGS TO KNOW:
        1. USE 0.2 POWER FOR TURNING
        2. USE 0.5 POWER FOR FORWARDS / BACKWARDS
        3. DON'T DELETE SHIT IF IT DOESN'T WORK, COMMENT IT
        */

        dt.initMotors();
        dt.initGyro();
        //
        waitForStart();
        dt.strafeRight(29, 0.5);
        sleep(200);
        dt.moveForward(12, 0.5);
    }
}