File content as of revision 6:5551834026ef:

#include "PID.h"
#include "mbed.h"



float ultimoErro = 0;
float Kp_Sensor = 0.675;        //0.65              //0.675
float Kd_Sensor = 24.55;        //24.45             //24.55
float IntegralEsquerda = 0;
float IntegralDireita = 0;
float Ki_rodaDireita = 1;
float Ki_rodaEsquerda = 1;
float Kp_rodaDireita = 1;
float Kp_rodaEsquerda = 1;
float Kd_rodaDireita = 1;
float Kd_rodaEsquerda = 1;

float PID_Sensores(float Erro){ //Função que aplica o PID e calcula a correção
   //Achando o erro do PID atraves da leitura dos sensores.
    float Correcao_Sensor = Kp_Sensor * (float)Erro + Kd_Sensor * ((float)Erro - (float)ultimoErro);
    
    //bt.printf("\n\rCorrecao: %.2f | Erro: %d | LastError: %d | KP: %.1f | KD: %.1f", Correcao_Sensor, Error, LastError, (Kp_Sensor * (float)Error), (Kd_Sensor * ((float)Error - (float)LastError)));
    //Comando para printar e verificar os dados de saida via bluetooth
    
    ultimoErro = Erro;
    
    return Correcao_Sensor;
}

float PID_rodaDireita(float velocidadeEsperada, float velocidadeReal)
{
    float Erro=velocidadeReal-velocidadeEsperada;//Achando o erro do PID atraves das velocidades.
    
    IntegralDireita=IntegralDireita + Erro;//Acumulador de erro da integral
    
    float Correcao_roda = Kp_rodaDireita * Erro + Kd_rodaDireita * (Erro - ultimoErro)+ IntegralDireita*Ki_rodaDireita ;

    ultimoErro = Erro;
    
    return Correcao_roda;   
}

float PID_rodaEsquerda(float velocidadeEsperada, float velocidadeReal)
{
    float Erro=velocidadeReal-velocidadeEsperada;//Achando o erro do PID atraves das velocidades.
    
    IntegralEsquerda=IntegralEsquerda + Erro;//Acumulador de erro da integral
    
    float Correcao_roda = Kp_rodaEsquerda * Erro + Kd_rodaEsquerda * (Erro - ultimoErro)+ IntegralEsquerda*Ki_rodaEsquerda ;

    ultimoErro = Erro;
    
    return Correcao_roda;   
}