/*servo_driver.nqc  Mohamad Hassoun  April 6, 2004
   
   NQC program to test the converted FUTABA S3003 servo. The servo is modified
   so that all internal electronics are bypassed (removed) except for the motor
   pot. A Lego RCX connector/wire connects RCX PORTA to power the motor. Another
   connector connects the 1st and 2nd pot pins to RCX SENSOR1. Therefore, the
   modified servo now requires 1 input and 1 output ports. The pot registers
   readings on the RCX SENSOR (sensor ports 1 is configured as TOUCH/RAW)
   in the range 20 - 320.
   
   Two parameters (in this simple servo driver program) control the speed,
   accuracy, and stability of the converted servo:
   1. TOL determine angular resolution. useful values for this parameter are in
   the range 1 - 5.  Small TOL values increase angular resolution at the cost
   of oscillation.
   2. SLOW controls the effective power level supplied to the servo motor.
   Useful values for SLOW are in the range 1 - 100. The larger values lead to
   slower servo operation; however, this negatively affects servo convergence
   (i.e., leads to pronounced oscillations).

   As tested, the values TOL = 8 and SLOW = 10 are a good compromize.
   
   The angle resolution is specified in increments of 22.5 degrees. This allows
   for resolving 9 servo positions (0 degrees through 180 degrees).
   The angle values are 0 - 8 where 0 means 0 degrees, 1 stands for 22.5 degrees,
   2 means 45 degrees, ... 8 means 180 degrees.
*/

int a, TOL, SLOW, angle, target, temp, array[9];

task main()
{
  TOL  = 8;
  SLOW = 10;
  SetSensor(SENSOR_1, SENSOR_TOUCH);
  SetSensorMode(SENSOR_1, SENSOR_MODE_RAW);
  
  target = 212;   //center the servo (90 degrees)
  start track_right;
  start track_left;
  Wait(100);
  array[0]=318; array[1]=293; array[2]=266; array[3]=242; array[4]=212;
  array[5]=183; array[6]=149; array[7]=117; array[8]=75;
  angle = 0;
  a = 1;
  while (true)
  {
  target = array[angle];
  SetUserDisplay(angle*(2250)/100,0);
  Wait(100);
  angle = angle + a;
  if(angle > 8) {a = -1; angle = 7;}
  if(angle < 0) {a =  1; angle = 1;}
  }
}
  
task track_right()
 {
	while(true)
	{
		if (SENSOR_1 < target - TOL)
     {OnFwd(OUT_A); Wait((target - TOL - SENSOR_1)/SLOW); Float(OUT_A);}
    if((SENSOR_1 >= target - TOL) && (SENSOR_1 <= target + TOL))
     {Off(OUT_A);}
   }
 }

task track_left()
 {
	while(true)
	{
    if (SENSOR_1 > target + TOL )
     {OnRev(OUT_A); Wait((SENSOR_1 - target - TOL)/SLOW); Float(OUT_A);}
   if((SENSOR_1 >= target - TOL) && (SENSOR_1 <= target + TOL))
     {Off(OUT_A);}
   }
 }