Codigo do seguidor de linha V2 2020.2
Dependencies: mbed
PID.cpp@6:5551834026ef, 2020-10-10 (annotated)
- Committer:
- rperoba
- Date:
- Sat Oct 10 19:08:11 2020 +0000
- Revision:
- 6:5551834026ef
- Parent:
- 0:9efe13b5d868
Criado e implementado as funcoes mara escrita e leitura de arquivo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rperoba | 0:9efe13b5d868 | 1 | #include "PID.h" |
rperoba | 0:9efe13b5d868 | 2 | #include "mbed.h" |
rperoba | 6:5551834026ef | 3 | |
rperoba | 0:9efe13b5d868 | 4 | |
rperoba | 0:9efe13b5d868 | 5 | |
rperoba | 0:9efe13b5d868 | 6 | float ultimoErro = 0; |
rperoba | 0:9efe13b5d868 | 7 | float Kp_Sensor = 0.675; //0.65 //0.675 |
rperoba | 0:9efe13b5d868 | 8 | float Kd_Sensor = 24.55; //24.45 //24.55 |
rperoba | 0:9efe13b5d868 | 9 | float IntegralEsquerda = 0; |
rperoba | 0:9efe13b5d868 | 10 | float IntegralDireita = 0; |
rperoba | 0:9efe13b5d868 | 11 | float Ki_rodaDireita = 1; |
rperoba | 0:9efe13b5d868 | 12 | float Ki_rodaEsquerda = 1; |
rperoba | 0:9efe13b5d868 | 13 | float Kp_rodaDireita = 1; |
rperoba | 0:9efe13b5d868 | 14 | float Kp_rodaEsquerda = 1; |
rperoba | 0:9efe13b5d868 | 15 | float Kd_rodaDireita = 1; |
rperoba | 0:9efe13b5d868 | 16 | float Kd_rodaEsquerda = 1; |
rperoba | 0:9efe13b5d868 | 17 | |
rperoba | 0:9efe13b5d868 | 18 | float PID_Sensores(float Erro){ //Função que aplica o PID e calcula a correção |
rperoba | 0:9efe13b5d868 | 19 | //Achando o erro do PID atraves da leitura dos sensores. |
rperoba | 0:9efe13b5d868 | 20 | float Correcao_Sensor = Kp_Sensor * (float)Erro + Kd_Sensor * ((float)Erro - (float)ultimoErro); |
rperoba | 0:9efe13b5d868 | 21 | |
rperoba | 0:9efe13b5d868 | 22 | //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))); |
rperoba | 0:9efe13b5d868 | 23 | //Comando para printar e verificar os dados de saida via bluetooth |
rperoba | 0:9efe13b5d868 | 24 | |
rperoba | 0:9efe13b5d868 | 25 | ultimoErro = Erro; |
rperoba | 0:9efe13b5d868 | 26 | |
rperoba | 0:9efe13b5d868 | 27 | return Correcao_Sensor; |
rperoba | 0:9efe13b5d868 | 28 | } |
rperoba | 0:9efe13b5d868 | 29 | |
rperoba | 0:9efe13b5d868 | 30 | float PID_rodaDireita(float velocidadeEsperada, float velocidadeReal) |
rperoba | 0:9efe13b5d868 | 31 | { |
rperoba | 0:9efe13b5d868 | 32 | float Erro=velocidadeReal-velocidadeEsperada;//Achando o erro do PID atraves das velocidades. |
rperoba | 0:9efe13b5d868 | 33 | |
rperoba | 0:9efe13b5d868 | 34 | IntegralDireita=IntegralDireita + Erro;//Acumulador de erro da integral |
rperoba | 0:9efe13b5d868 | 35 | |
rperoba | 0:9efe13b5d868 | 36 | float Correcao_roda = Kp_rodaDireita * Erro + Kd_rodaDireita * (Erro - ultimoErro)+ IntegralDireita*Ki_rodaDireita ; |
rperoba | 0:9efe13b5d868 | 37 | |
rperoba | 0:9efe13b5d868 | 38 | ultimoErro = Erro; |
rperoba | 0:9efe13b5d868 | 39 | |
rperoba | 0:9efe13b5d868 | 40 | return Correcao_roda; |
rperoba | 0:9efe13b5d868 | 41 | } |
rperoba | 0:9efe13b5d868 | 42 | |
rperoba | 0:9efe13b5d868 | 43 | float PID_rodaEsquerda(float velocidadeEsperada, float velocidadeReal) |
rperoba | 0:9efe13b5d868 | 44 | { |
rperoba | 0:9efe13b5d868 | 45 | float Erro=velocidadeReal-velocidadeEsperada;//Achando o erro do PID atraves das velocidades. |
rperoba | 0:9efe13b5d868 | 46 | |
rperoba | 0:9efe13b5d868 | 47 | IntegralEsquerda=IntegralEsquerda + Erro;//Acumulador de erro da integral |
rperoba | 0:9efe13b5d868 | 48 | |
rperoba | 0:9efe13b5d868 | 49 | float Correcao_roda = Kp_rodaEsquerda * Erro + Kd_rodaEsquerda * (Erro - ultimoErro)+ IntegralEsquerda*Ki_rodaEsquerda ; |
rperoba | 0:9efe13b5d868 | 50 | |
rperoba | 0:9efe13b5d868 | 51 | ultimoErro = Erro; |
rperoba | 0:9efe13b5d868 | 52 | |
rperoba | 0:9efe13b5d868 | 53 | return Correcao_roda; |
rperoba | 0:9efe13b5d868 | 54 | } |