AthenaAutoRed

package com.qualcomm.ftcrobotcontroller.opmodes;

import com.qualcomm.ftccommon.DbgLog;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorController;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;
import com.qualcomm.robotcore.util.ElapsedTime;



//------------------------------------------------------------------------------
//
// PushBotHardware
//

/**
 * Provides a single hardware access point between custom op-modes and the
 * OpMode class for the Push Bot.
 *
 * This class prevents the custom op-mode from throwing an exception at runtime.
 * If any hardware fails to map, a warning will be shown via telemetry data,
 * calls to methods will fail, but will not cause the application to crash.
 *
 * @author SSI Robotics
 * @version 2015-08-13-20-04
 *
 * Modified by Labanya Mukhopadhyay
 * Athena FTC team, EVHS,  San Jose
 */
//public class AthenaBotManual extends OpMode
public class AthenaBotAuto_Red_2 extends AthenaBotManual_1

{
    //--------------------------------------------------------------------------
    //
    // PushBotHardware
    //
    final static int ENCODER_CPR = 1440; //encoder counts per revolution
    final static double GEAR_RATIO = 2.0; //gear ratio
    final static double WHEEL_DIAMETER = 4.0; //diameter of wheel in inches
    final static double WHEEL_SEPARATION = 17.75;  // Distance between two front(or back) wheels

    final static double CIRCUMFERENCE = Math.PI * WHEEL_DIAMETER;

    final static double D1 = 59.0;  // Goes forward
    //final static double R1_ANGLE = 92.0; // right turn
    final static double L1_ANGLE = 92.0; // left turn
    final static double D2 = 56.0; // goes forward
    final static double D3 = 12.0;  // goes back
    final static double L2_ANGLE = 90.0; // left turn
    final static double D4 = 18.0;  // goes forward to park


    static double distance; //distance to drive in inches
    static double rotations;   // = DISTANCE/CIRCUMFERENCE;
    static double counts;      // = ENCODER_CPR*ROTATIONS*GEAR_RATIO;



    public double get_encoder_counts_dist(double dist)
    {


        distance = dist;
        rotations = distance / CIRCUMFERENCE;
        counts = ENCODER_CPR * rotations * GEAR_RATIO;

        return counts;
    }

    public boolean drive_straight_forward(double dist)
    {
        boolean l_state = false;
        // Run with encoders when both right and left motors have encoders, else run without encoders.
        run_without_drive_encoders();

        //
        // Start the drive wheel motors at full power.
        // GO FORWARD

        set_drive_power (1.0f, 1.0f);

        //
        // Have the motor shafts turned the required amount?
        //

        if (have_drive_encoders_reached (get_encoder_counts_dist(dist), get_encoder_counts_dist(dist))) {
            //
            // Reset the encoders to ensure they are at a known good value.
            //
            reset_drive_encoders();

            //
            // Stop the motors.
            //
            set_drive_power(0.0f, 0.0f);
            l_state = true;
        }
        return l_state;
    }

    public boolean drive_straight_backward(double dist)
    {
        boolean l_state = false;
        // Run with encoders when both right and left motors have encoders, else run without encoders.
        run_without_drive_encoders();

        //
        // Start the drive wheel motors at full power.
        // GO FORWARD

        set_drive_power (-1.0f, -1.0f);

        //
        // Have the motor shafts turned the required amount?
        //

        if (have_drive_encoders_reached (get_encoder_counts_dist(dist), get_encoder_counts_dist(dist))) {
            //
            // Reset the encoders to ensure they are at a known good value.
            //
            reset_drive_encoders();

            //
            // Stop the motors.
            //
            set_drive_power(0.0f, 0.0f);
            l_state = true;
        }

        return l_state;
    }

    public boolean turn_right(double degrees)
    {
        boolean l_state = false;

        // Run with encoders when both right and left motors have encoders, else run without encoders.
        run_without_drive_encoders();

        //
        // Start the drive wheel motors at full power.
        // GO FORWARD

        set_drive_power (1.0f, -1.0f);

        // Calculate distance for the required degree turn
        double turn_dist = (degrees * Math.PI * WHEEL_SEPARATION)/360.0;

        //
        // Have the motor shafts turned the required amount?
        //

        if (have_drive_encoders_reached (get_encoder_counts_dist(turn_dist), get_encoder_counts_dist(turn_dist))) {
            //
            // Reset the encoders to ensure they are at a known good value.
            //
            reset_drive_encoders();

            //
            // Stop the motors.
            //
            set_drive_power(0.0f, 0.0f);
            l_state = true;
        }
        return l_state;
    }

