#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S4,     gyro,           sensorI2CHiTechnicGyro)
#pragma config(Motor,  mtr_S1_C1_1,     left,          tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     left2,         tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     right,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     right2,        tmotorTetrix, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

/*For this code to work you need to change these two functions. You need to change what your motors are called. You need
	to adapt for how many drive motors you have on each side, we have 2.

	You are welcome to change the code and improve upon it. ENJOY THE CODE

	Thanks and good luck this year,

	Stark Industries


*/
void MoveLeftSide(int speed)
{
	motor[left] = speed;
	motor[left2] = speed;

	/*If you only have one motor on the left side called MOTORA you would replace the top two lines of code with this

	motor[MOTORA] = speed;

	*/
}

void MoveRightSide(int speed)
{
	motor[right] = speed;
	motor[right2] = speed;

	/*If you only have one motor on the right side called MOTORB you would replace the top two lines of code with this

	motor[MOTORB] = speed;

	*/
}
/* You can change these two lines of code if you want to adjust the speed of your motors while turning. We set the program

	So that during a turn our motors are not going more than 80% power and no less than 40% power. We did this in an attempt

	To keep our motors from burning out
*/

int maxValue = 80;
int minValue = 40;

/*
	You need to either name your gyro sensor to 'gyro' or you can change the code right here.

*/
float getAverage(int acurate,int wait)//Get the average reading of the gyro based on the parameter
{																			// accurate which tells how many reading to take and how long between the samples
	long sums = 0;											//value takes the sums of every value
	for(int i = 0; i < acurate ;i++)		//loop that goes through accurate number of iterations
	{
		sums += SensorValue[gyro];				///////////////////CHANGE THE NAME gyro TO WHAT YOUR PROGRAM CALLS THE GYRO SENSOR
		wait1Msec(wait);
	}

	sums = sums/accurate - 599; //sets sums to the average value of the loop minus 600 because of how the gyro values are given


	return sums; //returns sums
}


void MoveBothSides(int speed)
{
	MoveLeftSide(speed);
	MoveRightSide(speed);
}


void turnDir(int direction,int speed,int wait)//Code called by the gyro program turn that turns the appropriate direction for the
//																						time along with the correct speed
{
		if(speed>maxValue)						//make sure the values are not greater than the maxValue, if so then set the value to maxValue
			speed = maxValue;
		if(speed < minValue)						//make sure the values are not less than the minValue, if so then set the value to minValue
			speed = minValue;
		if (direction == 1) // if direction is left, then turn left
		{
			MoveLeftSide(speed);
			MoveRightSide(-speed);
		}
		else 							//if direction is right, then turn right
		{
			MoveLeftSide(-speed);
			MoveRightSide(speed);

		}
		wait1Msec(wait);	//wait for the appropriate time
}
void stopAll()//stops all motors
{
	MoveBothSides(0);
}
void turn(float tar)//code that uses gyro values to turn more precisely
{
	tar *= -.77777777778; 			//multiplies the digit to turn by a constant determined by repeated testing
	int buffer = 5;					//buffer range to still accept the gyro values
	int turn;								//variable used to store which direction to go
	if(tar > 0)							// if going left, then direction is left
		turn = -1;
	else
		turn = 1;							//if going right, then direction is right
	stopAll();							//stop all the motors
	float speed = getAverage(10,10)*turn;	// speed is equal to the average of the gyro multiplied by the turn direction

	if(speed < buffer&& speed > -buffer) // if speed is within the buffer then set speed to 0
		speed = 0;


	float deg = tar; // set another variable to the value of the goal turn angle

	ClearTimer(T1);			//clear the timer
	wait1Msec(500);			//wait a short time before going into loop

	while(turn *deg < 0) // if the direction multiplied by degrees still needed to turn is less than 0, than keep turning
	{

		ClearTimer(T1);// reset the timer

		if(turn *deg < 15)//if we are more than 15 degrees from the goal, than turn at max motor speeds
				turnDir(turn,maxValue,0);
		else if (turn*deg <0)	// if we are less than 15 degrees from the goal and turning left, then turn less so we don't overshoot
			turnDir(turn,(maxValue-((15-deg)*5)),0);
		else if (turn*deg >15)//if we are greater than 15 degrees from the goal, then turn at max motor speeds
				turnDir(turn,maxValue,0);
		else					//if we are less than 15 degrees from the goal and turning right, then turn less so we don’t overshoot
				turnDir(turn,(maxValue-((deg-15)*5)),0);


		speed = getAverage(1,0)*turn;		//update the speed variable to the current readings


		float time = time1[T1]; // get how long the timer has been going to calculate how many degrees we have already turned
		if(turn == 1)//if we are tunring left, than increase the value deg
			deg += ((time/1000)*speed);
	else					//if we are turning right, than decrease the value deg

		deg -= ((time/1000)*speed);
	}


}
task main()
{
turn(90);//Demo This will turn the robot 90 degrees to the right
//You can give this code positive values to turn clockwise
//You can give this code negative values to turn counter-clockwise
//You can give this code decimals or fractions it will accept and act accordingly


}