/* -- level3.nqc -- author: M.R.W. Dawson (2004) -- */
/* This is level 3 of the tortoise subsumption architecture */
/* The same as level 2, except front bump sensors will send it into reverse */


/* global variable definitions here */
int TURN;       /* constant that defines how long turning motor is stopped */
int FORWARD;    /* constant that defines how long driving motor is stopped */
int DARK;       /* constant that defines low lighting conditions */
int BRIGHT;     /* constant that defines bright lighting conditions */

/* Move the vehicle ahead by turning on motor C*/
task ahead() {
	while (true) {
		Fwd(OUT_C);
		On(OUT_C);
		Wait(25 * FORWARD);
		Float(OUT_C);
		Wait(25 * FORWARD);
	}
}

/* Move the vehicle backwards by turning on motor C*/
void reverse () {
	ClearTimer(2);    /* clear a timer */
	do {
		Rev(OUT_C);			/* put drive motor in reverse */
		On(OUT_C);
		Wait(25 * FORWARD);     /* run the motor */
		steer(10);			/* steer a bit */
		Float(OUT_C);		/* stop the motor */
		Wait(25 * FORWARD);	/* wait a bit */
	}
	while (Timer(2) < 20);    /* reverse for 2 seconds */
}


/* Turn on the turning motor to steer the motor */
void steer (int steer_time) {  	/* steer_time divided by 10 = seconds of steering */
	ClearTimer(0);   			/* reset timer 0 to zero */
	do {
		ClearTimer(1);   		/* reset timer 1 to zero */
		do {
			OnFwd (OUT_A);  	/* turn the steering motor on */
		    }
		while (Timer(1) < 1); 	/* for just a tenth of a second */
		ClearTimer(1);		/* reset timer 1 to zero */
		do {
			Off (OUT_A);	/* turn off the steering motor */
		    }
		while (Timer(1) < (2 * TURN)); /* until TURN time is reached */
		}
	while (Timer(0) < steer_time);	/* repeat all this until steer_time is reached */	
}

/* Set the turning constant according to light sensed */
task see_light () {
	while (true) {
		if ((SENSOR_2 > BRIGHT) && (SENSOR_2 < DARK)) /* definition of moderate light */
			{Off (OUT_A); }       /* no turning! */
		else 
				{SetPower (OUT_A, 2); TURN = 4; steer(10);}       /* moderate turning */
      	
	}
}

task touch_sense() {
while (true)
{
	if ((SENSOR_1 == 1) || (SENSOR_3 == 1))
		{
		stop see_light; /* stop the level 2 task */
		stop ahead;     /* stop the level 0 task */
		Float(OUT_A + OUT_C);
		if (SENSOR_1 == 1)
			{reverse();}
		start ahead;      /* restart the level 0 task */
		start see_light; /* restart the level 2 task */
		}
}
}


task main(){

/* define the sensor and its mode */
  SetSensorType(SENSOR_2, SENSOR_TYPE_LIGHT);
  SetSensorMode(SENSOR_2, SENSOR_MODE_RAW);
  SelectDisplay (DISPLAY_SENSOR_2);
/* set the power to the two motors */
  SetPower(OUT_A, 2);  /* turning motor */
  SetPower(OUT_C, 4);  /* drive motor */
  

FORWARD = 1;  		/* set the drive motor constant */
TURN = 10;     		/* initialize the turn motor constant */
DARK = 800;   		/* light readings larger than this number are dark */
BRIGHT = 500; 		/* light readings higher than this number are bright */

start ahead;     		/* start the level 0 task */
start see_light; 		/* start the level 2 task, which contols the level 1 task */
start touch_sense; 	/* start the level 3 task */
}