competitionV3.0

#pragma config(Motor,  port2,           frontRightMotor, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port3,           backRightMotor, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port4,           frontLeftMotor, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port5,           backLeftMotor, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port6,           claw,          tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port7,           tilter,        tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port8,           elevatorR,     tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port9,           elevatorL,     tmotorServoContinuousRotation, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*        Description: Competition template for VEX EDR                      */
/*                                                                           */
/*---------------------------------------------------------------------------*/

// 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"


/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the and usercontrol tasks will not be started.  This       */
/*  function is only called once after the cortex has been powered on and    */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/
void forward(){//moves robot forward
		motor[frontRightMotor] = -120;
    motor[backRightMotor]  = -120;
    motor[frontLeftMotor] = -120;
    motor[backLeftMotor]  = -120;
	} 

	void backward(){//moves robot backwards
		motor[frontRightMotor] = 120;
    motor[backRightMotor]  = 120;
    motor[frontLeftMotor] = 120;
    motor[backLeftMotor]  = 120;
	}

/*	void left(){
		motor[frontRightMotor] = 70;
    motor[backRightMotor]  = 70;
    motor[frontLeftMotor] = -70;
    motor[backLeftMotor]  = -70;
	}

	void right(){
		motor[frontRightMotor] = -70;
    motor[backRightMotor]  = -70;
    motor[frontLeftMotor] = 70;
    motor[backLeftMotor]  = 70;
	}*/

	void turnRight(){//rotates the robot right
		motor[frontRightMotor] = 120;
    motor[backRightMotor]  = 120;
    motor[frontLeftMotor] = -120;
    motor[backLeftMotor]  = -120;
	}

	void turnLeft(){//rotates the robot left
		motor[frontRightMotor] = -120;
    motor[backRightMotor]  = -120;
    motor[frontLeftMotor] = 120;
    motor[backLeftMotor]  = 120;
	}

	void elevate(){//raises the forklift up to is maximum point (takes max 1.5s)
		clearTimer(T1);
		while(time1[T1] < 1200){
			motor[elevatorR] = 120;
			motor[elevatorL] = 120;
		}
		motor[elevatorR] = 0;
		motor[elevatorL] = 0;
	}

	void elevateDown(){//lowers the forklift to its neutral position (takes max 1.4s)
		clearTimer(T2);
		while(time1[T2] < 1100){
			motor[elevatorR] = -120;
			motor[elevatorL] = -120;
		}
		motor[elevatorR] = 0;
		motor[elevatorL] = 0;
	}
	
	void tilt(){//tilts the forklift up
		clearTimer(T3);
		while(time1[T3] < 500){
			motor[tilter] = 70;
		}
		motor[tilter] = 0; 
	}
	
	void tiltDown(){//tilts the forklift down to pick up stuff 
		clearTimer(T3);
		while(time1[T3] < 500){
			motor[tilter] = -70;
		}
		motor[tilter] = 0; 
	}
	
	void grab(){//snaps the clamps together to 'grab' the thing   
		motor[claw] = 70;
	}
	
	void release(){//release the clamp
		clearTimer(T4);
		while(time1[T4] < 500){
			motor[claw] = -70;
		}
		motor[claw] = 0; 
	}
	
	void stopMotors() {//stops all motors 
		motor[frontRightMotor] = 0;
    motor[backRightMotor]  = 0;
    motor[frontLeftMotor] = 0;
    motor[backLeftMotor]  = 0;
    motor[claw] = 0;
    motor[tilter] = 0;
    motor[elevatorR] = 0; 
    motor[elevatorL] = 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, ...
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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.   */
/*---------------------------------------------------------------------------*/

task autonomous()
{
	clearTimer(T4);
	while(time1[T4] < 2000){
		forward(); 
	}
	clearTimer(T1);
	while(time1[T1] < 500){
		turnLeft();
	}
	elevate();
	for(int i=0; i < 4; i++){
		clearTimer(T3);
		while(time1[T3] < 200){
			backward();
		}
		clearTimer(T3);
		while(time1[T3] < 200){
			forward();
		}
	}//causes jerking motion which flips the tilter
	tilt();
	clearTimer(T2);
	while(time1[T2] < 2500){
		forward();
	}
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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.   */
/*---------------------------------------------------------------------------*/

task usercontrol()
{
  // User control code here, inside the loop  
	
  while (true)
  {
    if(vexRT[Btn7U] == 1){
    	forward();
  	}
  	/*if(vexRT[Btn7L] == 1){
  		left();
  	}
  	if(vexRT[Btn7R] == 1){
  		right();
  	}*/
  	if(vexRT[Btn7D] == 1){
  		backward();
  	}
  	if(vexRT[Btn8L] == 1){
  		turnLeft();
  	}
  	if(vexRT[Btn8R] == 1){
  		turnRight();
  	}
  	if(vexRT[Btn5U] == 1){
  		elevate();
  		tilt();
  	}
  	if(vexRT[Btn5D] == 1){
  		elevateDown();
  		tiltDown();
  	}
  	if(vexRT[Btn7D] == 1){
  		grab();
  	}
  	if(vexRT[Btn7U] == 1){
  		release();
  	}
  	stopMotors();
  }
}