/*---------------------------------------------------------------* 
| CSE MODELLIERUNGSTAGE 2012                                     |
| P-Regelung: Autmatische Abstandsregelung mit Ultraschallsensor |
| 02.06.2012  Tim Wagner                                         |
*---------------------------------------------------------------*/



#define Abstand_max 30		 //Definition des maximalen Abstandes zum Objekt 
#define Kproportional 4		 //Definition des proportionalen Faktors

int Sensorwert;				//Variable für den Wert vom Sensor... 
int pre_Speed;				//...und für die Geschwindigkeit           
char Speed;

task main()
{


 //Countdown

 PlayTone(400, 500);            
 Wait(800);
 
 PlayTone(400, 500);            
 Wait(800);
 
 PlayTone(800, 600);            
 Wait(600); 


//Initialisierung des Ultraschallsensors

SetSensorLowspeed(IN_1);


//Start der P - Regelung in einer "unendlichen" Schleife

while(true)
 {

//Berechnung der nötigen Geschwindigkeit:

  Sensorwert = SensorUS(IN_1);
  pre_Speed = (Sensorwert - Abstand_max) * Kproportional;
 
  
  if(pre_Speed > 100)          //Da in NXC die Motorensteuerung als Zahl "char" (-128 bis 127) verwendet
  {                            //würde ein zu großer Wert "überlaufen" und negativ werden.
   Speed = 100;                //Dadurch kommt es zu Fehlern in der Steuerung. Deshalb ist diese Unterscheidung
  }                            //und Begrenzung der Werte nötig.
  else
  {
    if(pre_Speed < -100)
    {
     Speed = -100;
    }
    else
    {
     Speed = pre_Speed;
    }
  }
  
  OnFwd(OUT_AB,-Speed);         //Weitergabe an die Motorsteuerung (Speed hier negativ, da unser Modell sonst rückwärts fahren würde)
 }
 
 
}