#pragma config(Sensor, S1,     lightRight,           sensorLightActive)
#pragma config(Sensor, S4,     lightLeft,          sensorLightActive)
#pragma config(Motor,  motorA,          motorRight,    tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorLeft,     tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*--------------------------------------------------------------------------------------------------------*\
|*                                                                                                        *|
|*                                          - P-Pegler -                                                  *|
|*                                            ROBOTC on NXT                                               *|
|*                                                                                                        *|
|*  Mit Hilfe eines P-Regler folgt das Fahrzeug einer Kante (schwarz/weiss)                               *|
|*                                                                                                        *|
|*                                        ROBOT CONFIGURATION                                             *|
|*    NOTES:                                                                                              *|
|*    1)  Die Lichtsensoren sind frontseitig nebeneinader montiert                                        *|
|*    2)  Stellen Sie sicher, dass vor dem Start des Programms Ihr Roboter mittig auf der Kante           *|
|*        platziert ist, damit der korrekte Teachwert (Sollwert) initialisiert werden kann                *|
|*        comparison in your program.                                                                     *|
|*                                                                                                        *|
|*    MOTORS & SENSORS:                                                                                   *|
|*    [I/O Port]              [Name]              [Type]              [Description]                       *|
|*    Port A                  motorRight          NXT                 Right motor                         *|
|*    Port C                  motorLeft           NXT                 Left motor                          *|
|*    Port 1                  lightRight          LightActiv          Right mounted                       *|
|*    Port 4                  lightLeft           LightActiv          Left mounted                        *|
\*--------------------------------------------------------------------------------------------------------*/

//globale Variablen und Konstanten
int lightTeach;                     // Bestimmung des Sollwert (Teach In)
int valueLightBlack;                // aktueller Sensorwert (Lichtsensor Rechts)
int valueLightWhite;                 // aktueller Sensorwert (Lichtsensor Lichts)
int targetSpeed = 5;               // Zielgeschwindigkeit Fahrzeug (%)
bool flag_initBlackColorLeft;
bool flag_BlackSide = false;
int invert_Stellgroesse;

//Regler
int fuehrungsgroesse_w = 0;
float regelgroesse_x;               // Istwert auf den ein allfaelliger Regler reagiert
float regelverstaerkung_Kp = 0.5;
float regelabweichung_e;
int stellgroesse_y;

task main()
{
  wait1Msec(2000);     // warte 2000 Millisekunden zum initialisieren der Lichtsensoren
  // Sensoren einlesen
  valueLightBlack = SensorValue(lightRight);
  valueLightWhite  = SensorValue(lightLeft);

  if  (valueLightBlack > valueLightWhite) {
    flag_initBlackColorLeft = true;
    invert_Stellgroesse = 1;
    } else {
    flag_initBlackColorLeft = false;
    invert_Stellgroesse = -1;
  }

  // TeachValue fuer Kante schwarz/weiss bestimmen
  lightTeach = valueLightBlack +  valueLightWhite;

  while(true)        // Endlosschleife
  {
    // Sesoren einlesen
    if (flag_initBlackColorLeft) {
      valueLightBlack = SensorValue(lightLeft);
      valueLightWhite  = SensorValue(lightRight);
      } else {
      valueLightBlack = SensorValue(lightLeft);
      valueLightWhite  = SensorValue(lightRight);
    }

    // Position berechnen
    // mit Hilfe dieser berechnung ergibt sich der Wert null, wenn Fahrzeug direkt
    // auf Kante schwarz / weiss steht. Somit kann die Führungsgrösse mit null de-
    // finiert werden
    regelgroesse_x = lightTeach - (valueLightBlack + valueLightWhite);

    // P Regler
    regelabweichung_e = fuehrungsgroesse_w - regelgroesse_x;

    if (regelabweichung_e < 0 && flag_BlackSide == false) {
      flag_BlackSide = true;
    } else if (regelabweichung_e > 0 && flag_BlackSide == true && valueLightBlack < valueLightWhite) {
      flag_BlackSide = false;
    }

    stellgroesse_y = regelverstaerkung_Kp * regelabweichung_e;

    // Stellglied ansteuern
    motor[motorLeft] = targetSpeed - (invert_Stellgroesse * stellgroesse_y);
    motor[motorRight] = targetSpeed + (invert_Stellgroesse * stellgroesse_y);

    // Position ausgeben
    nxtDisplayTextLine(2, "akt. Position:");
    nxtDisplayTextLine(3, "%d", regelgroesse_x);
  }
}