code

#pragma config(Sensor, port2,  gyro_sensor,    sensorVexIQ_Gyro)
#pragma config(Sensor, port3,  bumper_sensor,  sensorVexIQ_Touch)
#pragma config(Sensor, port4,  colour_sensor,  sensorVexIQ_Color12Color)
#pragma config(Sensor, port5,  distance_sensor, sensorVexIQ_Distance)
#pragma config(Sensor, port7,  touch_led,      sensorVexIQ_LED)
#pragma config(Motor,  motor1,          left_motor,    tmotorVexIQ, PIDControl, driveLeft, encoder)
#pragma config(Motor,  motor6,          right_motor,   tmotorVexIQ, PIDControl, reversed, driveRight, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
	bool finish = true;
	while (finish)
	{
		if (getTouchLEDValue(touch_led) == 1)
		{
			while (getTouchLEDValue(touch_led) == 0)
			{
				setMultipleMotors(75, left_motor, right_motor);
				if (getColorName(colour_sensor) == colorRed)
				{
					finish = false;
				}
				if (getBumperValue(bumper_sensor) == 1)
				{
					if (getDistanceValue(distance_sensor) < 350)
					{
						backward(90,degrees, 50);
						resetGyro(gyro_sensor);
						while (getGyroDegrees(gyro_sensor) < 80)
						{
							setMotor(left_motor, -50);
							setMotor(right_motor, 50);
						}
						stopAllMotors();
					}
					else
					{
						backward(90,degrees, 50);
						resetGyro(gyro_sensor);
						while(getGyroDegrees(gyro_sensor) > -80)
						{
							setMotor(left_motor, 50);
							setMotor(right_motor, -50);
						}
						stopAllMotors();
					}
				}
			}
		}
	}
}