Full code.c

#pragma config(Sensor, in7,    lineTrackerLeft, sensorLineFollower)
#pragma config(Sensor, in8,    lineTrackerRight, sensorLineFollower)
#pragma config(Sensor, dgtl1,  frontPanSwitch, sensorTouch)
#pragma config(Sensor, dgtl2,  backPanSwitch,  sensorTouch)
#pragma config(Sensor, dgtl3,  tableSwitch,    sensorTouch)
#pragma config(Sensor, dgtl4,  topElevSwitch,  sensorTouch)
#pragma config(Sensor, dgtl5,  bottomElevSwitch, sensorTouch)
#pragma config(Sensor, dgtl7,  sideSelSwitch,  sensorTouch)
#pragma config(Motor,  port1,           leftMotor1,    tmotorVex393_HBridge, openLoop, driveLeft)
#pragma config(Motor,  port2,           leftMotor2,    tmotorVex393_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor,  port3,           leftElevMotor, tmotorVex393_MC29, openLoop, driveLeft)
#pragma config(Motor,  port4,           dustPanMotor,  tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           rightElevMotor, tmotorVex393_MC29, openLoop, reversed, driveRight)
#pragma config(Motor,  port9,           rightMotor2,   tmotorVex393_MC29, openLoop, driveRight)
#pragma config(Motor,  port10,          rightMotor1,   tmotorVex393_HBridge, openLoop, reversed, driveRight)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)


//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(30) //The auto is 30 secs
#pragma userControlDuration(120) //The teleop is 2 mins

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

// Whenever this user program is run, the duration is automatically sent to the VEX master
// processor. The master CPU will only accept these commands once per power=on cycle. So, it's
// best to power cycle your VEX to ensure that they get recognized in case you were
// previously running a standard competition program with different durations.


// Testing Via VEXNET Using ROBOTC IDE
// ==================================================
// It is not necessary to own a VEXNET switch. ROBOTC has a test window that allows the same functionality.
// The test window has three buttons for "DISABLE", "AUTONOMOUS", "USER CONTROL".
// 1. Open the ROBOTC Debugger. Then open the "Competition Control" Debugger window.
// 2. Start your Driver Skills program running.
// 3. Click the "DISABLED" button on the "Competition Control" window.
// 4. Click the "USER CONTROL" button on the "Competition Control" window.
// 5. You need to manually time the 60 second duration.
// 6. Click the "DISABLED" button on the "Competition Control" window.
// 7. Repeat from step (4) to run subsequent test runs. There's no need to reset the VEX or restart your user program.

///**
//* Variables
//**/
//int desLinkPos;
//int upLinkLim;
//int lowLinkLim;
int tableFlipped;
int i;
int tablei;
int startTime;
int lfThreshold = 2500;

///**
//* Moves the Elevator to the desired position. There may be issues with it overshooting and oscilating a lot before settling.
//**/
//void moveElevator(int desiredPos){
//	if (desiredPos < SensorValue(linkPot) && (desiredPos <= upLinkLim || desiredPos >= lowLinkLim)){
//		while(SensorValue(linkPot) != desiredPos){
//			motor[leftLinkMotor] = 70;
//			motor[rightLinkMotor] = 70;
//		}
//	}
//	if (desiredPos > SensorValue(linkPot) && (desiredPos <= upLinkLim || desiredPos >= lowLinkLim)){
//		while(SensorValue(linkPot) != desiredPos){
//			motor[leftLinkMotor] = -20;
//			motor[rightLinkMotor] = -20;
//		}
//	}
//}

/**
* extends the pan if the limit switch is not pressed
**/
void extendPan(){
	//if(SensorValue(backPanSwitch) == 0){
	motor[dustPanMotor] = 80; //is this too fast?
	//} else {
	//motor[dustPanMotor] = 0;
	//}
}

/**
* retracts the pan if the limit switch is not pressed
**/
void retractPan(){
	//if(SensorValue(frontPanSwitch) == 0){
	motor[dustPanMotor] = -80; //is this too fast?
	//} else{
	//motor[dustPanMotor] = 0;
	//}
}

