#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     HTIRS2,         sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     nxtSonar,       sensorSONAR)
#pragma config(Sensor, S4,     nxtColor,       sensorCOLORFULL)
#pragma config(Motor,  motorA,          tubeArm,       tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     tennisArm,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop, encoder)
#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)

#include "JoystickDriver.c"
#pragma debuggerWindows("joystickSimple");

task main()
{
	while(true) {
		getJoystickSettings(joystick);

		if(joystick.joy1_y1 < 10 && joystick.joy1_y1 > -10) {		//Controls Left Motor
			motor[leftMotor] = 0;
			} else {
			motor[leftMotor] = joystick.joy1_y1 * 0.4;
		}

		if(joystick.joy1_y2 < 10 && joystick.joy1_y2 > -10) {		//Controls Right Motor
			motor[rightMotor] = 0;
			} else {
			motor[rightMotor] = joystick.joy1_y2 * 0.4;
		}

		if(joystick.joy2_y1 < 10 && joystick.joy2_y1 > -10) {		//Controls Ball Catcher
			motor[tennisArm] = 0;
			} else {
			motor[tennisArm] = joystick.joy2_y1 * 0.2;
			wait1Msec(150);
		}

		if(joystick.joy2_y2 < 10 && joystick.joy2_y2 > -10) {		//Controls Pulley


		}

		if(joy2Btn(1)) {
			motor[tubeArm] = 40;
			wait1Msec(150);
		} else {
			motor[tubeArm] = 0;
		}

		if(joy2Btn(2)) {
			motor[tubeArm] = -40;
			wait1Msec(150);
		} else {
			motor[tubeArm] = 0;
		}

	}

}