/* Michael R.W. Dawson -- vehicle2 code -- freeware -- July 9, 2003 */

/* global variable definitions here */
int TIME1,TIME3;
#define PAUSETIME 2 /* determines how long system waits */
#define RUNTIME 50  /* determines how long system runs */

/* This code is NQC stuff for creating a robot that is similar to
   Braitenberg vehicle 2.  It uses the amount of light measured by
   a sensor to determine how to run a motor.  Because the Lego motors
   are low in torque, instead of setting motor speed on the basis of a
   light sensor, the light sensor determines how long a time that a 
   motor will be turned on. */

/* This routine runs one motor for a time determined by the difference between sensors*/
task motor1() {
while (true)
  {
	/* Phase 1 -- forward motion, run the motor for a period of time */

	Float(OUT_A);  /* Float motor A (stop it, but don't put on the brake) */
	OnFwd(OUT_A);  /* Turn motor A on in the forward direction */
	Wait(RUNTIME); /* Wait for the runtime amount of time */
	Float(OUT_A);  /* Float the motor at the end of running it */

	/* Phase 2 use sensors to stop or run motor for a variable length of time */

	TIME1 = SENSOR_3 - SENSOR_1;  /* compute a time using the difference between sensors */
	if (TIME1 < 0)
		{
			TIME1 = TIME1 * -1; /* make the time positive */
		 	Float(OUT_A);	  /* float the motor */
			Wait(TIME1 * PAUSETIME); /* float it for this amount of time */
		}
	else
		{
			OnFwd(OUT_A);     /* run the motor forward */
			Wait(TIME1 * PAUSETIME);  /* do this for the computed time */
		}
  }
}

/* This routine runs the other motor for a time determined by the other light sensor.  It's code
is nearly identical to the code above, so I haven't bothered to provide detailed
comments on it.*/

task motor2() {
while (true)
  {
	Float(OUT_C);  /* note that this works with motor C */
	OnFwd(OUT_C);
	Wait(RUNTIME);
	Float(OUT_C);
	TIME3 = SENSOR_1 - SENSOR_3;  /* compare to task motor 1 */
	if (TIME3 < 0)
		{
			TIME3 = TIME3 * -1;    /* TIME3 will be different than TIME1 -- vehicle will therfore turn */
		 	Float(OUT_C);
			Wait(TIME3 * PAUSETIME);
		}
	else
		{
			OnFwd(OUT_C);
			Wait(TIME3 * PAUSETIME);
		}
  }
}



task main(){

  SetSensorType(SENSOR_1, SENSOR_TYPE_LIGHT); /* define as light sensor */
  SetSensorType(SENSOR_3, SENSOR_TYPE_LIGHT); /* ditto */
  SetSensorMode(SENSOR_1, SENSOR_MODE_RAW); /* raw mode works best! */
  SetSensorMode(SENSOR_3, SENSOR_MODE_RAW); /* ditto */
  SetPower(OUT_A + OUT_C, 4);               /* motors run at speed = 4 */
  SelectDisplay (DISPLAY_SENSOR_1);		  /* display sensor 1 for debugging behaviour */

start motor1;                              /* start motor 1 task */
start motor2;                              /* start motor 2 task */

/* the two above tasks will drive the two motors (at different "speeds") indefinitely
   until the robot is stopped or turned off */

}