// Example of a basic feedback loop - simple wall follower
//
// F.P. Ferrie
// September 21, 2007 (translated to NXC)
//



#include "NXCDefs.h"

#define WALLDIST   20
#define WALLBAND    5
#define BASESPEED  50
#define DELTASPEED 40
#define SPEEDPLUS  BASESPEED+DELTASPEED
#define SPEEDMINUS BASESPEED-DELTASPEED


// Program runs as a single thread.

task main(){
  int Distance, Error;

  // Initialize sensors and put cart in motion.  Touch sensor in Port 1, ultrasonic on Port 2
  TextOut(0,0,"Dist: ",true);
  SetSensorLowspeed(IN_1);	// US on port 4
  OnFwd(OUT_BC,BASESPEED);                    	// Start moving forward

  // Feedback loop: read sensor and determine error
  while(true){

    Distance = SensorUS(IN_1);	// Get current dist to wall

    Error = WALLDIST-Distance;	// Determine error
    NumOut(50,0,Distance);	// Debugging aid

    if (Abs(Error) <= WALLBAND)
       OnFwd(OUT_BC,BASESPEED);              	// In band, roll parallel
    else if (Error < 0)
       OnFwd(OUT_C,SPEEDMINUS);               	// Too close, reduce C speed
    else
       OnFwd(OUT_C,SPEEDPLUS);                	// Too far, increase C speed

  }//end while

}//end main

