auto+red

package org.firstinspires.ftc.teamcode;

import android.graphics.Color;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Hardware;


@Autonomous(name = "ALL", group = "nobody likes you")
public class autonomous_red extends LinearOpMode
{

    private enum beaconcolor
    {
        sRed,
        sBlue,
        sUnknown
    }

    private ElapsedTime time = new ElapsedTime();
    private int redtimes = 0;
    private int bluetimes = 0;
    private beaconcolor m_beaconcolor;
    private HardWare11226 m_hardware = new HardWare11226();
    boolean runonce;
    final double wheel_peremeter = 12.56;

    @Override
    public void runOpMode() throws InterruptedException {

        m_hardware.init(hardwareMap);

        runonce = true;

        m_hardware.leftMotor.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors
        m_hardware.rightMotor.setDirection(DcMotor.Direction.REVERSE);

        beginf();

        telemetry.addData("Status", "ready to run.");
        telemetry.update();

        waitForStart();
        time.reset();
        time.startTime();

        while (opModeIsActive())
        {
            telemetry.addData("Status: ", "while.");

            if (runonce == true)
            {
                telemetry.addData("Status: ", "if.");
                m_hardware.leftMotor.setPower(0.3);
                m_hardware.rightMotor.setPower(0.3);
                sleep(200);

                driving(52, 0.8);
                turning(-90, 90, -0.4, 0.4);
                driving(35, 0.3);
                telemetry.addData("done","2");
                telemetry.update();
                driving(-8, -0.4);
                driving(9, 0.3);
                telemetry.addData("done","3");
                telemetry.update();
                driving(-2, -0.2);
                beaconDetectRed();
                telemetry.addData("done", "4");
                telemetry.update();
                driving(-32, -0.4);
                turning(90, -90, 0.4, -0.4);
                telemetry.addData("done","5");
                telemetry.update();
                driving(46, 0.8);
                telemetry.addData("done","6");
                telemetry.update();
                turning(-90, 90, -0.4, 0.4);
                telemetry.addData("Status: ", "done.");
                runonce = false;
            }

            telemetry.update();
            idle();
        }
    }

    public void turning(int L_angle, int R_angle, double left_power, double right_power) {


        int leftTarget = (int) (((46.4955713 / wheel_peremeter) * 3.111) * L_angle);
        int rightTarget = (int) (((46.4955713 / wheel_peremeter) * 3.111) * R_angle);

        m_hardware.leftMotor.setTargetPosition(leftTarget);
        m_hardware.rightMotor.setTargetPosition(rightTarget);

        m_hardware.leftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        m_hardware.rightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        m_hardware.leftMotor.setPower(left_power);
        m_hardware.rightMotor.setPower(right_power);

        while (m_hardware.leftMotor.isBusy() && m_hardware.rightMotor.isBusy()) {
            telemetry.addData("Status: ", "isBusy.");
            telemetry.addData("left pos", m_hardware.leftMotor.getCurrentPosition());
            telemetry.addData("right pos", m_hardware.rightMotor.getCurrentPosition());
            telemetry.update();
        }

        Stop();
        beginf();
        sleep(350);

        telemetry.update();
    }

    public void driving(double distance, double power)
    {

        int Target = (int) ((distance / wheel_peremeter) * 1120);
        m_hardware.leftMotor.setTargetPosition(Target);
        m_hardware.rightMotor.setTargetPosition(Target);

        m_hardware.leftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        m_hardware.rightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        m_hardware.leftMotor.setPower(power);
        m_hardware.rightMotor.setPower(power);

        while (m_hardware.rightMotor.isBusy())
        {
            telemetry.addData("Status: ", "isBusy.");
            telemetry.addData("left pos", m_hardware.leftMotor.getCurrentPosition());
            telemetry.addData("right pos", m_hardware.rightMotor.getCurrentPosition());

        }

        Stop();
        beginf();
        sleep(350);

        telemetry.update();
    }

    public void Stop()
    {
        m_hardware.leftMotor.setPower(0);
        m_hardware.rightMotor.setPower(0);
    }


  public void beaconDetectRed() throws InterruptedException
  {
        float hsvValues[] = {0F, 0F, 0F};
        Color.RGBToHSV(m_hardware.color_sensor_red.red() * 8, m_hardware.color_sensor_red.green() * 8, m_hardware.color_sensor_red.blue() * 8, hsvValues);
        time.reset();
        while (time.milliseconds() <= 600) {
            if (hsvValues[0] <= 40 || hsvValues[0] >= 340)
            {
                redtimes++;
                telemetry.addData("red :", redtimes);
                telemetry.update();
            }
            else if (hsvValues[0] <= 240 || hsvValues[0] >= 220)
            {
                bluetimes++;
                telemetry.addData("blue :", bluetimes);
                telemetry.update();
            }
        }

        if (redtimes > bluetimes)
        {
            m_beaconcolor = beaconcolor.sRed;
            telemetry.addData("case red :", redtimes);
            telemetry.update();
        }
        else if (bluetimes > redtimes)
        {
            m_beaconcolor = beaconcolor.sBlue;
            telemetry.addData("case blue", bluetimes);
            telemetry.update();
        }
        else
        {
            m_beaconcolor = beaconcolor.sUnknown;
        }


        telemetry.addData("hue", hsvValues[0]);
        telemetry.addData("redtimes", redtimes);
        telemetry.addData("bluetimes", bluetimes);

        telemetry.update();

        switch (m_beaconcolor)
        {
            case sBlue:
                telemetry.addData("beacon color detected!!!", " blue");
                driving(-4, -0.3);
                sleep(4500);
                driving(5, 0.4);
                driving(5, -0.4);
                driving(5, 0.4);
                driving(5, -0.4);
                break;

            case sRed:
                telemetry.addData("beacon color detected!!!", " red");
                break;

            case sUnknown:
                telemetry.addData("beacon color NOT detected!!!", " search again for color");
                beaconDetectRed();
                break;
        }
      }


    public void beginf()
    {

        m_hardware.leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        m_hardware.rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        m_hardware.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        m_hardware.rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);


    }
}