WallFollower.nqc

// Motor speed and direction
 
#define MFWD 3
#define MFLT 2
#define MOFF 1
#define MREV 0
 
// Distance thresholds
#define XL3 527 
#define XL2 480
#define XL1 444
 
#define XR1 430
#define XR2 420
#define XR3 410
 
int value, x, dx;
int motl, motr;
 
task main()
{
  SetSensor(SENSOR_1,SENSOR_LIGHT);
  Wait(100); // Charge GP2D12 sensor
  SetUserDisplay(value,0); // Display distance (remove for RCX firmware 1.5)
 
  SetSensor(SENSOR_1,SENSOR_TOUCH); // Activate sensor
  Wait(5);
  SetSensorMode(SENSOR_1,SENSOR_MODE_RAW);
  value=SENSOR_1; // Get distance
  SetSensor(SENSOR_1,SENSOR_LIGHT); // Re-cahrge sensor for next time
  Wait(25);
  x=value;
  dx=0;
 
  until (false)
  {
 
    SetSensor(SENSOR_1,SENSOR_TOUCH);
    Wait(5); 
    SetSensorMode(SENSOR_1,SENSOR_MODE_RAW);
    value=SENSOR_1; // Get distance
    SetSensor(SENSOR_1,SENSOR_LIGHT); // and recharge
    Wait(15); // shorter delay because calculations take time!
 
    dx=x-value; //Distance variarion
    x=value;
 
    motl=MFWD;
    motr=MFWD;
    if (x>=XL1) // Close to the wall
    {
      if (x>=XL3)
      {
          motr=MREV; // Very close, turn in place
      }
      else if (x>=XL2) 
      {
          motr=MOFF; // close enough, turn 
      }
      else if (x>=XL1)
      {
          motr=MFLT; // a bit too close, shallow turn
      }
 
    }
    else // far from the wall
    {
      if (x<=XR2) // far from wall,
      {
        if (dx<2) // if small variation turn (avoids turning too short on salient angles)
            motl=MOFF;
        }
      else if (x<=XR1)
      {
          motl=MFLT; // a bit too far, shallow turn
 
      }
 
    }
 
    switch(motr)
    {
      case MFWD:
        OnFwd(OUT_C);
        break;
      case MFLT:
        Float(OUT_C);
        break;
      case MOFF:
        Off(OUT_C);
        break;
      case MREV:
        OnRev(OUT_C);
        break;
    }
    switch(motl)
    {
      case MFWD:
        OnFwd(OUT_A);
        break;
      case MFLT:
        Float(OUT_A);
        break;
      case MOFF:
        Off(OUT_A);
        break;
      case MREV:
        OnRev(OUT_A);
        break;
    }
 
  }
}