PID_Control.cpp

 void PID_SegueLinha(){

    int error = Estado_Atual() - setPoint;

    I+=error;
    
    int motorSpeed = KP * error + KD * (error - lastError) + KI*I;

    lastError=error; 

    int m1Speed = M1 + motorSpeed;
    
    int m2Speed = M2 - motorSpeed;
 
    if (m1Speed < 0) m1Speed = 0;           // Determina o limite inferior
    
    if (m2Speed < 0) m2Speed = 0;           // Determina o limite inferior
    
    if (m1Speed > MMAX) m1Speed = MMAX;     // Determina o limite superior
    
    if (m2Speed > MMAX) m2Speed = MMAX;     // Determina o limite superior

    Motor1(m1Speed);
    Motor2(m2Speed);

}