#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     lin1,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     lin2,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorD,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorF,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     motorE,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     motorG,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     lin3,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     lin4,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c";
void initializeRobot(){
	nMotorEncoder(motorD) =0;
	nMotorEncoder(motorE) =0;
	nMotorEncoder(motorF) =0;
	nMotorEncoder(motorG) =0;
	return;
}
void move(int direction, float revs);
void turn(int direction, float revs);
int fullrev = 1440;
task main()
{
initializeRobot(); //Prepares robot, communicates with Field control system.

waitForStart(); // Wait for the beginning of autonomous phase.
move(1,6);
}
//This function is for turning. The "int" is actually a boolean. Use 0 for a left turn, 1 for a right turn.
void turn(int direction, float revs)
{
	nMotorEncoder(motorD) =0;
	nMotorEncoder(motorE) =0;
	nMotorEncoder(motorF) =0;
	nMotorEncoder(motorG) =0;
	if (direction==0)
	{
		while(nMotorEncoder(motorD) > -(fullrev*revs) & nMotorEncoder(motorE) > -(fullrev*revs) & nMotorEncoder(motorF) > -(fullrev*revs) & nMotorEncoder(motorG) > -(fullrev*revs))
		{
			motor(motorD) = -60;
			motor(motorE) = -60;
			motor(motorF) = -60;
			motor(motorG) = -60;
		}
	}
	else
	{
		while(nMotorEncoder(motorD) < (fullrev*revs) & nMotorEncoder(motorE) < (fullrev*revs) & nMotorEncoder(motorF) < (fullrev*revs) & nMotorEncoder(motorG) < (fullrev*revs))
		{
			motor(motorD) = 60;
			motor(motorE) = 60;
			motor(motorF) = 60;
			motor(motorG) = 60;
		}
	}
}

//This function is for moving. The "int" is actually a boolean. Use 0 for forward, 1 for backward.
void move(int direction, float revs)
{
	nMotorEncoder(motorD) =0;
	nMotorEncoder(motorE) =0;
	nMotorEncoder(motorF) =0;
	nMotorEncoder(motorG) =0;
	if (direction==0)
	{
		while(nMotorEncoder(motorD) > -(fullrev*revs) & nMotorEncoder(motorE) < (fullrev*revs) & nMotorEncoder(motorF) > -(fullrev*revs) & nMotorEncoder(motorG) < (fullrev*revs))
		{
			motor(motorD) = -60;
			motor(motorE) = 60;
			motor(motorF) = -60;
			motor(motorG) = 60;
		}
	}
	else
	{
		while(nMotorEncoder(motorD) < (fullrev*revs) & nMotorEncoder(motorE) > -(fullrev*revs) & nMotorEncoder(motorF) < (fullrev*revs) & nMotorEncoder(motorG) > -(fullrev*revs))
		{
			motor(motorD) = 60;
			motor(motorE) = -60;
			motor(motorF) = 60;
			motor(motorG) = -60;
		}
	}
}