#pragma config(Sensor, in1,    fright,         sensorLineFollower)
#pragma config(Sensor, in2,    fleft,          sensorLineFollower)
#pragma config(Sensor, in3,    lightsensor,    sensorReflection)
#pragma config(Sensor, dgtl1,  red,            sensorLEDtoVCC)
#pragma config(Sensor, dgtl2,  yellow,         sensorLEDtoVCC)
#pragma config(Sensor, dgtl3,  green,          sensorLEDtoVCC)
#pragma config(Sensor, dgtl4,  touchclaw,      sensorTouch)
#pragma config(Sensor, dgtl5,  sonar,          sensorSONAR_inch)
#pragma config(Motor,  port1,           mleft,         tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port3,           headlight,     tmotorVexFlashlight, openLoop, reversed)
#pragma config(Motor,  port6,           mclaw,         tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           marm,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          mright,        tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
	turnLEDOff(green);
	turnLEDOff(yellow);
	turnLEDOff(red);
	while(1==1)
	{
		while(SensorValue(lightsensor)<501)
		{
			turnFlashlightOff(headlight);
			if(SensorValue(fleft)>1000)
			{
				turnLEDOff(green);
				turnLEDOn(yellow);
				startMotor(mleft,-32);
				startMotor(mright,-50);
			}
			if(SensorValue(fright)>1000)
			{
				turnLEDOff(green);
				turnLEDOn(red);
				startMotor(mleft,50);
				startMotor(mright,32);
			}
			else
			{
				turnLEDOn(green);
				turnLEDOff(yellow);
				startMotor(mleft,-32);
				startMotor(mright,32);
			}
		}
		if(SensorValue(touchclaw)>0)
		{
			turnLEDOff(green);
			turnLEDOn(yellow);
			turnLEDOff(red);
			turnFlashlightOn(headlight,-127);
			stopMotor(mleft);
			stopMotor(mright);
			startMotor(mclaw,-32);
			wait(1);
			stopMotor(mclaw);
			turnLEDOn(green);
			turnLEDOff(yellow);
			startMotor(mleft,90);
			startMotor(mright,-90);
			wait(5);
			stopMotor(mleft);
			stopMotor(mright);
			while(1==1)
			{
				turnFlashlightOff(headlight);
				if(SensorValue(sonar)<7.5) // inches
				{
					turnLEDOn(green);
					turnLEDOff(yellow);
					stopMotor(mleft);
					stopMotor(mright);
					startMotor(marm,-100);
					wait(2.25);
					stopMotor(marm);
					startMotor(mclaw,32);
					wait(1);
					turnLEDOn(yellow); // victory screech
					turnLEDOn(red);
					wait(0.5);
					turnLEDOff(green);
					turnLEDOff(yellow);
					turnLEDOff(red);
					wait(0.5);
					turnLEDOn(green);
					turnLEDOn(yellow);
					turnLEDOn(red);
					wait(0.5);
					turnLEDOff(green);
					turnLEDOff(yellow);
					turnLEDOff(red);
					wait(0.5);
					turnLEDOn(green);
					turnLEDOn(yellow);
					turnLEDOn(red);
					wait(0.5);
					turnLEDOff(green);
					turnLEDOff(yellow);
					turnLEDOff(red);
					wait(0.5);
					turnLEDOn(green);
					turnLEDOn(yellow);
					turnLEDOn(red);
					stopAllTasks();
				}
				else
				{
					turnLEDOff(green);
					turnLEDOn(yellow);
					turnLEDOff(red);
					startMotor(mleft,32);
					startMotor(mright,-32);
				}
			}
		}
		else
		{
			turnLEDOn(green);
			turnLEDOff(yellow);
			turnLEDOff(red);
			turnFlashlightOn(headlight,-127);
			startMotor(mleft,-32);
			startMotor(mright,32);
		}
	}
}