    public boolean turn_left(double degrees)
    {
        boolean l_state = false;

        // Run with encoders when both right and left motors have encoders, else run without encoders.
        run_without_drive_encoders();

        //
        // Start the drive wheel motors at full power.
        // GO FORWARD

        set_drive_power (-1.0f, 1.0f);

        // Calculate distance for the required degree turn
        double turn_dist = (degrees * Math.PI * WHEEL_SEPARATION)/360.0;

        //
        // Have the motor shafts turned the required amount?
        //

        if (have_drive_encoders_reached (get_encoder_counts_dist(turn_dist), get_encoder_counts_dist(turn_dist))) {
            //
            // Reset the encoders to ensure they are at a known good value.
            //
            reset_drive_encoders();

            //
            // Stop the motors.
            //
            set_drive_power(0.0f, 0.0f);
            l_state = true;
        }
        return l_state;
    }

    public void drop_climbers()
    {

        //this.resetStartTime();
        mStateTime.reset();

        for(double position=1.0;position>=0.0; position-=0.1) {

            v_servo_right_fling.setPosition (position);

            //if (this.getRuntime() > 0.1) { //this will wait 0.1 second;
            if (mStateTime.time() >= 0.1) { //this will wait 0.1 second;
                //code to run once wait is complete
                //this.resetStartTime();
                mStateTime.reset();
            }
        }

        // Wait for 1.0 sec after climbers drop
        if (mStateTime.time() >= 1.0) { //this will wait 1.0 second;
            //code to run once wait is complete
            mStateTime.reset();
        }

    }

    public void lift_right_fling()
    {

        //this.resetStartTime();
        mStateTime.reset();

        for(double position=0.0;position>=1.0; position+=0.1) {

            v_servo_right_fling.setPosition (position);

            //if (this.getRuntime() > 0.1) { //this will wait 0.1 second;
            if (mStateTime.time() >= 0.1) { //this will wait 0.1 second;
                //code to run once wait is complete
                //this.resetStartTime();
                mStateTime.reset();
            }
        }

    }
    /**
     * Construct the class.
     *
     * The system calls this member when the class is instantiated.
     */



    public AthenaBotAuto_Red_2()

    {
        //
        // Initialize base classes.
        //
        // All via self-construction.

        //
        // Initialize class members.
        //
        // All via self-construction.

    } // PushBotHardware

    //--------------------------------------------------------------------------
    //
    // init
    //
    /**
     * Perform any actions that are necessary when the OpMode is enabled.
     *
     * The system calls this member once when the OpMode is enabled.
     */
    @Override public void init ()

    {
        super.init();

        // Initialize the arm elbow servo
        init_arm();

        //this.resetStartTime();


    } // init

    //--------------------------------------------------------------------------
    //
    // start
    //
    /**
     * Perform any actions that are necessary when the OpMode is enabled.
     *
     * The system calls this member once when the OpMode is enabled.
     */
    @Override public void start ()

    {
        //
        // Only actions that are common to all Op-Modes (i.e. both automatic and
        // manual) should be implemented here.
        //
        // This method is designed to be overridden.
        //
        //
        // Call the PushBotHardware (super/base class) start method.
        //
        super.start ();

        //
        // Reset the motor encoders on the drive wheels.
        //
        reset_drive_encoders ();

    } // start

    //--------------------------------------------------------------------------
    //
    // loop
    /**
     * Implement a state machine that controls the robot during auto-operation.
     * The state machine uses a class member and encoder input to transition
     * between states.
     *
     * The system calls this member repeatedly while the OpMode is running.
     */
    @Override public void loop ()

