/*
 * This program uses my IRPD device tuned for the RCX
 * robot.  Motors A and C drive right and left treads,
 * The percentage margins decode to left/right/both/none.
 * Rev 4/7/99
 */

/* these define the inputs/outputs used */
#define FrontBumper IN_3
#define EYE	IN_2
#define LEFT	OUT_C
#define RIGHT	OUT_A

#define FULL_SPEED 10
#define NORMAL_SPEED 7
#define TURN_SPEED 5
#define DELAY      20
#define LBOTH 5
#define LNONE 90
#define LLEFT 55
#define LRIGHT 13

int eye;
int lastturn;
int bumping;


task main
{
       Sensor(FrontBumper, IN_SWITCH);
       Sensor(IN_2, IN_CFG(STYPE_LIGHT,  SMODE_PERCENT));
       bumping = 0;     //Not currently running bump mode
       start frontBump;
       
	Fwd(RIGHT+LEFT, NORMAL_SPEED);

	while(true)
	{
             if (bumping == 0) {    //Only do this if bumping is not in force!
		eye = EYE;
		
		if (eye < LBOTH)
		/* Something dead ahead */
		{
                    Rev(LEFT + RIGHT, NORMAL_SPEED);
                    Sleep(100);
                    //wait(EYE == LNONE);
                    if (lastturn == LLEFT) 
                    {
                       Fwd(LEFT,TURN_SPEED);
                       Rev(RIGHT,TURN_SPEED);
                       //wait(EYE > LNONE);
                       Sleep(50);
                       lastturn = LRIGHT;
                    } else 
                    {
                       Fwd(RIGHT,TURN_SPEED);
                       Rev(LEFT,TURN_SPEED);
                       //wait(EYE > LNONE);
                       Sleep(50);
                       lastturn = LLEFT;
                    }
                       
		}
		else if (eye < LLEFT && eye > LRIGHT)
		/* Something on the right */
		{
			Fwd(LEFT, TURN_SPEED);
			Rev(RIGHT, TURN_SPEED);
                          Sleep(DELAY);		
			//wait(EYE > LNONE);
		}
                else if  (eye > LLEFT && eye < LNONE)
		/* Something on the Left */
                {
 			Fwd(RIGHT, TURN_SPEED);
			Rev(LEFT, TURN_SPEED);
			Sleep(DELAY);
                          //wait(EYE > LNONE);
                }
		else
			continue;
		
		Fwd(RIGHT+LEFT, NORMAL_SPEED);
	}
    }
}

task frontBump {
    
	while (true) 
	{
		wait(FrontBumper == 1);
                   bumping = 1;                     //flag bumping!
                   Rev(LEFT + RIGHT, NORMAL_SPEED);
                    Sleep(100);
                    //wait(EYE == LNONE);
                    if (lastturn == LLEFT) 
                    {
                       Fwd(LEFT,TURN_SPEED);
                       Rev(RIGHT,TURN_SPEED);
                       //wait(EYE > LNONE);
                       Sleep(50);
                       lastturn = LRIGHT;
                    } else 
                    {
                       Fwd(RIGHT,TURN_SPEED);
                       Rev(LEFT,TURN_SPEED);
                       //wait(EYE > LNONE);
                       Sleep(50);
                       lastturn = LLEFT;
                    }

		wait(FrontBumper == 0);
		Fwd(RIGHT+LEFT, NORMAL_SPEED);
                 bumping = 0;                 //Done with bumping
	}
}