Competetive.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  blueSide1,      sensorDigitalIn)
#pragma config(Sensor, dgtl2,  blueSide2,      sensorDigitalIn)
#pragma config(Sensor, dgtl3,  blueSide3,      sensorDigitalIn)
#pragma config(Sensor, dgtl4,  blueBack1,      sensorDigitalIn)
#pragma config(Sensor, dgtl5,  blueBack2,      sensorDigitalIn)
#pragma config(Sensor, dgtl6,  blueBack3,      sensorDigitalIn)
#pragma config(Sensor, dgtl7,  redSide1,       sensorDigitalIn)
#pragma config(Sensor, dgtl8,  redSide2,       sensorDigitalIn)
#pragma config(Sensor, dgtl9,  redSide3,       sensorDigitalIn)
#pragma config(Sensor, dgtl10, redBack1,       sensorDigitalIn)
#pragma config(Sensor, dgtl11, redBack2,       sensorDigitalIn)
#pragma config(Sensor, dgtl12, redBack3,       sensorDigitalIn)
#pragma config(Sensor, I2C_1,  rightEncoder,   sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  leftEncoder,    sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           frontRight,    tmotorVex393_MC29, openLoop, reversed, driveRight, encoderPort, I2C_1)
#pragma config(Motor,  port3,           frontLeft,     tmotorVex393_MC29, openLoop, driveLeft, encoderPort, I2C_2)
#pragma config(Motor,  port4,           backRight,     tmotorVex393_MC29, openLoop, reversed, driveRight)
#pragma config(Motor,  port5,           backLeft,      tmotorVex393_MC29, openLoop, driveLeft)
#pragma config(Motor,  port6,           rightLauncher, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           leftLauncher,  tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           topBelt,       tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// Beware if you enable PID control...... It hasn't always ended well for the unexperienced programmer

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(105)

//This sets the time for doing certain moves if the encoders aren't working
//float feet10 = 7130; //7.13 seconds
//float completeTurn = 4680; //4.68 seconds

//29.85 is how many inches needed to turn for a 180
//float rotationsRight = distanceRight / circumference;
//float degreesRight = rotationsRight * 360;
//int encoderCountsRight = degreesRight * 4; // The encoder counts in quarter degrees.

// This is so we can find out how many times the left wheels have to turn to go the specefied distance

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

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

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;

	// All activities that occur before the competition starts
	resetMotorEncoder(frontRight);
	resetMotorEncoder(frontLeft);
}

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

float ratio;

float diameter = 5.125; // 5.125 makes us innacurate, how about 5.25?
float circumference = PI * diameter;

float distanceRight;
float rotationsRight;
float degreesRight;
int encoderCountsRight;

float distanceLeft;
float rotationsLeft;
float degreesLeft;
int encoderCountsLeft;

void ForwardForDistance(int speed, float targetDistance) /* Make speed negative to reverse and change the wait to abs to make sure it works*/
{
	//float rotations = targetDistance / circumference; /* On torque mode (stock), there are 627 counts to a revolution, high speed there is 392, and turbo speed has 261*/
	//float degrees = rotations/** 360*/;
	//float encoderCounts = degrees * 4;	// 24 / 18.375 is 1.30612244898

	//600 counts = 1 foot

	resetMotorEncoder(frontRight);
	resetMotorEncoder(frontLeft);

	wait1Msec(1000);

	float motorEncoderRight = targetDistance;	//600 = 1 foot
	float motorEncoderLeft = targetDistance;

	while (nMotorEncoder(frontRight) < motorEncoderRight && nMotorEncoder(frontLeft) < motorEncoderLeft)
	{
		motor[frontRight] = speed;
		motor[frontLeft] = speed;
		motor[backRight] = speed;
		motor[backLeft] = speed;
	}
}

