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

#define LS IN_2
#define MOTOREN OUT_AB

#define KP 8	// Konstante Proportionalglied
#define KD 2	// Konstante Differentialglied



int ist, soll, error,preError, diffteil, 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 = 50;
while(doit) {

    Wait(50);

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



    error = ist - soll;
    diffteil = error - preError;
    value = error * KP + diffteil * KD + speed;
    preError = error;




    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);

}


}