#pragma config(Sensor, S1,     wallSensor,     sensorSONAR)
#pragma config(Sensor, S2,     ballSensor,     sensorEV3_Ultrasonic)
#pragma config(Sensor, S3,     gyroSensor,     sensorEV3_Gyro, modeEV3Gyro_Rate)
#pragma config(Sensor, S4,     colorSensor,    sensorEV3_Color, modeEV3Color_Color)
#pragma config(Motor,  motorA,          arm,           tmotorEV3_Medium, PIDControl, encoder)
#pragma config(Motor,  motorB,          leftWheel,     tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          rightWheel,    tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard

// create a global variable to hold the heading value
int heading = 0;

// separate task to calculate heading
task getHeading () {

	int gyroRate = 0;
	int sleepTime = 250;	// read the heading value 4 times per second
	int sensorReadRate = 1000/sleepTime;

	while (true) {

		// get the angular velocity (degrees per second)
		gyroRate = SensorValue[gyroSensor];
		displayCenteredBigTextLine(2, "Gyro: %d", gyroRate);

		// convert the angular velocity into degrees and add it
		// to the heading value.
		heading = heading + gyroRate/sensorReadRate;

		// keep the heading value between 0 and 360 degrees
		heading = heading % 360;
		if (heading < 0) {
			heading = 360 + heading;
		}
		sleep(sleepTime);
	}
}

task main()
{

	startTask(getHeading);

	while(true){
		displayCenteredBigTextLine(10, "Heading: %d", heading);
		float degreesToTurn =
	}
}