Vex Code for our robot completely OP

vexCode.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    armPent,        sensorPotentiometer)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           backRight,     tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor,  port2,           frontLeft,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           leftIntake,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           rightLift,     tmotorVex393_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port5,           claw,          tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           leftLift,      tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port8,           rightIntake,   tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           frontRight,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          backLeft,      tmotorVex393_HBridge, 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)

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


// Globals
int TARGET_LEFT = 0;
int TARGET_RIGHT = 0;

/*---------------------------------------------------------------------------*/
/*                          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 autonomous 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 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;

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

void moveForward(int movementSpeed) {
	motor[backLeft] = movementSpeed;
	motor[backRight] = movementSpeed + ((movementSpeed < 0) ? -10 : 10);
	motor[frontLeft] = movementSpeed;
	motor[frontRight] = movementSpeed + ((movementSpeed < 0) ? -10 : 10);
}

void setLifts(int value) {
	motor[rightLift] = value;
	motor[leftLift] = value;
}

void doA360(int movementSpeed) {
	motor[backLeft] = -movementSpeed;
	motor[backRight] = movementSpeed + ((movementSpeed < 0) ? -10 : 10);
	motor[frontLeft] = -movementSpeed;
	motor[frontRight] = movementSpeed + ((movementSpeed < 0) ? -10 : 10);
}

void setMobileGoal(int value) {
	motor[rightIntake] = -value;
	motor[leftIntake] = -value;
}

task autonomous()
{
	// ..........................................................................
	// Insert user code here.
	// ..........................................................................

	int movementSpeed = 80;

	/*
	motor[claw] = 80;
	sleep(500);
	motor[claw] = 15;
	*/
	setMobileGoal(-100);
	moveForward(-movementSpeed);
	sleep(600);
	setMobileGoal(0);
	// This is only correct for the right side ???
	sleep(2600);
	moveForward(0);

	setMobileGoal(100);
	sleep(900);
	setMobileGoal(0);

	moveForward(movementSpeed);
	sleep(2000);
	moveForward(0);

	doA360(movementSpeed);
	sleep(700);
	moveForward(0);
	moveForward(-movementSpeed);
	sleep(300);
	doA360(movementSpeed);
	sleep(920);

	moveForward(-movementSpeed);
	sleep(2750);
	moveForward(0);

	setMobileGoal(-100);
	sleep(600);
	setMobileGoal(0);

	moveForward(movementSpeed);
	sleep(1500);
	moveForward(0);

}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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 for driving to allow concurrent movements, ie. driving and lifting
task drive() {
	while(true) {
		motor[backLeft] = vexRT[Ch3] + vexRT[Ch1];
		motor[backRight] = vexRT[Ch3] - vexRT[Ch1];
		motor[frontLeft] = vexRT[Ch3] + vexRT[Ch1];
		motor[frontRight] = vexRT[Ch3] - vexRT[Ch1];
	}
}

task clawControl() {
	while(true) {
		if(vexRT[Btn6U]) {
				motor[claw] = 80;
			} else if(vexRT[Btn6D]) {
				motor[claw] = -80;
			}
			else if (SensorValue[armPent] < 2000 && vexRT[Btn6U] !=  1 && vexRT[Btn6D] != 1)		//change pot value to when it needs to start closing...
			{
				motor[claw] = 20;
			}
			else {
				motor[claw] = 0;
			}
	}
}

// Stabilizes the left and right motors
task stabilize() {
	const float PROPORTIONAL_CONSTANT = 0.8;
	int speed_left = 0;
	int speed_right = 0;

	int error_left = 0;
	int error_right = 0;
	while(true) {
		error_left = TARGET_LEFT - nMotorEncoder[leftLift];
		error_right = TARGET_RIGHT - nMotorEncoder[rightLift];

		speed_left = error_left * PROPORTIONAL_CONSTANT;
		speed_right = error_right * PROPORTIONAL_CONSTANT;

		if( speed_left > 127 ) {
			speed_left = 127;
		} else if( speed_left < -127 ) {
			speed_left = -127;
		}

		if( speed_right > 127 ) {
			speed_right = 127;
		} else if( speed_right < -127 ) {
			speed_right = -127;
		}

		motor[leftLift] = speed_left;
		motor[rightLift] = speed_right;
	}
}

task usercontrol()
{
	// Set initial targets, otherwise motor would fall
	TARGET_LEFT = nMotorEncoder[leftLift];
	TARGET_RIGHT = nMotorEncoder[rightLift];

	// start concurrent tasks
	startTask(stabilize);
	startTask(drive);
	startTask(clawControl);

	// event loop
	while (true)
	{
		//
		if(vexRT[Btn6UXmtr2]) {
			motor[leftIntake] = -100;
			motor[rightIntake] = -100;
		}
		else if(vexRT[Btn6DXmtr2]) {
			motor[leftIntake] = 100;
			motor[rightIntake] = 100;
		} else {
			motor[leftIntake] = 0;
			motor[rightIntake] = 0;
		}




		if(vexRT(Btn5U)) {
			stopTask(stabilize);
			while(vexRT(Btn5U)) {
				setLifts(100);
			}
			TARGET_LEFT = nMotorEncoder[leftLift];
			TARGET_RIGHT = nMotorEncoder[rightLift];
			startTask(stabilize);

  	} else if(vexRT(Btn5D)) {
			 stopTask(stabilize);
			 while(vexRT(Btn5D)) {
					setLifts(-100);
				}
				TARGET_LEFT = nMotorEncoder[leftLift];
				TARGET_RIGHT = nMotorEncoder[rightLift];
				startTask(stabilize);
			}

  	}
}