mutex move;
int readingColor;
int startColor;
int Kproportional = 2;
int Kintegral = 0;
int Kderivative = 3;
int integral = 0;
int dt = 100;
int previous_error = 0;
int speed = 5;

task moveFwd(){
     while(true){
                 Acquire(move);
                 RotateMotor(OUT_AC, -35, 20);
                 Release(move);
     }
}

task checkLine(){
     startColor = SENSOR_1;
     while(true){
                 Acquire(move);
                 readingColor = SENSOR_1;
                 int error =  startColor - readingColor;
                 int proportional = Kproportional * error;
                 integral = integral + error;
                 int derivative = (error - previous_error) / dt;
                 int output = proportional + Kintegral * dt* integral + Kderivative * derivative;
                 previous_error = error;
                 int left = -output;
                 int right = output;
                        OnFwd(OUT_A, -output/2);
                        OnFwd(OUT_C, output/2);
                 Wait(dt);
                 TextOut(0, LCD_LINE1, NumToStr(readingColor), true);
                 TextOut(0, LCD_LINE2," l:" + NumToStr(left) + " r:" + NumToStr(right), true);
                 Release(move);
     }
}

task main(){
     SetSensorColorRed(IN_1);
     Precedes(checkLine, moveFwd);

}