#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,                     tServoNormal)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,                     tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

#define JOY1_BUTTON(x) joystick.joy1_Buttons >> x-1 & 0x1

void initializeRobot()
{
	return;
}

void autonomous()
{
	motor[motorD] = 255;
	motor[motorE] = 255;

	while (1)
		;
}

void teleop()
{
	/* Reset motors */
	motor[motorD] = 0;
	motor[motorE] = 0;

	/* Reset servos */
	servo[servo1] = 0;
	servo[servo2] = 0;

	while (1) {
		getJoystickSettings(joystick);

		if (joystick.joy1_y1 > 10) {
			motor[motorD] = 100;
		} else if (joystick.joy1_y1 < -10) {
			motor[motorD] = -100;
		} else {
			motor[motorD] = 0;
		}

		if (joystick.joy1_y2 > 10) {
			motor[motorE] = 100;
		} else if (joystick.joy1_y2 < -10) {
			motor[motorE] = -100;
		} else {
			motor[motorE] = 0;
		}

		if (JOY1_BUTTON(3) == 1) {
			motor[motorF] = 255;
		} else {
			motor[motorF] = 0;
		}

		if (JOY1_BUTTON(1) == 1) {
	servo[servo1] = 200;
	servo[servo2] = 55;
		} else {
	servo[servo1] = 255;
	servo[servo2] = 0;
		}
	}
}

task main()
{
	initializeRobot();

	waitForStart();

	if (joystick.UserMode) {
		teleop();
	} else {
		autonomous();
	}
}