Final.c

#pragma config(Sensor, in1,    armPot,         sensorPotentiometer)
#pragma config(Sensor, dgtl1,  liftPot,        sensorQuadEncoder)
#pragma config(Motor,  port1,           leftBack,      tmotorVex393_HBridge, openLoop, driveLeft)
#pragma config(Motor,  port2,           leftFront,     tmotorVex393_MC29, openLoop, driveLeft)
#pragma config(Motor,  port3,           lift,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           arm,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           roller,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           mogo,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           rightFront,    tmotorVex393_MC29, openLoop, driveRight)
#pragma config(Motor,  port10,          rightBack,     tmotorVex393_HBridge, openLoop, driveRight)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//Declare all the necessary variables
const int MAX_NUM_CONES = 10;
const int numStackStates = 10;
const int armDefault = 0;
const int liftDefault = 0;
int liftConeValues[MAX_NUM_CONES];
int armStackValues[numStackStates];
string liftStackValues[numStackStates];
int rollerStackValues[numStackStates];
int numCones = 0;
int powerDriveLeft = 0;
int powerDriveRight = 0;
int deadZone = 20;
int stackState = 0;
int minAccuracy = 10;
int hit = 0;
bool cancelStackComplete = true;
bool clearStackComplete = true;
bool stackComplete = true;

//Returns a value in the range [min,max] if num doesn't belong in that range
int capVal(int num, int max, int min)
{
	if(num > max)
	{
		return max;
	}
	else if(num < min)
	{
		return min;
	}
	return num;
}

//Drive control for the robot based on controller input
void drive(int chnn3,int chnn4)
{
	if(abs(chnn3) < deadZone && abs(chnn4) < deadZone)
	{
		powerDriveLeft = 0;
		powerDriveRight = 0;
	}
	int power = (int)sqrt(chnn3 * chnn3 + chnn4 * chnn4) * abs(chnn3)/chnn3;
	powerDriveLeft = capVal (power + 2 * chnn4, power, -power);
	powerDriveRight = capVal (power - 2 * chnn4, power, -power);
	motor[leftFront] = powerDriveLeft;
	motor[leftBack] = powerDriveLeft;
	motor[rightFront] = powerDriveRight;
	motor[rightBack] = powerDriveRight;
}


//Moves the lift and thus the arm to the specified position
bool moveTo(int armPos, int liftPos, int rollerDir)
{
	 motor[lift] = capVal((SensorValue[liftPot] - liftPos),100, -100);
	 motor[arm] = capVal((SensorValue[armPot] - armPos),100, -100);
	 motor[roller] = 100 * rollerDir;
	 return abs(SensorValue[liftPot] - liftPos) < minAccuracy && abs(SensorValue[armPot] - armPos) < minAccuracy;
}

bool moveToDefault()
{
	return moveTo(armDefault, liftDefault, 1);
}



//Autostacking a cone onto the cap and returns whether it was sucessful or not
bool stack()
{
	 if(stackState < numStackStates)
 	 {
 	    string liftStackValStr = liftStackValues[stackState];
 	    int liftStackVal = 0;
 	    int strStartIndex = stringFind(liftStackValStr, "+STACK_HEIGHT");
 	    if(strStartIndex != -1)
		 	{
		 		  stringDelete(liftStackValStr, strStartIndex, 13);
		 		  liftStackVal = liftConeValues[numCones];
		 	}
			liftStackVal += atoi(liftStackValStr);
			if(moveTo(armStackValues[stackState], liftStackVal, rollerStackValues[stackState]))
			{
				stackState++;
			}
			return false;
	 }
	 stackState = 0;
	 return true;
}

//Cancels a stack if necessary
bool cancelStack()
{
	 stackState = 0;
	 stackComplete = true;
	 return moveToDefault();
}

//Clear the stack
bool clearStack()
{
	numCones = 0;
	stackState = 0;
	cancelStackComplete = true;
  stackComplete = true;
	return moveToDefault();
}

//Returns 0 for no flick, 1 for a upward flick, and - 1 for a downward flick
int flick(int input)
{
	 if(hit != 0)
	 {
			if(abs(input) < 10)
			{
				 int temp = hit;
				 hit = 0;
				 return temp;
			}
			if(time1[T1] > 1000)
			{
				 hit = 0;
			}
	 }
	 if(127 - abs(input) < 10)
	 {
		  if(input < 0)
			{
					hit = -1;
			}
			else
			{
					hit = 1;
			}
			clearTimer(T1);
	 }

	 return 0;
}

//Score on the Mobile Goal
void mobileGoal(bool upButton, bool downButton)
{
	 if(upButton && !downButton)
	 {
		 motor[mogo] = 127;
	 }
	 else if (downButton && !upButton)
	 {
		 motor[mogo] = -127;
	 }
	 else
	 {
		 motor[mogo] = 0;
	 }
}

//Lets the user control the lift if he wants to override the auto function
bool manualLiftOverride(bool upButton, bool downButton)
{
	 if(upButton && !downButton)
	 {
		 motor[lift] = 127;
	 }
	 else if (downButton && !upButton)
	 {
		 motor[lift] = -127;
	 }
	 else if (downButton && upButton)
	 {
		 motor[lift] = 0;
	 }
	 return !upButton && !downButton;
}


task main()
{
	 assign();
	 while(true)
	 {
			drive(vexRT[Ch3], vexRT[Ch4]);
			mobileGoal(vexRT[Btn6U],vexRT[Btn6D]);
			if(manualLiftOverride(vexRT[Btn5U], vexRT[Btn5D]))
			{
				 cancelStackComplete = true;
				 clearStackComplete = true;
				 stackComplete = true;
				 return;
			}
			int flickDirX = flick(vexRT[Ch1]);
			int flickDirY = flick(vexRT[Ch2]);
			if(flickDirY == 1)
			{
					numCones++;
			}
			if(flickDirY == -1)
			{
	 				numCones--;
					cancelStackComplete = false;
			}
			if(flickDirX == 1)
			{
					stackComplete = false;
			}
			if(flickDirX == -1)
			{
					clearStackComplete = false;
			}

			if(!clearStackComplete)
			{
					clearStackComplete = clearStack();
					continue;
			}
			if(!cancelStackComplete)
			{
					cancelStackComplete = cancelStack();
					continue;
			}

			if(!stackComplete)
			{
				  stackComplete = stack();
				  continue;
			}
			moveToDefault();
	 }

}