KoreaCompetition

#pragma config(Motor,  port1,           claw2,         tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           frontRight,    tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port3,           backRight,     tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port4,           frontLeft,     tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port5,           backLeft,      tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port6,           leftLift1,     tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port7,           leftLift2,     tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port8,           rightLift1,    tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port9,           rightLift2,    tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port10,          claw,          tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#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"

void autonomous(){
	stopClaw();
	clearTimer(T1);
	while(time1[T1]<150){
		grab();
	}
	stopClaw();
	unLift();
	clearTimer(T2);
	while(time1[T2]<4000){
		forward();
	}
	stopMotors();
	for(int i=0; i<4; i++){
		clearTimer(T3);
		while(time1[T3]<500){
			forward();
		}
		clearTimer(T3);
		while(time1[T3]<300){
			stopMotors();
		}
		clearTimer(T3);
		while(time1[T3]<400){
			grab();
		}
		stopClaw();
	  clearTimer(T3);
		while(time1[T3]<400){
			drop();
		}
		clearTimer(T4);
		while(time1[T4]<500){
			backward();
		}
		clearTimer(T3);
		while(time1[T3]<300){
			stopMotors();
		}
	}
	clearTimer(T4);
	while(time1[T4]<300){
		backward();
	}
	stopLifters();
}

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

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

void forward() {//moves robot forward
	motor[frontRight] = 127;
	motor[backRight] = 127;
	motor[backLeft] = 127;
	motor[frontLeft] = 127;
}

void backward(){//moves robot backwards
	motor[frontRight] = -127;
	motor[backRight] = -127;
	motor[backLeft] = -127;
	motor[frontLeft] = -127;
}

void left(){//rotates the robot right
	motor[frontRight] = 127;
	motor[backRight] = -127;
	motor[backLeft] = 127;
	motor[frontLeft] = -127;
}

void right(){//rotates the robot left
	motor[frontRight] = -127;
	motor[backRight] = 127;
	motor[backLeft] = -127;
	motor[frontLeft] = 127;
}
void stopLifters(){ //stops lifters
	motor[leftLift1] = 0;
	motor[leftLift2] = 0;
	motor[rightLift1] = 0;
	motor[rightLift2] = 0;
}

void lift(){ //activates arm
	//	while(SensorValue(highButton) == 0){
	motor[leftLift1] = -127;
	motor[leftLift2] = -127;
	motor[rightLift1] = -127;
	motor[rightLift2] = -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[rightLift1] = 127;
	motor[rightLift2] = 127;
}

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

void stopMotors(){ //STOPS ALL MOTORS
	motor[frontRight] = 0;
	motor[backRight] = 0;
	motor[backLeft] = 0;
	motor[frontLeft] = 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(){
	autonomous();
}

task userControl(){
		//Create "deadzone" variables. Adjust threshold value to increase/decrease deadzone
	int X2 = 0, Y1 = 0, X1 = 0, threshold = 15;
	
	//Loop Forever
	while(1 == 1)
	{
		//Create "deadzone" for Y1/Ch3
		if(abs(vexRT[Ch3]) > threshold)
		Y1 = vexRT[Ch3];
		else
		Y1 = 0;
		//Create "deadzone" for X1/Ch4
		if(abs(vexRT[Ch4]) > threshold)
		X1 = vexRT[Ch4];
		else
		X1 = 0;
		//Create "deadzone" for X2/Ch1
		if(abs(vexRT[Ch1]) > threshold)
		X2 = -vexRT[Ch1];
		else
		X2 = 0;
		 
		//Remote Control Commands
		motor[frontRight] = Y1 - X2 - X1;
		motor[backRight] =  Y1 - X2 + X1;
		motor[frontLeft] = Y1 + X2 + X1;
		motor[backLeft] =  Y1 + X2 - X1;

		while(vexRT[Btn7U] == 1){
			forward();
		}
		while(vexRT[Btn7L] == 1){
			left();
		}
		while(vexRT[Btn7R] == 1){
			right();
		}
		while(vexRT[Btn7D] == 1){
			backward();
		}
		if(vexRT[Btn8U] == 1){
			unLift();
		}
		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();
		}
	}
}