//*!!Code automatically generated by 'ROBOTC' configuration wizard.              !!*//
//*!!Analog,     1,                     , ,                                      !!*//
//*!!Camera,     0,                     , ,                                      !!*//
//*!!PSCtrl,     0,                     , ,                                      !!*//
//*!!Sensor,   in1,                 gyro, sensorGyro,      ,                     !!*//
//*!!Motor,  port1,                 left, tmotorNormal,                          !!*//
//*!!Motor,  port2,                right, tmotorNormal,                          !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors gyro                 = (tSensors) in1;  //sensorGyro         //*!!!!*//
const tMotor   left                 = (tMotor) port1;  //tmotorNormal       //*!!!!*//
const tMotor   right                = (tMotor) port2;  //tmotorNormal       //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

void straight(int speed, int time);
void turn_left(int lspeed, int ltime);
void stop();

task main()
{
	//Objective: Go straight with Gyro Sensor for 10 Seconds
	//Then turn 90 degrees and stop

	bMotorReflected[port2] = true;
	wait1Msec(300);

	straight(30, 1500);
	SensorValue[gyro] = 0;
	turn_left(40, 1000);
	stop();
}

void straight(int speed, int time)
{

	ClearTimer(T1);

	while(time1[T1] < time)
	{
			if(SensorValue[gyro] > 0)
			{
			motor[left] =  speed;
			motor[right] =   speed + 20;
			}
			else
			{
			motor[left] =   speed + 20;
			motor[right] =  speed;
			}
	}
}

void turn_left(int lspeed, int ltime)
{
	int gyro_sensor = 0;
	int degree_of_turn = -900; //degrees of turn - in tenths
	int error_amount = 50;    //degree of error (10 degree range) - in tenths

	ClearTimer(T1);

	while(true)
	{
		if(time1[T1] > ltime)
		{
		  break;
		}

		gyro_sensor = SensorValue[gyro];

		if(gyro_sensor < (degree_of_turn - error_amount))
		{
			motor[left] = lspeed;
			motor[right] = -lspeed;
		}

		if(gyro_sensor > (degree_of_turn + error_amount))
		{
			motor[left] = -lspeed;
			motor[right] = lspeed;
		}

		if(gyro_sensor > (degree_of_turn - error_amount) && gyro_sensor < (degree_of_turn + error_amount) )
		{
			motor[left] = 0;
			motor[right] = 0;
		}
	}
}

void stop()
{
	motor[port1] = 0;
	motor[port2] = 0;
	wait1Msec(500);
}