void TurnLeft(float degreesToTurn, int speed)
{
	//float rotationsRight = distanceRight / circumference; /* On torque mode (stock), there are 627 counts to a revolution, high speed there is 392, and
	// turbo speed has 261*/
	//float degreesRight = rotationsRight * 627.2;
	//float encoderCountsRight = degreesRight * 4; // 24 / 18.375 is 1.30612244898
	////float rotationsLeft = distanceLeft / circumference;
	//float rotationsLeft = rotationsRight * -1; // This is the same as calculating the rotations directly
	//float degreesLeft = rotationsLeft * 627.2;
	//float encoderCountsLeft = degreesLeft * 4;

	ratio = 180 / degreesToTurn; // We find the ratio of degrees to turn to doing a 180 turn
	distanceLeft = 29.85 / ratio; /* This then compares the distance to turn of a 180 to the ratio made above to make
	the distance the robot has to turn*/

	rotationsLeft = distanceLeft / circumference;
	degreesLeft = rotationsLeft * 360;
	encoderCountsLeft = degreesLeft * 4; // The encoder counts in quarter degrees.
	encoderCountsRight = encoderCountsLeft * -1; // This is the same as calculating the counts directly, but saves time :)

	resetMotorEncoder(frontRight);
	resetMotorEncoder(frontLeft);

	motor[frontRight] = speed * -1;
	motor[frontLeft] = speed;
	motor[backRight] = speed * -1;
	motor[backLeft] = speed;

	while(nMotorEncoder[frontRight] < encoderCountsRight && nMotorEncoder[frontLeft] < encoderCountsLeft)
	{
		if (abs(nMotorEncoder[frontRight]) > abs(nMotorEncoder[frontLeft]))
		{ /* Since one value is negative and one is positive, abs (absolute value) makes it so that we can make sure the robot
			turns evenly*/
			motor[frontRight] = speed / -2;
			motor[backRight] = speed / -2;
		}
		else if(abs(nMotorEncoder[frontRight]) < abs(nMotorEncoder[frontLeft]))
		{
			motor[frontLeft] = speed / 2; // These are negative because we are still turning
			motor[backLeft] = speed / 2;
		}
		else if (nMotorEncoder[frontRight] == nMotorEncoder[frontLeft])
		{
			motor[frontRight] = speed;
			motor[frontLeft] = speed;
			motor[backRight] = speed;
			motor[backLeft] = speed;
		}
	}
}

void TurnRight(float degreesToTurn, int speed)
{
	ratio = 180 / degreesToTurn;
	distanceRight = 29.85 / ratio;

	rotationsRight = distanceRight / circumference;
	degreesRight = rotationsRight * 360;
	encoderCountsRight = degreesRight * 4;

	encoderCountsLeft = encoderCountsRight * -1;

	resetMotorEncoder(frontRight);
	resetMotorEncoder(frontLeft);

	motor[frontRight] = speed;
	motor[frontLeft] = speed * -1;
	motor[backRight] = speed;
	motor[backLeft] = speed * -1;

	while(nMotorEncoder[frontRight] < encoderCountsRight && nMotorEncoder[frontLeft] < encoderCountsLeft)
	{
		if (abs(nMotorEncoder[frontRight]) > abs(nMotorEncoder[frontLeft]))
		{
			motor[frontRight] = speed / 2;
			motor[backRight] = speed / 2;
		}
		else if(abs(nMotorEncoder[frontRight]) < abs(nMotorEncoder[frontLeft]))
		{
			motor[frontLeft] = speed / -2;
			motor[backLeft] = speed / -2;
		}
		else if (nMotorEncoder[frontRight] == nMotorEncoder[frontLeft])
		{
			motor[frontRight] = speed;
			motor[frontLeft] = speed;
			motor[backRight] = speed;
			motor[backLeft] = speed;
		}
	}
}

void Shoot(int launchSpeed, float timeToWait)	//spinTime allows the launcher to get up to speed

{
	float spinTime = 2;

	motor[rightLauncher] = launchSpeed;
	motor[leftLauncher] = launchSpeed;

	wait1Msec(spinTime * 1000);

	motor[topBelt] = 127;

	wait1Msec(timeToWait * 1000); // Since 1 second = 1000 milliseconds

	motor[rightLauncher] = 0;
	motor[leftLauncher] = 0;
	motor[topBelt] = 0;
}

void Wait(bool wait, float timeToWait)
{
	if(wait == true)
	{
		wait1Msec(timeToWait * 1000);
	}
}

