#pragma config(Motor,  port1,           bottomIntake,  tmotorVex393TurboSpeed_HBridge, openLoop, reversed)
#pragma config(Motor,  port3,           LMDrive,       tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port4,           LBDrive,       tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           LFDrive,       tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port6,           RFDrive,       tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           RBDrive,       tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           RMDrive,       tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          topIntake,     tmotorVex393HighSpeed_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

bool slow = false;
int square(int n)
{
	int sq;
	sq = n*n;
	if(n < 0)
	{
		sq = -sq;
	}
	return sq;
}
int curve(int n)
{
	int outputPower = (square(n))/127;
	int minPower = 20;
	if(abs(n) > 3 && abs(outputPower) <= minPower)
	{
		return sgn(n)*minPower ;
	}
	else
	{
		return outputPower;
	}
}
int drive(int n)
{
	if(abs(n)<3)
	{
		return 0;
	}
	else if (slow == true)
	{
		return n/2.7;
	}
	else
	{
		return n;
	}
}
task main()
{
	while(1)
	{
		//if(abs(vexRT[ch2]) > 15)
		//{
		if(vexRT[Btn8D] == 1)
		{
			if(slow == true)
			{
				slow = false;
			}
			if(slow == false)
			{
				slow = true;
			}
		}
		motor[RFDrive] =
		motor[RMDrive] =
		motor[RBDrive] =
		drive(vexRT[ch2]);
		//}
		//else
		//{
		//motor[RFDrive] =
		//motor[RMDrive] =
		//motor[RBDrive] =
		//0;
		//}
		//if(abs(vexRT[ch3]) > 15)
		//{
		motor[LFDrive] =
		motor[LMDrive] =
		motor[LBDrive] =
		drive(vexRT[ch3]);
		//}
		//else
		//{
		//motor[LFDrive] =
		//motor[LMDrive] =
		//motor[LBDrive] =
		//0;
		//}
		/*if(vexRT[Btn6U] == 1 && vexRT[Btn5D] != 1)
		{
		motor[topIntake] = 127;
		}
		else if(vexRT[btn5D] != 1)
		{
		motor[topIntake] = 0;
		}
		if (vexRT[Btn6D] == 1 && vexRT[Btn5D] != 1)
		{
		motor[bottomIntake] = 127;
		}
		else if(vexRT[Btn5D] != 1)
		{
		motor[bottomIntake] = 0;
		}
		if(vexRT[btn5D] == 1)
		{
		motor[bottomIntake] = -127;
		motor[topIntake] = -127;
		}*/
		motor[topIntake] = ((vexRT[Btn5U] - vexRT[Btn5D])*127);
		motor[bottomIntake] = ((vexRT[Btn6U] - vexRT[Btn6D])*127);
	}
}