/**
* raises Elevator if the pot is not at the upper limit
**/
void raiseElevator(){
	//if(SensorValue(linkPot) < upLinkLim){ //250 is max
	//if(SensorValue(topElevSwitch) == 1){
	motor[leftElevMotor] 	= 127;//is this in percent?
	motor[rightElevMotor] = 127;
	//}
	//}
}

/**
* lowers Elevator if the pot is not at the upper limit
**/
void lowerElevator(){
	//if(SensorValue(linkPot) > lowLinkLim){
	//if(SensorValue(bottomElevSwitch) == 1){
	motor[leftElevMotor] 	= -127; //is this too fast
	motor[rightElevMotor] = -127;
	//}
	//}
}


void stopAllMotors(){
	while(true){
		motor[leftMotor1] 		= 0;
		motor[leftMotor2] 		= 0;
		motor[rightMotor1] 		= 0;
		motor[rightMotor2]    = 0;
		motor[leftElevMotor]  = 0;
		motor[rightElevMotor] = 0;
		motor[dustPanMotor]		= 0;
		if(vexRT[Btn8L] == 1)
			break;
	}
}

void followLine(int isReverse){
	if(isReverse == 0){
		//If both sensors see dark move forward.
		if(SensorValue(lineTrackerLeft) > lfThreshold && SensorValue(lineTrackerRight) > lfThreshold) {
			motor[leftMotor1]  = 40; //127 is full forward, -127 is full reverse or it might be out of 100%
			motor[leftMotor2]  = 40;
			motor[rightMotor1] = 40;
			motor[rightMotor2] = 40;
		}
	//If both sensors see light move forward (this is to clear the yellow tape in the starting box)
		else if(SensorValue(lineTrackerLeft) < lfThreshold && SensorValue(lineTrackerRight) < lfThreshold) {
			motor[leftMotor1]  = 40;
			motor[leftMotor2]  = 40;
			motor[rightMotor1] = 40;
			motor[rightMotor2] = 40;
		}
		//If the left sensor sees light counter-steer to the left.
		else if(SensorValue(lineTrackerLeft) < lfThreshold){ //could be sersorValue[sensorName]
			motor[leftMotor1]  = 0;
			motor[leftMotor2]  = 0;
			motor[rightMotor1] = 40;
			motor[rightMotor2] = 40;
		}
		//If the right sensor sees light counter-steer to the right.
		else if(SensorValue(lineTrackerRight) < lfThreshold) {
			motor[leftMotor1]  = 40;
			motor[leftMotor2]  = 40;
			motor[rightMotor1] = 0;
			motor[rightMotor2] = 0;
		}
	} else {
	//If both sensors see dark move forward.
		if(SensorValue(lineTrackerLeft) > lfThreshold && SensorValue(lineTrackerRight) > lfThreshold) {
			motor[leftMotor1]  = -40; //127 is full forward, -127 is full reverse or it might be out of 100%
			motor[leftMotor2]  = -40;
			motor[rightMotor1] = -40;
			motor[rightMotor2] = -40;
		}
	//If both sensors see light move forward (this is to clear the yellow tape in the starting box)
		else if(SensorValue(lineTrackerLeft) < lfThreshold && SensorValue(lineTrackerRight) < lfThreshold) {
			motor[leftMotor1]  = -40;
			motor[leftMotor2]  = -40;
			motor[rightMotor1] = -40;
			motor[rightMotor2] = -40;
		}
		//If the left sensor sees light counter-steer to the left.
		else if(SensorValue(lineTrackerLeft) < lfThreshold){ //could be sersorValue[sensorName]
			motor[leftMotor1]  = 0;
			motor[leftMotor2]  = 0;
			motor[rightMotor1] = -40;
			motor[rightMotor2] = -40;
		}
		//If the right sensor sees light counter-steer to the right.
		else if(SensorValue(lineTrackerRight) < lfThreshold) {
			motor[leftMotor1]  = -40;
			motor[leftMotor2]  = -40;
			motor[rightMotor1] = 0;
			motor[rightMotor2] = 0;
		}
	}
}

