int offset = 200;
int tp = 280;

int integral = 0;
int lasterror = 0;
int derivative = 0;

long error, turn;
int powera, powerb;

unsigned int rawData[], normData[];
int cval;
int scaledData[];
float test = 0;
int poprzednia =0;
int poprzedniaPoprzednia = 0;
bool jasny = true;
task main()
{
  SetSensorColorFull(IN_3);
  bool isBlack = false;
  bool isStarted = false;
  int counter = 0;
  int treshold = (280-40)/2;

  while(true) {
    OnFwd(OUT_A, 15);
    OnFwd(OUT_B, 15);
    int res = ReadSensorColorEx(IN_3, cval, rawData, normData, scaledData);
    int value = (normData[0] + normData[1] + normData[2])/3;

         if(poprzednia && poprzedniaPoprzednia) {
           if(poprzednia < value && poprzedniaPoprzednia > poprzednia) {
             if(value - poprzednia > 3 && poprzedniaPoprzednia - poprzednia > 3) {
               counter++;
             }
           }
       }
       poprzedniaPoprzednia  = poprzednia;
      poprzednia = value;

    Wait(100);
    ClearScreen();
    NumOut(0, LCD_LINE1, counter,1);
    NumOut(0, LCD_LINE2, value);
    NumOut(0, LCD_LINE3, poprzednia);
    NumOut(0, LCD_LINE4, poprzedniaPoprzednia);
    }
}