CompetitionCodeV4.1

#pragma config(Motor,  port1,           claw2,         tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           rightMotor,    tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port3,           leftMotor,     tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port4,           leftLift1,     tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port5,           leftLift2,     tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port6,           leftLift3,     tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port7,           rightLift1,    tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port8,           rightLift2,    tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port9,           rightLift3,    tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port10,          claw,          tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// This code is for the VEX cortex platform
#pragma platform(VEX2)

// Select Download method as "competition"
#pragma competitionControl(Competition)

#pragma autonomousDuration(15)
#pragma userControlDuration(120)

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

//basic void methods
	void grab(){
		motor[claw] = 80;
		motor[claw2] = 80;
	}

	void drop(){
		motor[claw] = -80;
		motor[claw2]= -80;
	}

	void forward() {//moves robot forward
		motor[leftMotor] = -127;
		motor[rightMotor] = -127;
	}

	void backward(){//moves robot backwards
		motor[leftMotor] = 127;
		motor[rightMotor] = 127;
	}

	void left(){//rotates the robot right
		motor[leftMotor] = -127;
		motor[rightMotor] = 127;
	}

	void right(){//rotates the robot left
		motor[leftMotor] = 127;
		motor[rightMotor] = -127;
	}
	void stopLifters(){ //stops lifters
		motor[leftLift1] = 0;
		motor[leftLift2] = 0;
		motor[leftLift3] = 0;
		motor[rightLift1] = 0;
		motor[rightLift2] = 0;
		motor[rightLift3] = 0;
	}

	void lift(){ //activates arm
		//	while(SensorValue(highButton) == 0){
		motor[leftLift1] = -127;
		motor[leftLift2] = -127;
		motor[leftLift3] = -127;
		motor[rightLift1] = -127;
		motor[rightLift2] = -127;
		motor[rightLift3] = -127;
		//	}
		/*	motor[leftLift1] = 0;
		motor[leftLift2] = 0;
		motor[leftLift3] = 0;
		motor[rightLift1] = 0;
		motor[rightLift2] = 0;
		motor[rightLift3] = 0;*/
	}

	void stopClaw(){
		motor[claw2]=0;
		motor[claw]=0;
	}

	void unLift(){ //unlifts
		//while(SensorValue(lowButton) == 0){
		motor[leftLift1] = 127;
		motor[leftLift2] = 127;
		motor[leftLift3] = 127;
		motor[rightLift1] = 127;
		motor[rightLift2] = 127;
		motor[rightLift3] = 127;
	}

	void hang(){ //hangs over the thing
		motor[leftLift1] = -80;
		motor[leftLift2] = -80;
		motor[leftLift3] = -80;
		motor[rightLift1] = -80;
		motor[rightLift2] = -80;
		motor[rightLift3] = -80;
	}

	void stopMotors(){ //STOPS ALL MOTORS
		motor[leftMotor] = 0;
		motor[rightMotor] = 0;
	}

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


	// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
	// used by the competition include file, for example, you might want
	// to display your team name on the LCD in this function.
	// bDisplayCompetitionStatusOnLcd = false;

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

task autonomous(){
	//auton code here
stopClaw();
	clearTimer(T1);
	while(time1[T1]<650){
		grab();
	}
	stopClaw();
	clearTimer(T2);
	while(time1[T2]<2600){
		forward();
	}
	stopMotors();
	unLift();
	for(int i=0; i<4; i++){
		clearTimer(T3);
		while(time1[T3]<500){
			forward();
		}
		clearTimer(T3);
		while(time1[T3]<300){
			stopMotors();
		}
		clearTimer(T4);
		while(time1[T4]<500){
			backward();
		}
		clearTimer(T3);
		while(time1[T3]<500){
			stopMotors();
		}
		stopLifters();
	}
}

task usercontrol(){
  // User control code here, inside the loop

  while (true){
  	while(vexRT[Btn7U] == 1){
			forward();
		}
		while(vexRT[Btn7L] == 1){
			left();
		}
		while(vexRT[Btn7R] == 1){
			right();
		}
		while(vexRT[Btn7D] == 1){
			backward();
		}
		/*	if(vexRT[Btn5D] == 1){
		clearTimer(T1);
		while(time1[T1]<1000){
		lift();
		}
		stopLifters();
		}
		if(vexRT[Btn5U] == 1){
		clearTimer(T1);
		while(time1[T1]<1100){
		unLift();
		}
		stopLifters();
		}*/
		if(vexRT[Btn8U] == 1){
			clearTimer(T1);
			while(time1[T1]<100){
				unLift();
			}
			stopLifters();
		}
		if(vexRT[Btn8D] == 1){
			clearTimer(T1);
			while(time1[T1]<100){
				lift();
			}
			stopLifters();
		}
		if(vexRT[Btn8L] == 1){
			stopLifters();
		}

		if(vexRT[Btn6D] == 1){
			stopClaw();
			clearTimer(T3);
			while(time1[T3]<50){
				drop();
			}
			stopClaw();
		}

		if(vexRT[Btn6U] == 1){
			grab();
			/*clearTimer(T3);
			while(time1[T3]<10){
				grab();
			}
			stopClaw();*/
		}

		stopMotors();
	}
}