void Stop()
{
	motor[frontRight] = 0;
	motor[frontLeft] = 0;
	motor[backRight] = 0;
	motor[backLeft] = 0;
}

task autonomous()
{

	// These 'if' statements check to see if there is a jumper in the selected port. If there is a jumper in that port, that portion of code will
	// be ran. If the boolean is equal to false, the statement will equate to true, running that code. So a true value on the jumper
	// equals false  for the "if" statement, while a false value will equal true. Confusing, huh?
	// Refer to this link so you can properly set the turns: http://content.vexrobotics.com/docs/vrc-nothing-but-net/VRC-Nothing-But-Net-Field-Specifications-20150408.pdf
	// Put a wait for every single autonomous setting so if you don't absolutely need it, you can just comment it out

	//float rotationsRight = distanceRight / circumference; // On torque mode (stock), there are 627 counts to a revolution, high speed there is 392, and
	//// turbo speed has 261
	//float degreesRight = rotationsRight * 627.2;
	//float encoderCountsRight = degreesRight * 4; // 24 / 18.375 is 1.30612244898
	//float rotationsLeft = distanceLeft / circumference;
	//float degreesLeft = rotationsLeft * 627.2;
	//float encoderCountsLeft = degreesLeft * 4;

	if (SensorBoolean[blueSide1] == false) //Ports 1-6 will be for the blue team: 3 different choices per square. 7-12 will be for the red team.
	{

	//	distanceRight = 24; //24 in. 2 off, 36 in. 5 off. Circumference?
	//	distanceLeft = 24;

			//wait1Msec(3000);

	//	while (nMotorEncoder[frontRight] < encoderCountsRight && nMotorEncoder[frontLeft] < encoderCountsLeft)
	//	{
			//if (abs(nMotorEncoder[frontRight]) > abs(nMotorEncoder[frontLeft]))
			//{ // Since one value is negative and one is positive, abs (absolute value) makes it so that we can make sure the robot
			//	// turns evenly
			//	motor[frontRight] = 63;
			//	motor[backRight] = 63;
			//}
			//else if(abs (nMotorEncoder[frontRight]) < abs(nMotorEncoder[frontLeft]))
			//{
			//	motor[frontLeft] = -63; // These are negative because we are still turning
			//	motor[backLeft] = -63;
			//}
			//else if (abs(nMotorEncoder[frontRight]) == abs(nMotorEncoder[frontLeft]))
			//{
  	//		motor[frontRight] = 127;
  	//		motor[frontLeft] = -127;
  	//		motor[backRight] = 127;
  	//		motor[backLeft] = -127;
			//}
	//	}

	//		//wait1Msec(feet10 / 5);

	//		//motor[backRight] = -127; //Turn 45 degrees right
	//		//motor[frontRight] = -127;

	//	distanceRight = halfTurn / 4;
	//	distanceLeft = halfTurn / -4; // The left wheels have to spin the opposite direction, so this is negative

	//	//float rotationsRight = distanceRight / circumference;
	//	//float degreesRight = rotationsRight * 360;
	//	//float encoderCountsRight = degreesRight * 1.30612244898;
	//	//float rotationsLeft = distanceLeft / circumference;
	//	//float degreesLeft = rotationsLeft * 360;
	//	//float encoderCountsLeft = degreesLeft * 1.30612244898;

	//	while (nMotorEncoder[frontRight] <= encoderCountsRight && nMotorEncoder[frontLeft] <= encoderCountsLeft)
	//	{
	//		if (abs(nMotorEncoder[frontRight]) > abs(nMotorEncoder[frontLeft]))
	//		{ // Since one value is negative and one is positive, abs (absolute value) makes it so that we can make sure the robot
	//			// turns evenly
	//			motor[frontRight] = 63;
	//			motor[backRight] = 63;
	//		}
	//		else if (abs(nMotorEncoder[frontRight]) < abs(nMotorEncoder[frontLeft]))
	//		{
	//			motor[frontLeft] = -63; // These are negative because we are still turning
	//			motor[backLeft] = -63;
	//		}
	//		else if (abs(nMotorEncoder[frontRight]) == abs(nMotorEncoder[frontLeft]))
	//		{
 // 			motor[frontRight] = 127;
 // 			motor[frontLeft] = -127;
 // 			motor[backRight] = 127;
 // 			motor[backLeft] = -127;
	//		}
	//	}

	//		//wait1Msec(completeTurn / 8);

	//		//motor[frontRight] = 127;
	//		//motor[frontLeft] = 127;
	//		//motor[backRight] = 127;
	//		//motor[backLeft] = 127;

	//	distanceRight = 48;
	//	distanceLeft = 48;

	//	//float rotationsRight = distanceRight / circumference;
	//	//float degreesRight = rotationsRight * 360;
	//	//float encoderCountsRight = degreesRight * 1.30612244898;
	//	//float rotationsLeft = distanceLeft / circumference;
	//	//float degreesLeft = rotationsLeft * 360;
	//	//float encoderCountsLeft = degreesLeft * 1.30612244898;

	//	while (nMotorEncoder[frontRight] <= encoderCountsRight && nMotorEncoder[frontLeft] <= encoderCountsLeft)
	//	{
	//		if (nMotorEncoder[frontRight] > nMotorEncoder[frontLeft])
	//		{
	//			motor[frontRight] = 63; // If the right side has travelled farther than the left side, the right
	//			// side will slow down to half speed and vice versa below
	//			motor[backRight] = 63;
	//		}
 // 		else if (nMotorEncoder[frontRight] < nMotorEncoder[frontLeft])
 // 		{
 // 			motor[frontLeft] = 63;
 // 			motor[backLeft] = 63;
 // 		}
 // 		else if (nMotorEncoder[frontRight] == nMotorEncoder[frontLeft])
 // 		{
 // 			motor[frontRight] = 127;
 // 			motor[frontLeft] = 127;
 // 			motor[backRight] = 127;
 // 			motor[backLeft] = 127;
 // 		}
	//	}

	//	//wait1Msec(feet10);

	//	motor[frontRight] = 0;
	//	motor[frontLeft] = 0;
	//	motor[backRight] = 0;
	//	motor[backLeft] = 0;

	//	// This is where we will start shooting
	//	motor[rightLauncher] = 127;
	//	motor[leftLauncher] = 127;
	//	motor[topBelt] = 127;

		Wait(false, 3);

		ForwardForDistance(127, 24);

		TurnLeft(45, 127);

		ForwardForDistance(127, 48);

		Stop();

		Shoot(127, 15);
	} //Just copy the code above with the new functions

	else if (SensorBoolean[blueSide2] == false)
	{
			//motor[frontRight] = 127;
			//motor[frontLeft] = 127;
			//motor[backRight] = 127;
			//motor[backLeft] = 127;

			////wait1Msec(feet10 / 6.667);

			//motor[frontRight] = -127; // 45 degree turn
			//motor[backRight] = -127;

			////wait1Msec(completeTurn / 8);

			//motor[frontRight] = 127;
			//motor[backRight] = 127;

			////wait1Msec(feet10 * 0.6);

			//motor[frontRight] = 0;
			//motor[frontLeft] = 0;
			//motor[backRight] = 0;
			//motor[backLeft] = 0;

			//motor[rightLauncher] = 127;
			//motor[leftLauncher] = 127;
			//motor[topBelt] = 127;

		Wait(false, 3);

		ForwardForDistance(127, 24);

		TurnLeft(45, 127);

		ForwardForDistance(127, 60);

		Stop();

		Shoot(127, 15);
	}

	else if (SensorBoolean[blueSide3] == false)
	{

		Wait(false, 3);

		ForwardForDistance(127, 72);

		Stop();

		Shoot(127, 15);
	}

	else if (SensorBoolean[blueBack1] == false)
	{
		//motor[frontRight] = 127; //Move forward 2 feet
			//motor[frontLeft] = 127;
			//motor[backRight] = 127;
			//motor[backLeft] = 127;

			////wait1Msec(feet10 / 5);

			//motor[frontLeft] = -127; //7130 10 feet
			//motor[backLeft] = -127; //4680 360 turn

			////wait1Msec(completeTurn / 8);

			//motor[frontRight] = 127;
			//motor[frontLeft] = 127;
			//motor[backRight] = 127;
			//motor[backLeft] = 127;

			////wait1Msec(feet10 * 0.75);

			//motor[frontRight] = 0;
			//motor[frontLeft] = 0;
			//motor[backRight] = 0;
			//motor[backLeft] = 0;

			//motor[rightLauncher] = 127;
			//motor[leftLauncher] = 127;
			//motor[topBelt] = 127;

		Wait(false, 3);

		ForwardForDistance(127, 24);

		TurnRight(45, 127);

		ForwardForDistance(127, 48);

		Stop();

		Shoot(127, 15);
	}

	else if (SensorBoolean[blueBack2] == false)
	{
		Wait(false, 3);

		ForwardForDistance(127, 24);

		TurnRight(45, 127);

		ForwardForDistance(127, 60);

		Stop();

		Shoot(127, 15);
	}

	else if (SensorBoolean[blueBack3] == false)
	{
		Wait(false, 3);

		ForwardForDistance(127, 72);

		Stop();

		Shoot(127, 15);
	}

	else if (SensorBoolean[redSide1] == false) // This is where the red team autonomous code will go.
	{
			//motor[frontRight] = 127; // Move 2 feet
			//motor[frontLeft] = 127;
			//motor[backRight] = 127;
			//motor[backLeft] = 127;

			////wait1Msec(feet10 / 6.667);

			//motor[frontRight] = -127; // 45 degree turn
			//motor[backRight] = -127;

			////wait1Msec(completeTurn / 8);

			//motor[frontRight] = 127;
			//motor[backRight] = 127;

			////wait1Msec(feet10 * 0.55);

			//motor[frontRight] = 0;
			//motor[frontLeft] = 0;
			//motor[backRight] = 0;
			//motor[backLeft] = 0;

			//motor[rightLauncher] = 127;
			//motor[leftLauncher] = 127;
			//motor[topBelt] = 127;

		Wait(false, 3);

		ForwardForDistance(127, 24);

		TurnRight(45, 127);

		ForwardForDistance(127, 48);

		Stop();

		Shoot(127, 15);
	}

	else if (SensorBoolean[redSide2] == false)
	{
		Wait(false, 3);

		ForwardForDistance(127, 24);

		TurnRight(45, 127);

		ForwardForDistance(127, 60);

		Stop();

		Shoot(127, 15);
	}

	else if (SensorBoolean[redSide3] == false)
	{
		Wait(false, 3);

		ForwardForDistance(127, 72);

		Stop();

		Shoot(127, 15);
	}

	else if (SensorBoolean[redBack1] == false)
	{
		Wait(false, 3);

		ForwardForDistance(127, 24);

		TurnLeft(45, 127);

		ForwardForDistance(127, 48);

		Stop();

		Shoot(127, 15);
	}

	else if (SensorBoolean[redBack2] == false)
	{
		Wait(false, 3);

		ForwardForDistance(127, 24);

		TurnLeft(45, 127);

		ForwardForDistance(127, 60);

		Stop();

		Shoot(127, 15);
	}

	else if (SensorBoolean[redBack3] == false)
	{
		Wait(false, 3);

		ForwardForDistance(127, 72);

		Stop();

		Shoot(127, 15);
	}

	else // This will be our standard program or if we don't put the jumper in the correct port.
	{
		ForwardForDistance(127, 600);

		Stop();

		Wait(true, 3);

		ForwardForDistance(127, 6);

		Stop();
	}
}

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

