VEX TESTING 4

#include "main.h"

/**
 * A callback function for LLEMU's center button.
 *
 * When this callback is fired, it will toggle line 2 of the LCD text between
 * "I was pressed!" and nothing.
 */
void on_center_button() {}

/**
 * Runs initialization code. This occurs as soon as the program is started.
 *
 * All other competition modes are blocked by initialize; it is recommended
 * to keep execution time for this mode under a few seconds.
 */
void initialize() {

}

/**
 * Runs while the robot is in the disabled state of Field Management System or
 * the VEX Competition Switch, following either autonomous or opcontrol. When
 * the robot is enabled, this task will exit.
 */
void disabled() {}

/**
 * Runs after initialize(), and before autonomous when connected to the Field
 * Management System or the VEX Competition Switch. This is intended for
 * competition-specific initialization routines, such as an autonomous selector
 * on the LCD.
 *
 * This task will exit when the robot is enabled and autonomous or opcontrol
 * starts.
 */
void competition_initialize() {}

/**
 * Runs the user autonomous 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 autonomous
 * mode. Alternatively, this function may be called in initialize or opcontrol
 * for non-competition testing purposes.
 *
 * If the robot is disabled or communications is lost, the autonomous task
 * will be stopped. Re-enabling the robot will restart the task, not re-start it
 * from where it left off.
 */
void autonomous() {}

/**
 * Runs the 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 no competition control is connected, this function will run immediately
 * following initialize().
 *
 * If the robot is disabled or communications is lost, the
 * operator control task will be stopped. Re-enabling the robot will restart the
 * task, not resume it from where it left off.
 */
 void opcontrol() {

 	pros::Motor front_r(20);
 	pros::Motor front_l(19);
 	pros::Motor back_r(14);
 	pros::Motor back_l(13);


 	pros:: Motor left_mtr(2);
 	pros:: Motor right_mtr(3);
 	pros::Motor lift_mtr (1);
 	pros::Motor pivot_mtr (4);
 	pros::Controller master(pros::E_CONTROLLER_MASTER);

 /*

 //intake
 if (master.get_digital(pros::E_CONTROLLER_DIGITAL_R1) == 1)
 {
 				 left_mtr.move(-120);
 				 right_mtr.move(120);
 			pros::delay(20);
 }

 while (master.get_digital(pros::E_CONTROLLER_DIGITAL_R2) == 1)
 {
 							 left_mtr.move(120);
 							 right_mtr.move(-120);
 						pros::delay(20);
 }
 if (pros::E_CONTROLLER_DIGITAL_R1 == 1 && pros::E_CONTROLLER_DIGITAL_R2  == 1)
 {

 	left_mtr.move(120);
 	right_mtr.move(-120);
 pros::delay(0);

 }



 */
 //lift


 while (true){
 	//////////////////////////////LIFT//////////////////////////////
 if(master.get_digital(pros::E_CONTROLLER_DIGITAL_X))
 {
 				 lift_mtr.move(-70);
 pros::delay(10);
 }
 else
 {
 			lift_mtr.move(0);
 			pros::delay(10);
 }

 if (master.get_digital(pros::E_CONTROLLER_DIGITAL_B))
 {
 lift_mtr.move(70);
 pros::delay(10);
 }
 else
 {
 lift_mtr.move(0);
 pros::delay(10);
 }


 	//////////////////////////////LIFT//////////////////////////////
 //////////////////////////////INTAKE//////////////////////////////
 if (master.get_digital(pros::E_CONTROLLER_DIGITAL_R1) == 1)
 {
 left_mtr.move(-120);
 right_mtr.move(120);
 pros::delay(20);
 }
 else
 {
	 left_mtr.move(0);
	 right_mtr.move(0);
			 pros::delay(10);
 }


 if (master.get_digital(pros::E_CONTROLLER_DIGITAL_R2) == 1)
 {
 left_mtr.move(120);
 right_mtr.move(-120);
 pros::delay(20);
 }
 else
 {
	 left_mtr.move(0);
	 right_mtr.move(0);
			 pros::delay(10);
 }

 //////////////////////////////INTAKE//////////////////////////////
 //////////////////////////////PIVOT ARM//////////////////////////////
 if (master.get_digital(pros::E_CONTROLLER_DIGITAL_DOWN) == 1)
 {
 pivot_mtr.move(-100);
 pros::delay(20);
 }

 else
 {
 pivot_mtr.move(0);
 pros::delay(20);
 }

 if (master.get_digital(pros::E_CONTROLLER_DIGITAL_UP) == 1) {
 pivot_mtr.move(100);
 pros::delay(20);
 }

 else {
 pivot_mtr.move(0);
 pros::delay(20);
 }
 //////////////////////////////PIVOT ARM//////////////////////////////

 //arms

 if (master.get_digital(pros::E_CONTROLLER_DIGITAL_DOWN) == 1) {
 				 pivot_mtr.move(-100);
 			pros::delay(20);
 			 }

 			 else {
 				 pivot_mtr.move(0);
 				pros::delay(20);

 			 }

 			 if  (master.get_digital(pros::E_CONTROLLER_DIGITAL_UP) == 1) {
 			 				 pivot_mtr.move(100);
 			 			pros::delay(20);
 			 			 }

 						 else {
 							 pivot_mtr.move(0);
 							pros::delay(20);
 						 }



 ///////////////////////DRIVE////////////////////////////////

 		int left  = master.get_analog(ANALOG_LEFT_Y);
 		int right = master.get_analog(ANALOG_RIGHT_Y);
 			front_r = -right;
 		back_r = -right;
 			front_l = left;
 			back_l = left;

 			pros::delay(20);
 ///////////////////////////////////////////////////////
 }
 }