BOONIES

#pragma config(Motor, port2, LWheel, tmotorServoContinuousRotation, openLoop) //Left Wheel Motor
#pragma config(Motor, port3, RWheel, tmotorServoContinuousRotation, openLoop, reversed) //Right Wheel Motor
#pragma config(Motor, port4, LLift1, tmotorServoContinuousRotation, openLoop) //Left Bottom Lift
#pragma config(Motor, port5, LLift2, tmotorServoContinuousRotation, openLoop) //Left Second Lift
#pragma config(Motor, port6, RLift1, tmotorServoContinuousRotation, openLoop, reversed) //Right Bottom Lift
#pragma config(Motor, port7, RLift2, tmotorServoContinuousRotation, openLoop, reversed) //Right Second Lift
#pragma config(Motor, port8, Claw1, tmotorServoStandard, openLoop) //Claw Motor1
#pragma config(Motor, port9, Claw2, tmotorServoStandard, openLoop, reversed) //Claw Motor2
//Three-wire motors: tmotorVex393
//Two-wire motors: tmotorServoContinuousRotation

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(105)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
    // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
    // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
    bStopTasksBetweenModes = true;

    // All activities that occur before the competition starts
    // Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

void stopWheels()
{
		motor[LWheel] = 0;
		motor[RWheel] = 0;	
}

void moveForward()
{
		ClearTimer(T1);
		while(time1[T1] < 1000) // 1 second: move forward
		{
				motor[RWheel] = 127;
				motor[LWheel] = 127;
		}
		stopWheels();
}

void moveBackward()
{
		ClearTimer(T1);
		while(time1[T1] < 1000) // 1 second: move backward
		{
				motor[RWheel] = -127;
				motor[LWheel] = -127;
		}
}

task autonomous()
{
    // Insert user code here.
    // AutonomousCodePlaceholderForTesting();  // Remove this function call once you have "real" code.

		// wait1Msec("insert milliseconds"); // Waits for X Milliseconds
		// random("insert maxvalue"); // Gets random number from 0 to X max

		// wait1Msec(random(5000)); // Wait up to 5 seconds
		// motor[RWheel] = random(127); // Chooses Random motor value between -127 and 127
		// time1["timer name"] = milliseconds, time10["timer name"] = centiseconds, time100["timer name"] = centiseconds

		////////////////////////////////
		//START OF AUTONOMOUS
		////////////////////////////////

		wait1Msec(1000); // Wait 1 second before starting autonomous
		moveForward(); // 1 second forward
		moveForward();

}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

int threshold = 8; // Threshold y-axis must pass to activate wheel motors. Default = 12.7
int clawlim = 120; // Threshold x-axis must pass to activate fubar motors.

