/*---------------------------------------------------------------*
| CSE MODELLIERUNGSTAGE 2016                                    |
| P-Regler                                                     |
*---------------------------------------------------------------*/

#define LS IN_2
#define MOTOREN OUT_AB

#define KP 8	// Konstante Proportionalglied



int ist, soll, error, value;
char speed;

bool doit = true;


int sensorRuhe()
{
   int srl = Sensor(LS);
   return srl;
}

task main() {
      SetSensorLowspeed(IN_1)    ;

SetSensorLight(LS);

ClearScreen();
TextOut(0, LCD_LINE1, "Kransteuerung");
TextOut(1, LCD_LINE7, "Zum Beenden");
TextOut(3, LCD_LINE8, "orange druecken");

soll = sensorRuhe();

int speed = 35;
while(doit) {

    Wait(50);

    if (ButtonPressed(BTNCENTER, false)) {
       doit = false;
    }
    
      if (SensorUS(IN_1) <= 20) {
       speed = 0;
    }
      // if (SensorUS(IN_1) > 20) {
       //speed = 35;
   // }
    ist = Sensor(LS);



    error = ist - soll;
    value = error * KP + speed;




 if(value > 100) {                    //Da in NXC die Motorensteuerung als Zahl "char" (-128 bis 127) verwendet wird
        value = 100;                         //würde ein zu großer Wert "überlaufen" und negativ werden.
                                             //Dadurch kommt es zu Fehlern in der Steuerung. Deshalb ist diese Unterscheidung
    }                                        //und Begrenzung der Werte nötig.
    else {

         if(value < -100) {
              value = -100;
         }
         else {
              //speed = value ;
         }

    }

    OnFwd(MOTOREN, value);

}


}