/*---------------------------------------------------------------*
| CSE MODELLIERUNGSTAGE 2013                                     |
| PD-Regeler                                                |
| 21.03.2013  Tim Wagner                                         |
*---------------------------------------------------------------*/

#define LS IN_2
#define MOTOREN OUT_AB

#define KP 10	// Konstante Proportionalglied
#define KD 15	// Konstante Differentialglied


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

bool doit = true;


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

task main() {


SetSensorLight(LS);

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

soll = sensorRuhe();


while(doit) {

    Wait(50);

    if (ButtonPressed(BTNCENTER, false)) {
       doit = false;
    }
    
    
    ist = Sensor(LS);



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

    preError = error;



    if(value > 100) {                    //Da in NXC die Motorensteuerung als Zahl "char" (-128 bis 127) verwendet wird
        speed = 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) {
              speed = -100;
         }
         else {
              speed = value;
         }
         
    }

    OnFwd(MOTOREN, speed);

}

}