/**
* Flips table by slowly going forward and raising the Elevator for either 2 secs or until btn8d is pressed, whichever comes first.
**/
void flipTable(){
	int startTime;
	if(tablei == 0){
		startTime = time1[T1]; //keeps start time without having to clear a timer
		tablei = 1;
	}
	if (time1[T1] - startTime <= 2000 && vexRT[Btn8D] == 0){
		motor[leftMotor1]  		= 50;
		motor[leftMotor2]  		= 50;
		motor[rightMotor1] 		= 50;
		motor[rightMotor2] 		= 50;
		raiseElevator();
		tableFlipped = 1;
		} else{
		motor[leftMotor1] 		= 0;
		motor[leftMotor2] 		= 0;
		motor[rightMotor1] 		= 0;
		motor[rightMotor2]    = 0;
		motor[leftElevMotor]  = 0;
		motor[rightElevMotor] = 0;
	}
}

void idealAuto(){
	if(tableFlipped == 0){
		if(SensorValue(bottomElevSwitch) == 0 || SensorValue(backPanSwitch) == 0){
			lowerElevator();
			extendPan();
		} else{
			//If table switch is pressed (if a table has been found), drive forward slowly while rasing the Elevator. This will theoretically raise the table
			if(SensorValue(tableSwitch) == 1){
				flipTable();
			} else {
				followLine(0);
			}
		}
	} else{
		int right = 1;
		//side selection
		if(SensorValue(sideSelSwitch) == right){
			//if sensors see dark
			if(SensorValue(lineTrackerLeft) > lfThreshold && SensorValue(lineTrackerRight) > lfThreshold){
				motor[leftMotor1]	 = -40;
				motor[leftMotor2]	 = -40;
				motor[rightMotor1] = -60;
				motor[rightMotor2] = -60;
			//is sensors see light
			}else if(SensorValue(lineTrackerLeft) < lfThreshold && SensorValue(lineTrackerRight) < lfThreshold){
				motor[leftMotor1]	 = -60;
				motor[leftMotor2]	 = -60;
				motor[rightMotor1] = -60;
				motor[rightMotor2] = -60;
			//if left sensors sees light
			}else if(SensorValue(lineTrackerLeft) < lfThreshold && SensorValue(lineTrackerRight) > lfThreshold){
				motor[leftMotor1]	 = -60;
				motor[leftMotor2]	 = -60;
				motor[rightMotor1] = -40;
				motor[rightMotor2] = -40;
			} else {
				followLine(1);
			}
		//side selection
		} else{
			//if sensors see dark
			if(SensorValue(lineTrackerLeft) > lfThreshold && SensorValue(lineTrackerRight) > lfThreshold){
				motor[leftMotor1]	 = -60;
				motor[leftMotor2]	 = -60;
				motor[rightMotor1] = -40;
				motor[rightMotor2] = -40;
			//if sensors see light
			}else if(SensorValue(lineTrackerLeft) < lfThreshold && SensorValue(lineTrackerRight) < lfThreshold){
				motor[leftMotor1]	 = -60;
				motor[leftMotor2]	 = -60;
				motor[rightMotor1] = -60;
				motor[rightMotor2] = -60;
			//if right sensor sees light
			}else if(SensorValue(lineTrackerLeft) > lfThreshold && SensorValue(lineTrackerRight) < lfThreshold){
				motor[leftMotor1]	 = -40;
				motor[leftMotor2]	 = -40;
				motor[rightMotor1] = -60;
				motor[rightMotor2] = -60;
			} else {
				followLine(1);
			}
		}
	}
}

//second step to involved auto
void semiIdealAuto(){
	//Lower Elevator to the floor and fully extend the pan at the start of auto
	if(SensorValue(bottomElevSwitch) == 0 || SensorValue(backPanSwitch) == 0){
		//moveLinkage(lowLinkLim);
		lowerElevator();
		extendPan();
		} else{
		//If table switch is pressed (if a table has been found), drive forward slowly while rasing the Elevator. This will theoretically raise the table
		if(SensorValue(tableSwitch) == 1){
			flipTable();
			} else {
			followLine(0);
		}
	}
}

