LCD autonomous selection

/** @file opcontrol.c
 * @brief File for operator control code
 *

#include "main.h"
#include "coolTeam.h"

/*
 * Runs the user operator control code. This function will be started in its own task with the
 * default priority and stack size whenever the robot is enabled via the Field Management System
 * or the VEX Competition Switch in the operator control mode. If the robot is disabled or
 * communications is lost, the operator control task will be stopped by the kernel. Re-enabling
 * the robot will restart the task, not resume it from where it left off.
 *
 * If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
 * run the operator control task. Be warned that this will also occur if the VEX Cortex is
 * tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
 *
 * Code running in this task can take almost any action, as the VEX Joystick is available and
 * the schedular is operational. However, proper use of delay() or taskDelayUntil() is highly
 * recommended to give other tasks (including system tasks such as updating LCDs) time to run.
 *
 * This task should never exit; it should end with some kind of infinite loop, even if empty.
 */
void operatorControl() {

    while(true) {
        const short leftButton = 1;
        const short centerButton = 2;
        const short rightButton = 4;

        //Wait for Press--------------------------------------------------
        void waitForPress()
        {
            while(lcdReadButtons(uart1) == 0){
                wait(2000);
            }
        }
        //----------------------------------------------------------------

        //Wait for Release------------------------------------------------
        void waitForRelease()
        {
            while(lcdReadButtons(uart1) != 0){
                wait(2000);
            }
        }
        //----------------------------------------------------------------


        //Declare count variable to keep track of our choice
        int count = 0;

        //------------- Beginning of User Interface Code ---------------
        //Clear LCD
        lcdClear(uart1);
        //Loop while center button is not pressed
        while(lcdReadButtons(uart1) != centerButton) {
            //Switch case that allows the user to choose from 4 different options
            switch(count){
            case 0:
                //Display first choice
                lcdSetText(uart1, 1, "Autonomous 1");
                lcdSetText(uart1, 2, "<Enter>");
                waitForPress();
                //Increment or decrement "count" based on button press
                if(lcdReadButtons(uart1) == leftButton)
                {
                    waitForRelease();
                    count = 3;
                }
                else if(lcdReadButtons(uart1) == rightButton)
                {
                    waitForRelease();
                    count++;
                }
                break;
            case 1:
                //Display second choice
                lcdSetText(uart1, 1, "Autonomous 2");
                lcdSetText(uart1, 2, "<Enter>");
                waitForPress();
                //Increment or decrement "count" based on button press
                if(lcdReadButtons(uart1) == leftButton)
                {
                    waitForRelease();
                    count--;
                }
                else if(lcdReadButtons(uart1) == rightButton)
                {
                    waitForRelease();
                    count++;
                }
                break;
            case 2:
                //Display third choice
                lcdSetText(uart1, 1, "Autonomous 3");
                lcdSetText(uart1, 2, "<Enter>");
                waitForPress();
                //Increment or decrement "count" based on button press
                if(lcdReadButtons(uart1) == leftButton)
                {
                    waitForRelease();
                    count--;
                }
                else if(lcdReadButtons(uart1) == rightButton)
                {
                    waitForRelease();
                    count++;
                }
                break;
            case 3:
                //Display fourth choice
                lcdSetText(uart1, 1, "Autonomous 4");
                lcdSetText(uart1, 2, "<Enter>");
                waitForPress();
                //Increment or decrement "count" based on button press
                if(lcdReadButtons(uart1) == leftButton)
                {
                    waitForRelease();
                    count--;
                }
                else if(lcdReadButtons(uart1) == rightButton)
                {
                    waitForRelease();
                    count = 0;
                }
                break;
            default:
                count = 0;
                break;

            }
            //------------- End of User Interface Code ---------------------

            //------------- Beginning of Robot Movement Code ---------------
            //Clear LCD
            lcdClear(uart1);
            //Switch Case that actually runs the user choice
            switch(count){
            case 0:
                //If count = 0, run the code correspoinding with choice 1
                lcdSetText(uart1, 1, "Autonomous 1");
                lcdSetText(uart1, 2, "is running");
                wait(2000);                        // Robot waits for 2000 milliseconds

                // initializes encoders
                Encoder right = encoderInit(5, 6, false);

                //wait for 0.5 seonds
                wait(750);

                //robot drives towards the driver while intaking buckyballs
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 22) {
                    //base
                    motorSet(2, 70);
                    motorSet(3, 80);
                    motorSet(4, 70);
                    motorSet(5, 80);

                    //intake
                    motorSet(1, -127);
                    motorSet(10, -127);
                }

                //wait for 0.2 seconds
                wait(200);

                //resets the encoder value to zero
                encoderReset(right);

                //robot drives backwards from the driver
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 22) {
                    //base
                    motorSet(2, -70);
                    motorSet(3, -80);
                    motorSet(4, -70);
                    motorSet(5, -80);
                }
                //stops all the motors
                motorStopAll();

                //rotate the robot 90 degrees(
                wait(2000);

                //lowers ball manipulator to the ground for 1.1 seconds
                motorSet(8, -100);
                motorSet(9, -100);
                wait(1100);
                motorStopAll();

                //wait for 0.2 seconds
                wait(200);

                //resets the encoder value to zero
                encoderReset(right);


                //robot drives backwards
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 9) {
                    //base
                    motorSet(2, -70);
                    motorSet(3, -80);
                    motorSet(4, -70);
                    motorSet(5, -80);
                }

                //lifts ball manipulator for 0.55 seconds
                motorSet(8, 100);
                motorSet(9, 100);
                wait(550);
                motorStopAll();

                //resets the encoder value to zero
                encoderReset(right);

                //robot turns left
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < (ROBOT_CURCUMFIRENCE/4) + 4) {
                    motorSet(2, -100);
                    motorSet(3, 100);
                    motorSet(4, -100);
                    motorSet(5, 100);
                }
                //wait for 0.2 seconds
                wait(200);

                //resets the encoder value to zero
                encoderReset(right);

                //robot drives backwards
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 30) {
                    motorSet(2, -70);
                    motorSet(3, -80);
                    motorSet(4, -70);
                    motorSet(5, -80);
                }

                //wait for 0.2 seconds
                wait(200);

                //resets the encoder value to zero
                encoderReset(right);

                //robot turns left
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < (ROBOT_CURCUMFIRENCE/2)) {
                    motorSet(2, -70);
                    motorSet(3, 80);
                    motorSet(4, -70);
                    motorSet(5, 80);
                }

                //releases the buckyballs from the robot
                motorSet(1, 127);
                motorSet(10, 127);

                //lifts the big ball manipulator for 0.9 seconds
                motorSet(8, 127);
                motorSet(9, 127);
                wait(900);
                motorStop(8);
                motorStop(9);

                //lowers the big ball manipulator for 0.9 seconds
                motorSet(8, -127);
                motorSet(9, -127);
                wait(600);
                motorStop(8);
                motorStop(9);
                //wait(1000);                           // Robot runs previous code for 3000 milliseconds before moving on
                break;
            case 1:
                //If count = 1, run the code correspoinding with choice 2
                lcdSetText(uart1, 1, "Autonomous 2");
                lcdSetText(uart1, 2, "is running");
                wait(2000);                        // Robot waits for 2000 milliseconds

                // Move reverse at full power for 3 seconds
                // initializes encoders
                // Encoder right = encoderInit(5, 6, false);

                //wait for 0.5 seonds
                wait(750);


                //lifts the ball manipulator for 0.19 seconds
                motorSet(8, -100);
                motorSet(9, -100);
                wait(190);
                motorStopAll();

                //robot drives towards barrier
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 28) {
                    //base
                    motorSet(2, 70);
                    motorSet(3, 80);
                    motorSet(4, 70);
                    motorSet(5, 80);
                }
                //stops all motors
                motorStopAll();

                //wait for 0.2 seconds
                wait(200);

                //resets the encoder value to zero
                encoderReset(right);

                //releases bucky balls for 1 second
                motorSet(1, 127);
                motorSet(10, 127);
                wait(1000);
                motorStopAll();



                //robot drives backwards towards bumpf
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 28) {
                    motorSet(2, -70);
                    motorSet(3, -80);
                    motorSet(4, -70);
                    motorSet(5, -80);
                }
                //stops all motor
                motorStopAll();

                //rotate the robot about 45 degrees
                wait(1250);

                //resets the encoder value to zero
                encoderReset(right);

                //wait 0.2 seconds
                wait(200);

                //lifts the arm for 1 second
                motorSet(6, 127);
                motorSet(7, 127);
                wait(1000);
                motorStopAll();

                //robot drives bacwards towards bump
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 40) {
                    // base
                    motorSet(2, 70);
                    motorSet(3, 80);
                    motorSet(4, 70);
                    motorSet(5, 80);
                }
                //wait for 0.2 seconds
                wait(200);

                //resets the encoder value to zero
                encoderReset(right);

                //robot drives bacwards towards bump
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 40) {
                    //base
                    motorSet(2, -70);
                    motorSet(3, -80);
                    motorSet(4, -70);
                    motorSet(5, -80);
                }
                    // Robot runs previous code for 3000 milliseconds before moving on
                    break;
            case 2:
                //If count = 2, run the code correspoinding with choice 3
                lcdSetText(uart1, 1, "Autonomous 3");
                lcdSetText(uart1, 2, "is running");
                wait(2000);                        // Robot waits for 2000 milliseconds

                //Turn right for 3seconds
                // initializes encoders
                //Encoder right = encoderInit(5, 6, false);

                //wait for 0.5 seonds
                wait(750);


                //lifts the ball manipulator for 0.19 seconds
                motorSet(8, -100);
                motorSet(9, -100);
                wait(190);
                motorStopAll();

                //robot drives towards barrier
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 28) {
                    //base
                    motorSet(2, 70);
                    motorSet(3, 80);
                    motorSet(4, 70);
                    motorSet(5, 80);
                }
                //stops all motors
                motorStopAll();

                //wait for 0.2 seconds
                wait(200);

                //resets the encoder value to zero
                encoderReset(right);

                //releases bucky balls for 1 second
                motorSet(1, 127);
                motorSet(10, 127);
                wait(1000);
                motorStopAll();



                //robot drives backwards towards bumpf
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 28) {
                    motorSet(2, -70);
                    motorSet(3, -80);
                    motorSet(4, -70);
                    motorSet(5, -80);
                }
                //stops all motor
                motorStopAll();

                //rotate the robot about 45 degrees
                wait(1250);

                //resets the encoder value to zero
                encoderReset(right);

                //wait 0.2 seconds
                wait(200);

                //lifts the arm for 1 second
                motorSet(6, 127);
                motorSet(7, 127);
                wait(1000);
                motorStopAll();

                //robot drives bacwards towards bump
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 40) {
                    // base
                    motorSet(2, 70);
                    motorSet(3, 80);
                    motorSet(4, 70);
                    motorSet(5, 80);
                }
                //wait for 0.2 seconds
                wait(200);

                //resets the encoder value to zero
                encoderReset(right);

                //robot drives bacwards towards bump
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 40) {
                    //base
                    motorSet(2, -70);
                    motorSet(3, -80);
                    motorSet(4, -70);
                    motorSet(5, -80);
                }

            case 3:
                //If count = 3, run the code correspoinding with choice 4
                lcdSetText(uart1, 1, "Autonomous 4");
                lcdSetText(uart1, 2, "is running");
                wait(2000);                        // Robot waits for 2000 milliseconds

                // initializes encoders
                //Encoder right = encoderInit(5, 6, false);

                //wait for 0.5 seonds
                wait(750);

                //robot drives towards the driver while intaking buckyballs
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 22) {
                    //base
                    motorSet(2, 70);
                    motorSet(3, 80);
                    motorSet(4, 70);
                    motorSet(5, 80);

                    //intake
                    motorSet(1, -127);
                    motorSet(10, -127);
                }

                //wait for 0.2 seconds
                wait(200);

                //resets the encoder value to zero
                encoderReset(right);

                //robot drives backwards from the driver
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 22) {
                    //base
                    motorSet(2, -70);
                    motorSet(3, -80);
                    motorSet(4, -70);
                    motorSet(5, -80);
                }
                //stops all the motors
                motorStopAll();

                //rotate the robot 90 degrees(
                wait(2000);

                //lowers ball manipulator to the ground for 1.1 seconds
                motorSet(8, -100);
                motorSet(9, -100);
                wait(1100);
                motorStopAll();

                //wait for 0.2 seconds
                wait(200);

                //resets the encoder value to zero
                encoderReset(right);


                //robot drives backwards
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 9) {
                    //base
                    motorSet(2, -70);
                    motorSet(3, -80);
                    motorSet(4, -70);
                    motorSet(5, -80);
                }

                //lifts ball manipulator for 0.55 seconds
                motorSet(8, 100);
                motorSet(9, 100);
                wait(550);
                motorStopAll();

                //resets the encoder value to zero
                encoderReset(right);

                //robot turns right
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < (ROBOT_CURCUMFIRENCE/4) + 4) {
                    motorSet(2, 100);
                    motorSet(3, -100);
                    motorSet(4, 100);
                    motorSet(5, -100);
                }
                //wait for 0.2 seconds
                wait(200);

                //resets the encoder value to zero
                encoderReset(right);

                //robot drives backwards
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < 30) {
                    motorSet(2, -70);
                    motorSet(3, -80);
                    motorSet(4, -70);
                    motorSet(5, -80);
                }

                //wait for 0.2 seconds
                wait(200);

                //resets the encoder value to zero
                encoderReset(right);

                //robot turns right
                while ((abs(encoderGet(right)) / ENCODER_COEFFICIENT) < (ROBOT_CURCUMFIRENCE/2)) {
                    motorSet(2, 70);
                    motorSet(3, -80);
                    motorSet(4, 70);
                    motorSet(5, -80);
                }

                //releases the buckyballs from the robot
                motorSet(1, 127);
                motorSet(10, 127);

                //lifts the big ball manipulator for 0.9 seconds
                motorSet(8, 127);
                motorSet(9, 127);
                wait(900);
                motorStop(8);
                motorStop(9);

                //lowers the big ball manipulator for 0.9 seconds
                motorSet(8, -127);
                motorSet(9, -127);
                wait(600);
                motorStop(8);
                motorStop(9);
                //wait(1000);                          // Robot runs previous code for 3000 milliseconds before moving on
                break;
            default:
                lcdSetText(uart1, 1, "No valid choice");
                lcdSetText(uart1, 2, "was made.");
                break;
                }
}
}
}