    {

//----------------------------------------------------------------------
        //
        // State: Initialize (i.e. state_0).
        //
        switch (v_state)
        {
            //
            // Synchronize the state machine and hardware.

            // SETUP

            case 0:
                //
                // Reset the encoders to ensure they are at a known good value.
                //
                reset_drive_encoders ();

                //
                // Transition to the next state when this method is called again.
                //
                v_state++;

                break;
            //
            // Drive forward until the encoders exceed the specified values.

            // DRIVE FORWARD

            case 1:
                //
                // Tell the system that motor encoders will be used.  This call MUST
                // be in this state and NOT the previous or the encoders will not
                // work.  It doesn't need to be in subsequent states.
                //
                // run_using_encoders ();

                // Raise the arm elbow, so it won't hit the beacon while dropping climbers
                //raise_arm();

                if(drive_straight_forward(D1))
                {

                    //
                    // Transition to the next state when this method is called
                    // again.
                    //
                    v_state++;
                }
                break;
            //
            // Wait...
            //
            case 2:
                if (have_drive_encoders_reset ())
                {
                    v_state++;
                }
                break;
            //
            // Turn left until the encoders exceed the specified values.
            //
            case 3:
                // run_using_encoders ();
                // TURN RIGHT
                //run_without_drive_encoders();
                //set_drive_power (1.0f, -1.0f);

                //int d_2 = 800;

                if (turn_left (L1_ANGLE))
                {

                    v_state++;
                }
                break;
            //
            // Wait...
            //
            case 4:
                if (have_drive_encoders_reset ())
                {
                    v_state++;
                }
                break;
            //
            // Turn right until the encoders exceed the specified values.
            //
            case 5:
                // run_using_encoders ();
                //run_without_drive_encoders();

                //int d_3 = 6000;
                //set_drive_power (1.0f, 1.0f);
                if (drive_straight_forward (D2))
                {
                    //reset_drive_encoders ();
                    //set_drive_power (0.0f, 0.0f);
                    v_state++;
                }

                break;
                /* boolean has_reached_white_line = false;
                if (has_reached_white_line) {
                    v_state++;
                } */
            //
            // Wait...
            //
            case 6:
                if (have_drive_encoders_reset ())
                {
                    v_state++;
                }
                break;

            case 7:
                //v_servo_right_fling.setPosition (0.0);
                drop_climbers();
                v_state++;
                //
                // Perform no action - stay in this case until the OpMode is stopped.
                // This method will still be called regardless of the state machine.
                //
            case 8:
                //v_servo_right_fling.setPosition (1.0);
                lift_right_fling();
                v_state++;

            case 9:
                if (drive_straight_backward(D3))
                {
                    //reset_drive_encoders ();
                    //set_drive_power (0.0f, 0.0f);
                    v_state++;
                }
                break;
            case 10:
                if (have_drive_encoders_reset ())
                {
                    v_state++;
                }
                break;
            case 11:

                if (turn_left (L2_ANGLE))
                {
                    v_state++;
                }
                break;
            //
            // Wait...
            //
            case 12:
                if (have_drive_encoders_reset ())
                {
                    v_state++;
                }
                break;
            //
            // Turn right until the encoders exceed the specified values.
            //
            case 13:
                // run_using_encoders ();
                //run_without_drive_encoders();

                // Park the robot
                //set_drive_power (1.0f, 1.0f);
                if (drive_straight_forward (D4))
                {
                    //reset_drive_encoders ();
                    //set_drive_power (0.0f, 0.0f);
                    v_state++;
                }

                break;
                /* boolean has_reached_white_line = false;
                if (has_reached_white_line) {
                    v_state++;
                } */
            //
            // Wait...
            //
            case 14:
                if (have_drive_encoders_reset ())
                {
                    v_state++;
                }
                break;

            default:
                //
                // The autonomous actions have been accomplished (i.e. the state has
                // transitioned into its final state.
                //
                break;
        }

        //
        // Send telemetry data to the driver station.
        //
        update_telemetry (); // Update common telemetry
        telemetry.addData("23", "State: " + v_state);
        //update_gamepad_telemetry ();

    } // loop

    //--------------------------------------------------------------------------
    //
    // stop
    //
    /**
     * Perform any actions that are necessary when the OpMode is disabled.
     *
     * The system calls this member once when the OpMode is disabled.
     */
    @Override public void stop ()
    {
        //
        // Nothing needs to be done for this method.
        //

    } // stop


    //--------------------------------------------------------------------------
    //
    // v_state
    //
    /**
     * This class member remembers which state is currently active.  When the
     * start method is called, the state will be initialized (0).  When the loop
     * starts, the state will change from initialize to state_1.  When state_1
     * actions are complete, the state will change to state_2.  This implements
     * a state machine for the loop method.
     */
    private int v_state = 0;

    private ElapsedTime mStateTime = new ElapsedTime();

} // AthenaBotAuto