main_teleop

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.util.ElapsedTime;

@TeleOp(name = "Teleop")
public class Main_Teleop extends LinearOpMode {
    private HardWare11226 m_hardware = new HardWare11226();
    private boolean m_shootingbool = false;
    private boolean m_driveboolean = false;
    private ElapsedTime m_shootingtime = new ElapsedTime();

    private enum shootingstate
    {
        sStop,
        sWarming,
        sShooting
    }

    private enum capballstate
    {
        sLock,
        sLift
    }
    private shootingstate m_shootingstate;
    private capballstate m_capballstate;
    private double left_Power, right_Power, cap_Power;
    @Override
    public void runOpMode() throws InterruptedException {


        m_hardware.init(hardwareMap);
        m_hardware.capBallMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        m_hardware.shootingServo.setPosition(0.2);
        m_hardware.capBallServo.setPosition(0.5);


        waitForStart();

        while (opModeIsActive())
        {
            left_Power = gamepad1.left_stick_y;
            right_Power = gamepad1.right_stick_y;
            cap_Power = gamepad2.right_stick_y;
            if (!gamepad1.left_bumper && !gamepad1.right_bumper)
            {
                driving(left_Power, right_Power);
            }
            else
            {
                straightdriving();
            }
            collect();
            shooting();
            capball();
            changedirection();
        }
    }

    private void driving(double p_left, double p_right)
    {
        if (p_left > 0.2 || p_left < -0.2)
        {
            m_hardware.leftMotor.setPower(p_left);
        }
        else
        {
            m_hardware.leftMotor.setPower(0);
        }
        if (p_right > 0.2 || p_right < -0.2)
        {
            m_hardware.rightMotor.setPower(p_right);
        }
        else
        {
            m_hardware.rightMotor.setPower(0);
        }
    }

    private void collect()
    {
        if (gamepad2.left_bumper && !gamepad2.right_bumper)
        {
            m_hardware.collectMotor.setPower(-1);
        }
        else if(gamepad2.right_bumper && !gamepad2.left_bumper)
        {
            m_hardware.collectMotor.setPower(0.4);
        }
        else
        {
            m_hardware.collectMotor.setPower(0);
        }

    }

    private void straightdriving()
    {
        if (gamepad1.right_bumper)
        {
            m_hardware.leftMotor.setPower(0.5);
            m_hardware.rightMotor.setPower(0.5);
        }
        else if (gamepad1.left_bumper)
        {
            m_hardware.leftMotor.setPower(-0.5);
            m_hardware.rightMotor.setPower(-0.5);
        }

    }

    private void shooting()
    {
        if (gamepad2.b)
        {
            m_shootingbool = !m_shootingbool;
            if (m_shootingbool == true)
            {
                m_shootingstate = m_shootingstate.sWarming;
            }

            else
            {
                m_shootingstate = m_shootingstate.sStop;
            }

        }

        switch (m_shootingstate)
        {
            case sStop:
                sfStop();
                break;
            case sWarming:
                sfWarming();
                break;
            case sShooting:
                sfShooting();
                break;
        }
    }

    private void capball()
    {
        if(gamepad1.y)
        {
            m_capballstate = m_capballstate.sLock;
        }
        switch (m_capballstate)
        {
            case sLock:
                sfLock();
                break;
            case sLift:
                sfLift(cap_Power);
                break;

        }
    }

    private void movingball()
    {
            if (gamepad2.y)
            {

                m_hardware.shootingServo.setPosition(0);
            }
            else
            {
                m_hardware.shootingServo.setPosition(0.2);
            }
    }

    private void changedirection()
    {
        if (gamepad1.left_bumper)
        {
            m_driveboolean = !m_driveboolean;
            if (m_driveboolean = false)
            {
                m_hardware.leftMotor.setDirection(DcMotor.Direction.REVERSE);
                m_hardware.rightMotor.setDirection(DcMotor.Direction.FORWARD);
            }
            else
            {
                m_hardware.leftMotor.setDirection(DcMotor.Direction.FORWARD);
                m_hardware.rightMotor.setDirection(DcMotor.Direction.REVERSE);
            }
        }
    }

    private void sfStop()
    {
        m_hardware.shootingMotor.setPower(0);
    }

    private void sfWarming()
    {
        m_hardware.shootingMotor.setPower(1);
        m_shootingtime.reset();
        m_shootingtime.startTime();
        if (m_shootingtime.milliseconds() > 1000)
        {
            m_shootingstate = m_shootingstate.sShooting;
        }
    }

    private void sfShooting()
    {
        movingball();
    }



    private void sfLock()
    {
        m_hardware.capBallServo.setPosition(0);
        m_capballstate = m_capballstate.sLift;
    }

    private void sfLift(double p_cap_power)
    {
        if (p_cap_power > 0.4 || p_cap_power < -0.2)
        {
            m_hardware.capBallMotor.setPower(p_cap_power);
        }

    }
}