#pragma config(Sensor, in1,    PEB,            sensorAnalog)
#pragma config(Motor,  port1,           leftFront,     tmotorVex393_HBridge, openLoop, reversed, driveLeft)
#pragma config(Motor,  port2,           leftRear,      tmotorVex393_MC29, openLoop, driveLeft)
#pragma config(Motor,  port3,           topLeft,       tmotorVex393TurboSpeed_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor,  port4,           bottomLeft,    tmotorVex393TurboSpeed_MC29, openLoop, driveLeft)
#pragma config(Motor,  port5,           roller,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           lift,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           bottomRight,   tmotorVex393TurboSpeed_MC29, openLoop, reversed, driveRight)
#pragma config(Motor,  port8,           topRight,      tmotorVex393TurboSpeed_MC29, openLoop, driveRight)
#pragma config(Motor,  port9,           rightRear,     tmotorVex393_MC29, openLoop, reversed, driveRight)
#pragma config(Motor,  port10,          rightFront,    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(1500)
#pragma userControlDuration(10500)

#include "Vex_Competition_Includes.c"

void pre_auton()
{
	bLCDBacklight = true;									// Turn on LCD Backlight
	string mainBattery, PEB;

	while (true)
	{
		clearLCDLine(0);											// Clear line 1 (0) of the LCD
		clearLCDLine(1);											// Clear line 2 (1) of the LCD

		//Display the Primary Robot battery voltage
		displayLCDString(0, 0, "Primary: ");
		sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
		displayNextLCDString(mainBattery);

		//Display the Backup battery voltage
		displayLCDString(1, 0, "PEB: ");
		sprintf(PEB, "%1.2f%c", sensorAnalog/6.5, 'V');	//Build the value to be displayed
		displayNextLCDString(PEB);

		//Short delay for the LCD refresh rate
		wait1Msec(100);
	}
}



task autonomous()
{

}



task usercontrol()
{
	while (true)

	{
		motor[leftRear] = vexRT[Ch3]+ vexRT[Ch1]- vexRT[Ch4];
		motor[leftFront] = vexRT[Ch3]+ vexRT[Ch1]+ vexRT[Ch4];
		motor[rightFront] = vexRT[Ch3]- vexRT[Ch1]- vexRT[Ch4];
		motor[rightRear] = vexRT[Ch3]- vexRT[Ch1]+ vexRT[Ch4];

		if(vexRT[Btn5U] == 1)
		{
			motor[roller] = -127;
			motor[lift] = -127;
		}
		else if(vexRT[Btn5D] == 1)
		{
			motor[roller] = 127;
			motor[lift] = 127;
		}
		else
		{
			motor[roller]=0;
			motor[lift]=0;
		}
		if(vexRT[Btn6U] == 1)
		{
			motor[topLeft] = 45;
			motor[bottomLeft] = 45;
			motor[topRight] = 45;
			motor[bottomRight] = 45;
		}
		else if(vexRT[Btn6D] == 1)
		{
			motor[topLeft] = 38;
			motor[bottomLeft] = 38;
			motor[topRight] = 38;
			motor[bottomRight] = 38;
		}
		else
		{
			motor[topLeft] = 0;
			motor[bottomLeft] = 0;
			motor[topRight] = 0;
			motor[bottomRight] = 0;
		}
	}
}