// These integers are for the joystick values..... Variables are your best friend!!!
/*int	frontRightValue;
int backRightValue;
int frontLeftValue;
int backLeftValue;*/

//void SlowDownLaunchers()
//{
//	wait1Msec(500);

//	rightLauncherValue = rightLauncherValue - 5;
//	leftLauncherValue = leftLauncherValue - 5;

//	if (
//}

int rightLauncherValue;
int leftLauncherValue;
int topBeltValue;
int _threshold = 20;

task usercontrol()
{
	while (true)
	{

		// This is the main execution loop for the user control program. Each time through the loop
		// your program should update motor + servo values based on feedback from the joysticks.
		// Based on what wheels are installed, change the program between regular and mecanum wheels

		/*frontRightValue = 0;
		backRightValue = 0;
		frontLeftValue = 0;
		backLeftValue = 0;

		if (abs(vexRT[Ch2]) > 10)
		{
				frontRightValue = vexRT[Ch2];
				backRightValue = vexRT[Ch2];
		}
		if (abs(vexRT[Ch3]) > 10)
		{
				frontLeftValue = vexRT[Ch3];
				backLeftValue = vexRT[Ch3];
		}
		if (abs(vexRT[Ch2Xmtr2]) > 10)
		{
				frontRightValue = vexRT[Ch2Xmtr2];
				backRightValue = vexRT[Ch2Xmtr2];
		}
		if (abs(vexRT[Ch3Xmtr2]) > 10)
		{
				frontLeftValue = vexRT[Ch3Xmtr2];
				backLeftValue = vexRT[Ch3Xmtr2];
		}

		motor[frontRight] = frontRightValue;
		motor[backRight] = backRightValue;
		motor[frontLeft] = frontLeftValue;
		motor[backLeft] = backLeftValue;*/

		// Under here is the code for mecanum wheels
		//Create "deadzone" variables. Adjust threshold value to increase/decrease deadzone
	  int X2 = 0, Y1 = 0, X1 = 0, threshold = 15;

    //Create "deadzone" for Y1/Ch3
    if (abs(vexRT[Ch3]) > threshold)
    {
      Y1 = vexRT[Ch3];
    }
    else if (abs(vexRT[Ch3Xmtr2]) > threshold)
    {
    	Y1 = vexRT[Ch3Xmtr2];
    }
    else
    {
      Y1 = 0;
    }
    //Create "deadzone" for X1/Ch4
    if (abs(vexRT[Ch4]) > threshold)
    {
      X1 = vexRT[Ch4];
    }
    else if (abs(vexRT[Ch4Xmtr2]) > threshold)
    {
    	X1 = vexRT[Ch4Xmtr2];
    }
    else
    {
      X1 = 0;
    }
    //Create "deadzone" for X2/Ch1
    if (abs(vexRT[Ch1]) > threshold)
    {
      X2 = vexRT[Ch1];
    }
    else if (abs(vexRT[Ch1Xmtr2]) > threshold)
    {
    	X2 = vexRT[Ch1Xmtr2];
    }
    else
    {
      X2 = 0;
    }

    //Remote Control Commands
    motor[frontRight] = Y1 - X2 - X1;
    motor[backRight] =  Y1 - X2 + X1;
    motor[frontLeft] = Y1 + X2 + X1;
    motor[backLeft] =  Y1 + X2 - X1;

		rightLauncherValue = 0;
		leftLauncherValue = 0;

		if (vexRT[Btn5D] == 1 || vexRT[Btn5DXmtr2] == 1) // 1 means the button is pressed, and 0 means the button is not pressed
		{
			rightLauncherValue = 127; // The motors are spinning at full speed
			leftLauncherValue = 127;
		}

		if (vexRT[Btn5D] == 0 || vexRT[Btn5DXmtr2] == 0)
		{
			while (rightLauncherValue >= _threshold && leftLauncherValue >= _threshold)
			{
				wait1Msec(500);
				rightLauncherValue -= 5;
				leftLauncherValue -= 5;
			}

			if (rightLauncherValue <= _threshold && leftLauncherValue <= _threshold)
			{
				rightLauncherValue = 0;
				leftLauncherValue = 0;
			}
		}

		motor[rightLauncher] = rightLauncherValue;
		motor[leftLauncher] = leftLauncherValue;

		topBeltValue = 0;

		if (vexRT[Btn6U] == 1 || vexRT[Btn6UXmtr2] == 1)
		{
			topBeltValue = 127;
		}

		if (vexRT[Btn8D] == 1 || vexRT[Btn8DXmtr2] == 1)
		{
			topBeltValue = -127;
		}

		motor[topBelt] = topBeltValue;
	}
}