//first step to involved auto
void backupAutoWFollow(){
	if(i == 0){
		startTime = time1[T1]; //keeps start time without having to clear a timer
		i = 1;
	}
	if(time1[T1] - startTime <= 1000){
		motor[leftMotor1]  		= 127;
		motor[leftMotor2]  		= 127;
		motor[rightMotor1] 		= 127;
		motor[rightMotor2] 		= 127;
	} else {
		followLine(0);
	}
}

//auto that works
void backupAuto(){
	//int startTime;
	if(i == 0){
		startTime = time1[T1]; //keeps start time without having to clear a timer
		i = 1;
	}
	if(time1[T1] - startTime <= 4000){
		//int right = 1;
		//if(SensorValue(sideSelSwitch) == right){
			motor[leftMotor1]  		= 127;
			motor[leftMotor2]  		= 127;
			motor[rightMotor1] 		= 110;
			motor[rightMotor2] 		= 110;
		//} else {
		//	motor[leftMotor1]  		= 110;
		//	motor[leftMotor2]  		= 110;
		//	motor[rightMotor1] 		= 127;
		//	motor[rightMotor2] 		= 127;
		//}
	}else {
		stopAllMotors();
	}
}

/**
* Before game
* All activities that occur before the competition starts
* Example: clearing encoders, setting servo positions, ...
**/
void pre_auton() {
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;
	clearTimer(T1); //it was spelled with "c" twice, while once with "C"'
	clearTimer(T2);
	i = 0;
	tablei = 0;

	//desLinkPos = 0;
	//upLinkLim = 2065;
	//lowLinkLim = 2375;
}

/**
* Auto
**/
task autonomous() {
	while(true){
		//writeDebugStreamLine("Start of Match");
		//writeDebugStreamLine("AUTO");
		writeDebugStreamLine("Before BackupAuto %i", i);
		//idealAuto();
		//semiIdealAuto();
		//backupAutoWFollow();
		backupAuto();
		writeDebugStreamLine("%i", i);
		writeDebugStreamLine("");

		//writeDebugStreamLine("%d Left line tracker: %d", time1[T2], SensorValue(lineTrackerLeft));
		//writeDebugStreamLine("%d Right line tracker: %d", time1[T2], SensorValue(lineTrackerRight));

		if(vexRT[Btn8R] == 1){
			break;
		}
	}
}

/**
* Teleop
**/
task usercontrol() {
	while (true) { //User control code goes in this while loop
		//writeDebugStreamLine("");
		//writeDebugStreamLine("TELEOP");
		//writeDebugStreamLine("%d Left line tracker: %d", time1[T2], SensorValue(lineTrackerLeft));
		//writeDebugStreamLine("%d Right line tracker: %d", time1[T2], SensorValue(lineTrackerRight));

		motor[leftMotor1]  = vexRT[Ch3];   // Left Joystick Y value
		motor[leftMotor2]  = vexRT[Ch3];
		motor[rightMotor1] = vexRT[Ch2];   // Right Joystick Y value
		motor[rightMotor2] = vexRT[Ch2];

		//Elevator
		//if button 5D is pressed, raise
		if(vexRT[Btn5D] == 1){
			//desLinkPos++;
			raiseElevator();
		}
		//if button 6D is pressed, lower
		else if (vexRT[Btn6D] == 1){
			//desLinkPos--;
			lowerElevator();
		}
		//otherwise, don't move
		//TODO change so that it is holding the weight, tho gravity should not be able to backdrive the motors.
		//May need some sort of control sensor to gain feedback from this elevator to account for the variable load
		else {
			motor[leftElevMotor] = 0;
			motor[rightElevMotor] = 0;
		}

		//Dustpan
		if(vexRT[Btn6U] == 1){
			retractPan();
			} else if(vexRT[Btn5U] == 1){
			extendPan();
			} else {
			motor[dustPanMotor] = 0;
		}

		//flip table automation
		if(vexRT[Btn7D]==1){
			flipTable();
		}

		if(vexRT[Btn7L] == 1){
			//stopAllMotors();
			startTask(autonomous);
		}
	}
}