#pragma config(Sensor, S1,     ,                    sensorTouch)
#pragma config(Sensor, S2,     SENSOR_1,            sensorLightActive)
#pragma config(Sensor, S4,     ,                    sensorTouch)
#pragma config(Motor,  motorA,          right,         tmotorNormal, openLoop, )
#pragma config(Motor,  motorC,          left,        tmotorNormal, openLoop, )
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// 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;
    }

  }
}

**Error**:Undefined procedure 'SetSensor'.
**Error**:Undefined variable 'SENSOR_LIGHT'. 'short' assumed.
**Error**:Undefined procedure 'Wait'.
*Warning*:Invalid '=' operation for types 'TDisplayPrecision' and 'short'
**Error**:Undefined variable 'SENSOR_TOUCH'. 'short' assumed.
**Error**:Undefined variable 'SENSOR_MODE_RAW'. 'short' assumed.
*Warning*:Invalid '=' operation for types 'TSensorModes' and 'short'
*Warning*:Invalid '=' operation for types 'TSensorModes' and 'short'
**Error**:Undefined procedure 'OnFwd'.
**Error**:Undefined variable 'OUT_C'. 'short' assumed.
**Error**:Undefined procedure 'Float'.
**Error**:Undefined procedure 'Off'.
**Error**:Undefined procedure 'OnRev'.
**Error**:Undefined procedure 'OnFwd'.
**Error**:Undefined variable 'OUT_A'. 'short' assumed.
**Error**:Undefined procedure 'Float'.
**Error**:Undefined procedure 'Off'.
**Error**:Undefined procedure 'OnRev'.

#pragma config(Sensor, S2,     sonarSensor,         sensorLightActive)
#pragma config(Motor,  motorA,          right,         tmotorNormal, openLoop)
#pragma config(Motor,  motorC,          left,          tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// 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 motleft, motright;

task main()
{
  SetSensor(sonarSensor,SENSOR_LIGHT);
  wait10Msec(100); // Charge GP2D12 sensor
  SetUserDisplay(value); // Display distance
 
  SetSensor(sonarSensor,SENSOR_TOUCH); // Activate sensor
  wait10Msec(5);
  SetSensorMode(sonarSensor,SENSOR_MODE_RAW);
  value=SensorValue(sonarSensor); // Get distance
  SetSensor(sonarSensor,SENSOR_LIGHT); // Re-cahrge sensor for next time
  wait10Msec(25);
  x=value;
  dx=0;

  until (false)
  {

    SetSensor(Sensorlight,SENSOR_TOUCH);
    wait10Msec(5);
    SetSensorMode(Sensorlight,SENSOR_MODE_RAW);
    value=Sensorlight; // Get distance
    SetSensor(Sensorlight,SENSOR_LIGHT); // and recharge
    wait10Msec(15); // shorter delay because calculations take time!

    dx=x-value; //Distance variarion
    x=value;

    motleft=MFWD;
    motright=MFWD;
    if (x>=XL1) // Close to the wall
    {
      if (x>=XL3)
      {
          motright=MREV; // Very close, turn in place
      }
      else if (x>=XL2)
      {
          motright=MOFF; // close enough, turn
      }
      else if (x>=XL1)
      {
          motright=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)
            motleft=MOFF;
        }
      else if (x<=XR1)
      {
          motleft=MFLT; // a bit too far, shallow turn

      }

    }

    switch(motright)
    {
      case MFWD:
        motor[motorA] = 100;
         motor[motorC] = 0;     
        break;
      case MFLT:
        Float(OUT_C);
        break;
      case MOFF:
        motor[motorA] = 100;
        motor[motorC] = 0;
        break;
      case MREV:
       motor[motorA] = 100;
        motor[motorC] = -100;   
       break;
    }
    switch(motleft)
    {
      case MFWD:
        motor[motorA] = 100;
         motor[motorC] = 100;   
        break;
      case MFLT:
        Float(OUT_A);
        break;
      case MOFF:
        motor[motorA] = 0;
        motor[motorC] = 100;
        break;
      case MREV:
        motor[motorA] = -100;
         motor[motorC] = 100;
        break;
    }

  }
}

#pragma config(Sensor, S2,     sonarSensor,    sensorSONAR)
#pragma config(Motor,  motorA,          right,         tmotorNormal, openLoop)
#pragma config(Motor,  motorC,          left,          tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// 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 motleft, motright;

task main()
{
  SetSensor(S2,SENSOR_LIGHT);
  wait10Msec(100); // Charge GP2D12 sensor
  nxtDisplayCenteredTextLine(0, "Sonar Reading"); // Display distance
 
  SetSensor(S2,SENSOR_TOUCH); // Activate sensor
  wait10Msec(5);
 
  SetSensorMode(S2,SENSOR_MODE_RAW);
  value=SensorValue(S2); // Get distance
  SetSensor(S2,SENSOR_LIGHT); // Re-cahrge sensor for next time
  wait10Msec(25);
  x=value;
  dx=0;

  while (true)
  {

    SetSensor(S2,SENSOR_TOUCH);
    wait10Msec(5);
    SetSensorMode(S2,SENSOR_MODE_RAW);
    value=S2; // Get distance
    SetSensor(S2,SENSOR_LIGHT); // and recharge
    wait10Msec(15); // shorter delay because calculations take time!

    dx=x-value; //Distance variarion
    x=value;

    motleft=MFWD;
    motright=MFWD;
    if (x>=XL1) // Close to the wall
    {
      if (x>=XL3)
      {
          motright=MREV; // Very close, turn in place
      }
      else if (x>=XL2)
      {
          motright=MOFF; // close enough, turn
      }
      else if (x>=XL1)
      {
          motright=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)
            motleft=MOFF;
        }
      else if (x<=XR1)
      {
          motleft=MFLT; // a bit too far, shallow turn

      }

    }

    switch(motright)
    {
      case MFWD:
        motor[motorA] = 100;
         motor[motorC] = 0;
        break;
      case MFLT:
        motor[motorA] = 50;
         motor[motorC] = 0; 
        break;
      case MOFF:
        motor[motorA] = 100;
        motor[motorC] = 0;
        break;
      case MREV:
       motor[motorA] = 100;
        motor[motorC] = -100;
       break;
    }
    switch(motleft)
    {
      case MFWD:
        motor[motorA] = 100;
         motor[motorC] = 100;
        break;
      case MFLT:
        motor[motorA] = 0;
         motor[motorC] = 50;
        break;
      case MOFF:
        motor[motorA] = 0;
        motor[motorC] = 100;
        break;
      case MREV:
        motor[motorA] = -100;
         motor[motorC] = 100;
        break;
    }

  }
}