#pragma config(Sensor, S1,     touchSensor,    sensorEV3_Touch)
#pragma config(Sensor, S2,     touchSensor2,   sensorEV3_Touch)
#pragma config(Sensor, S3,     colorSensor,    sensorEV3_Color)
#pragma config(Sensor, S4,     sonarSensor,    sensorEV3_Ultrasonic)
#pragma config(Motor,  motorB,          rightMotor,    tmotorEV3_Large, PIDControl, driveRight, encoder)
#pragma config(Motor,  motorC,          leftMotor,     tmotorEV3_Large, PIDControl, driveLeft, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
Create a program to control the positive and negative speed of the robot by a press of two
Touch Sensors. Display the speed on the EV3 screen.
*/

//Create an integer (whole number) variable to store our speed value.
int speed = 0;

void Accelerate()
{
	//When I press the touch sensor button.
	if(getTouchValue(touchSensor) == 1)
	{
		//Add 10 to our 'speed' variable
		if(speed < 100) speed = speed + 10;

		//Create a loop to wait for the touch sensor button to be released.
		while(getTouchValue(touchSensor) == 1)
		{
			//Wait for button to be released
			sleep(10);
		}
		//Set motorB and motorC speed to the value of the 'speed' variable.
		setMotorSpeed(motorB, speed);
		setMotorSpeed(motorC, speed);
	}
}

void Decelerate()
{
	//When I press the touch sensor #2 button.
	if(getTouchValue(touchSensor2) == 1)
	{
		//Subtract 10 from our 'speed' variable.
			if(speed > -100) speed = speed - 10;

		//Create a loop to wait for the touch sensor #2 button to be released.
		while(getTouchValue(touchSensor2) == 1)
		{
			//Wait for button to be released
			sleep(10);
		}
		//Set motorB and motorC speed to the value of the 'speed' variable.
		setMotorSpeed(motorB, speed);
		setMotorSpeed(motorC, speed);
	}
}

task main()
{
	//Create a string to store our text to be displayed to the LCD.
	string displaySpeed;

	//Repeat our control loop forever.
	while(true)
	{
		Accelerate();
		Decelerate();

		//Format our string to display "Speed: 50" or similar values.
		stringFormat(displaySpeed, "Speed: %d   ", speed);

		//Draw the string on the LCD screen.
		drawTextAt(49, 54, displaySpeed);

		//Set motorB and motorC speed to the value of the 'speed' variable.
		setMotorSpeed(motorB, speed);
		setMotorSpeed(motorC, speed);
	}
}