#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(Sensor, S3,     IRSeeker2,      sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     motor_flag,    tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motor_lin2,    tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motor_left1,   tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motor_left2,   tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     motor_right1,  tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     motor_right2,  tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     notused,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_2,     motor_lin4,    tmotorTetrix, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoContinuousRotation)
#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";
/**
* Helper function to scale analog joystick values for tetrix dc motors.
*/

void initializeRobot()	//don't touch
{//absolutely nothing
 					//Set all motor rotation counts to 0
	nMotorEncoder(motor_lin2) =0;//E
	nMotorEncoder(motor_lin4) =0;//G

}
int scaleForMotor(int joyValue)
{

const int DEADZONE = 5;
const int MAX_MOTOR_VAL = 154;	//don't touch
const float MAX_JOY_VAL = 127;

//check for deadzone
if(abs(joyValue) < DEADZONE) {
return 0;
}

//calculate scaled value
int direction = joyValue / abs(joyValue); // 1 or -1
float ratio = ((joyValue * joyValue) / (MAX_JOY_VAL * MAX_JOY_VAL));
int scaledVal = (ratio * MAX_MOTOR_VAL) * direction;	//don't touch

return scaledVal;
}


task main()
{

	initializeRobot();

 	waitForStart();	//wait for field control system to start

while(true)
{



	int Dm = nMotorEncoder(motor_left1);	//Save the current rotation values to integers Dm, Em, Fm, and Gm for future
	int Em = nMotorEncoder(motor_left2);
	int Fm = nMotorEncoder(motor_right1);
	int Gm = nMotorEncoder(motor_right2);

	//Display rotation values of the motors on the 1st, 2nd, 3rd, and 4th lines of the NXT display.
nxtDisplayTextLine(2, "%d", nMotorEncoder(motor_lin2));
nxtDisplayTextLine(4, "%d", nMotorEncoder(motor_lin4));

getJoystickSettings(joystick);	//receive inputs from controllers


motor[motor_left1] = scaleForMotor(joystick.joy1_y1/-127*150);
motor[motor_left2] = scaleForMotor(joystick.joy1_y1/-127*150);
motor[motor_right1] = scaleForMotor(joystick.joy1_y2/127*150);
motor[motor_right2] = scaleForMotor(joystick.joy1_y2/127*150);	//tank controls

if (joy2Btn(4) == 1) //controller2_Y_slide_up

	{
		motor(motor_lin2) = -60;
		motor(motor_lin4) = 60;
		nxtDisplayTextLine(1, "slide down");	//print to NXT display
	}
else if (joy1Btn(8) == 1) //controller1_lefttrigger_sweeper_in

	{
		motor(motorA) = -60;
		motor(motorB) = -60;
		nxtDisplayTextLine(1, "sweeper in");
	}

	else if (joy1Btn(1) == 1) //controller1_Y_armdown

	{
		//servo(servo3) = 1;
		nxtDisplayTextLine(1, "sweeper in");
	}

else if (joy2Btn(2) == 1) //controller2_A_slide_down

	{
		if (nMotorEncoder(motor_lin4) > -8300) //made up number for now
		{
		motor(motor_lin2) = 60;
		motor(motor_lin4) = -60;
		nxtDisplayTextLine(1, "slide up");
		}

	}

else if (joy1Btn(7) == 1) //controller1_lefttrigger_Y_sweeperout

	{
		motor(motorA) = 60;
		motor(motorB) = 60;
		nxtDisplayTextLine(1, "sweeper out");
	}

else if (joy1Btn(2) == 1) //controller1_A_bucket_down

	{

		int count = 1;
		while(count == 1)
		{
		servo(servo1) = 120;
		servo(servo2) = 136;
		nxtDisplayTextLine(1, "bucket down");

		if (joy1Btn(2) == 1)
		{
			count++;
		}
	}
	}

	else if (joy1Btn(3) == 1) //controller1_B_bucket_up

	{

		int count = 1;
		while(count == 1)
		{
		servo(servo1) = 86;
		servo(servo2) = 170;
		nxtDisplayTextLine(1, "bucket down");

		if (joy1Btn(3) == 1)
		{
			count++;
		}
	}

	}

	else if (joy1Btn(4) == 1) //controller1_Y_armmid

	{

		int count = 1;
		while(count == 1)
		{
		servo(servo1) = 46;
		servo(servo2) = 210;

		nxtDisplayTextLine(1, "bucket down");
		if (joy1Btn(4) == 1)
		{
			count++;
		}
	}

	}


else if (joy2Btn(1) == 1)
{
	motor(motor_flag) = 60;
}
else
{
	if (joy2Btn(3) == 1)
	{
	motor(motor_flag) = -60;
}
else //absolutely nothing
	{
		//servo(servo1) = 255;
		//servo(servo2) = 1;
		motor(motorA) = 0;	//Every motor stopped, servos back to its position
		motor(motorB) = 0;
		motor(motor_lin2) = 0;
		motor(motor_lin4) = 0;
		motor(motor_flag) = 0;
		nxtDisplayTextLine(1, "slide stop");

		nxtDisplayTextLine(1, "Nothing, bucket up");
	}

	}
}
}