task usercontrol() // User control code here, inside the loop
{
    while (true)
    {
        // This is the main execution loop for the user control program. Each time through the loop
        // your program should update motor + servo values based on feedback from the joysticks.

        // Insert user code here. This is where you use the joystick values to update your motors, etc.

        //UserControlCodePlaceholderForTesting(); // Remove this function call once you have "real" code.


        ////////////////////////////////
////////// Right wheel
        ////////////////////////////////
        if (abs(vexRT[Ch2]) > threshold) // If the left joystick is greater than or less than the threshold:
        {
            motor[RWheel] = (vexRT[Ch2]); // Left Joystick Y value / 2.
        }
        if (abs(vexRT[Ch2]) < threshold) // If the left joystick is within the threshold:
        {
            motor[RWheel] = 0; // Stop the left motor (cancel noise)
        }
        ////////////////////////////////

        ////////////////////////////////
////////// Left wheel
        ////////////////////////////////
        if (abs(vexRT[Ch3]) > threshold) // If the left joystick is greater than or less than the threshold:
        {
            motor[LWheel] = (vexRT[Ch3]); // Left Joystick Y value / 2.
        }
        if (abs(vexRT[Ch3]) < threshold) // If the left joystick is within the threshold:
        {
            motor[LWheel] = 0; // Stop the left motor (cancel noise)
        }
        ////////////////////////////////



        ////////////////////////////////
////////// Right Lifts
        ////////////////////////////////
        if(vexRT[Btn8U] == 1 || vexRT[Btn8D] == 1 || vexRT[Btn8L] == 1 || vexRT[Btn8R] == 1 || vexRT[Btn6U] == 1 || vexRT[Btn6D] == 1) // Test for right lift button press
        {
        		if(vexRT[Btn6U] == 1 || vexRT[Btn6D] == 1) // Test for right dual lift buttons
        		{
        				if(vexRT[Btn6U] == 1 && vexRT[Btn6D] == 1) // If both pressed: stops motors
        				{
		                motor[RLift1] = 0;
		               	motor[RLift2] = 0;
        				}
        				else
        				{
		      					if(vexRT[Btn6U] == 1) // right dual lifts up
		      					{
		                		motor[RLift1] = 127;
		               			motor[RLift2] = 127;
		      					}
		      					if(vexRT[Btn6D] == 1) // right dual lifts down
		      					{
		                		motor[RLift1] = -127;
		               			motor[RLift2] = -127;
		      					}
		      			}
        		}
        		else
        		{
        				if(vexRT[Btn8L] == 1 || vexRT[Btn8R] == 1) // Test for right bottom lift buttons
		        		{
		        				if(vexRT[Btn8L] == 1 && vexRT[Btn8R] == 1) // If both pressed: stops motors
		        				{
		                		motor[RLift1] = 0;
		        				}
		        				else
		        				{
				      					if(vexRT[Btn8L] == 1) // right bottom lift up
				      					{
				                		motor[RLift1] = 127;
				      					}
				      					if(vexRT[Btn8R] == 1) // right bottom lift down
				      					{
				                		motor[RLift1] = -127;
				      					}
				      			}
		        		}
		        		else
		        		{
		        				motor[RLift1] = 0; // if right bottom lift buttons are not in use: sets to 0
		        		}
        				if(vexRT[Btn8U] == 1 || vexRT[Btn8D] == 1) // Test for right top lift buttons
		        		{
		        				if(vexRT[Btn8U] == 1 && vexRT[Btn8D] == 1) // If both pressed: stops motors
		        				{
				                motor[RLift2] = 0;
			        			}
			        			else
			        			{
				      					if(vexRT[Btn8U] == 1) // right top lift up
				      					{
				                		motor[RLift2] = 127;
				      					}
				      					if(vexRT[Btn8D] == 1) // right top lift down
				      					{
				                		motor[RLift2] = -127;
				      					}
				      			}
		        		}
		        		else
		        		{
		        				motor[RLift2] = 0; // if right top lift buttons are not in use: sets to 0
		        		}
		        }
			  }
				else // no right lift button press
	      {
	          motor[RLift1] = 0;
	          motor[RLift2] = 0;
	      }
        ////////////////////////////////

				////////////////////////////////
////////// Left Lifts
        ////////////////////////////////
        if(vexRT[Btn7U] == 1 || vexRT[Btn7D] == 1 || vexRT[Btn7L] == 1 || vexRT[Btn7R] == 1 || vexRT[Btn5U] == 1 || vexRT[Btn5D] == 1) // Test for left lift button press
        {
        		if(vexRT[Btn5U] == 1 || vexRT[Btn5D] == 1) // Test for left dual lift buttons
        		{
        				if(vexRT[Btn5U] == 1 && vexRT[Btn5D] == 1) // If both pressed: stops motors
        				{
		                motor[LLift1] = 0;
		               	motor[LLift2] = 0;
        				}
        				else
        				{
		      					if(vexRT[Btn5U] == 1) // left dual lifts up
		      					{
		                		motor[LLift1] = 127;
		               			motor[LLift2] = 127;
		      					}
		      					if(vexRT[Btn5D] == 1) // left dual lifts down
		      					{
		                		motor[LLift1] = -127;
		               			motor[LLift2] = -127;
		      					}
		      			}
        		}
        		else
        		{
        				if(vexRT[Btn7L] == 1 || vexRT[Btn7R] == 1) // Test for left bottom lift buttons
		        		{
		        				if(vexRT[Btn7L] == 1 && vexRT[Btn7R] == 1) // If both pressed: stops motors
		        				{
				              	motor[LLift1] = 127;
		        				}
		        				else
		        				{
				      					if(vexRT[Btn7L] == 1) // left bottom lift up
				      					{
				                		motor[LLift1] = 127;
				      					}
				      					if(vexRT[Btn7R] == 1) // left bottom lift down
				      					{
				                		motor[LLift1] = -127;
				      					}
				      			}
		        		}
		        		else
		        		{
		        				motor[LLift1] = 0; // if left bottom lift buttons are not in use: sets to 0
		        		}
        				if(vexRT[Btn7U] == 1 || vexRT[Btn7D] == 1) // Test for left top lift buttons
		        		{
		        				if(vexRT[Btn7U] == 1 && vexRT[Btn7D] == 1) // If both pressed: stops motors
		        				{
				                motor[LLift2] = 0;
		        				}
		        				else
		        				{
				      					if(vexRT[Btn7U] == 1) // left top lift up
				      					{
				                		motor[LLift2] = 127;
				      					}
				      					if(vexRT[Btn7D] == 1) // left top lift down
				      					{
				                		motor[LLift2] = -127;
				      					}
				      			}
		        		}
		        		else
		        		{
		        				motor[LLift2] = 0; // if left top lift buttons are not in use: sets to 0
		        		}
		        }
			  }
				else // no left lift button press
	      {
	          motor[LLift1] = 0;
	          motor[LLift2] = 0;
	      }
        ////////////////////////////////

        ////////////////////////////////
////////// Claw Motor 1 + 2
        ////////////////////////////////
        if (abs(vexRT[Ch1]) > clawlim) // If the left joystick is greater than or less than the threshold:
        {
            motor[Claw1] = (vexRT[Ch2]);
            motor[Claw2] = (vexRT[Ch2]);
        }
        if (abs(vexRT[Ch1]) < clawlim) // If the left joystick is within the threshold:
        {
            motor[Claw1] = 0; // Stop the left motor (cancel noise)
            motor[Claw2] = 0; // Stop the left motor (cancel noise)
        }
        ////////////////////////////////

        ////////////////////////////////
////////// Claw Motor 1 + 2
        ////////////////////////////////
        /*if (abs(vexRT[Ch4]) > clawlim) // If the left joystick is greater than or less than the threshold:
        {
            motor[Claw1] = (vexRT[Ch2]);
            motor[Claw2] = (vexRT[Ch2]);
        }
        if (abs(vexRT[Ch4]) < clawlim) // If the left joystick is within the threshold:
        {
            motor[Claw1] = 0; // Stop the left motor (cancel noise)
            motor[Claw2] = 0; // Stop the left motor (cancel noise)
        }*/
        ////////////////////////////////
    } // while (true)
} // usercontrol()