RioBotz / Mbed 2 deprecated SeguidorDeLinhaV2

Codigo do seguidor de linha V2 2020.2

Dependencies:   mbed

PID.cpp@0:9efe13b5d868, 2020-09-19 (annotated)

Committer:
rperoba
Date:
Sat Sep 19 20:43:59 2020 +0000
Revision:
0:9efe13b5d868
Child:
2:4db1486af4d5
Primeira versao do codigo do seguidor de linha v2,utilizando bibliotecas para melhor organizar suas funcoes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rperoba 0:9efe13b5d868 1 #include "PID.h"
rperoba 0:9efe13b5d868 2 #include "mbed.h"
rperoba 0:9efe13b5d868 3 #include "encoder.h"
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 }