CompetitionCodeV4.0

#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] = -50;
		motor[claw2]= -50;
	}
	
	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(){ //lifts lift (does not use buttons)
		//	while(SensorValue(highButton) == 0){
		motor[leftLift1] = -127;
		motor[leftLift2] = -127;
		motor[leftLift3] = -127;
		motor[rightLift1] = -127;
		motor[rightLift2] = -127;
		motor[rightLift3] = -127;
		//	}
	}
	
	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
}

task usercontrol(){
  // User control code here, inside the loop  
  // Used timers to serve as a while loop to prevent the constant 'jerking' motion of the forklift that keeps checking if the button is pressed.	
  while (true){
  	while(vexRT[Btn7U] == 1){
			forward();
		}
		while(vexRT[Btn7L] == 1){
			left();
		}
		while(vexRT[Btn7R] == 1){
			right();
		}
		while(vexRT[Btn7D] == 1){
			backward();
		}
		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){
			clearTimer(T3);
			while(time1[T3]<10){
				drop();
			}
			stopClaw();
		}

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

		stopMotors();
	}
}