Codigo comentado da IronCup 01/03/2020
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:b803244146f7
- Child:
- 1:444af82e35cb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Oct 16 22:35:17 2019 +0000 @@ -0,0 +1,289 @@ +//========================= BIBLIOTECAS =========================// + +#include "mbed.h" + +//========================= DEFINICOES =========================// + +PwmOut pwm(p25); +DigitalOut dir (p26); +PwmOut pwm1(p24); +DigitalOut dir1 (p23); +//DigitalIn button(p18); +//DigitalOut led(p20); + +Serial bt(p28, p27); + +//InterruptIn A (p21); +//DigitalIn B (p22); +//InterruptIn C (p29); +//DigitalIn D (p30); + +//Ticker END; +//Ticker END1; + +DigitalIn s1(p5); +DigitalIn s2(p6); +DigitalIn s3(p7); +DigitalIn s4(p8); +DigitalIn s5(p9); +DigitalIn s6(p10); +DigitalIn s7(p12); +DigitalIn s8(p13); + +//#define Kp_Roda 1 +//#define Kd_Roda 1 +//#define Ki_Roda 1 + +//============== VARIAVEIS MAIS IMPORTANTES ================// +//Separadas do resto para facilitar a alteração + +float ParametroCorrecao = 5.3; +float Kp_Sensor = 0.145; //0.14; //0.2 +float Kd_Sensor = 1.64; //1.75; //1.15 +float BaseSpeed = 0.17; + +//==========================================================// +//#define VMAX_PWM 100 +//#define VMAX_Encoder 100 + +//#define TESTE_MOTOR +//#define TESTE_SENSOR + +Timeout END; +//========================= VARIAVEIS GLOBAIS ========================= + +int Position; +int Error=0; +int Correction; +int TotalError = 0; +int LastSensor = 0; +int SET_POINT = 45; + +//float Turbo = BaseSpeed; + +//short LimiteFim; +//short ContaFim = 0; +//bool LastRead = false; +//bool Preto[15]; +//int Threshold = 600; + +int LastError = 0; +//int Integral_Esquerda = 0, Integral_Direita = 0; +int MAX_INTEGRAL = 50; +int MIN_INTEGRAL = -50; +float Correcao_Sensor; + +//int Contador_PID_Esquerda = 1, Contador_PID_Direita = 1; +//int Esquerda_LastError = 0, Direita_LastError = 0; +float Esquerda_Esperada, Esquerda_Real, Esquerda_Corrigida; +float Direita_Esperada, Direita_Real, Direita_Corrigida; +//int Esquerda_realSpeed, Direita_realSpeed; +//int Direita_Count = 0, Direita_LastCount = 0; +//int Esquerda_Count = 0, Esquerda_LastCount = 0; +//int Esquerda_lastExpectedSpeed, Direita_lastExpectedSpeed; + +/*================================================= +-------------- CODIGO DOS SENSORES ---------------- +=================================================*/ + +//------------------ FUNCAO ----------------------- + +void Setup_Sensores (void){ //Define o modo de cada sensor como PullUp + s1.mode(PullUp); + s2.mode(PullUp); + s3.mode(PullUp); + s4.mode(PullUp); + s5.mode(PullUp); + s6.mode(PullUp); + s7.mode(PullUp); + s8.mode(PullUp); +} + +void Leitura_Sensores (void){ //Faz a leitura dos sensores e retorna a posição + + if (s4.read()==0 && s5.read()==0) {Position = 45;} //Se ler os dois do meio, está no SetPoint + else if (s7.read()==0) {Position = 70;} //Ele só vai ler os sensores das extremidades + else if (s8.read()==0) {Position = 80;} //se tiver certeza que não está no meio (ajuda o LF a passar pelos cruzamentos) + if (s1.read()==0) {Position = 10;} + if (s2.read()==0) {Position = 20;} + if (s3.read()==0) {Position = 30;} + if (s4.read()==0) {Position = 40;} + if (s5.read()==0) {Position = 50;} + if (s6.read()==0) {Position = 60;} + + //bt.printf("\n\rPosicao: %d", Position); //Print de teste +} + +/*================================================= +--------------- CODIGO DOS MOTORES ---------------- +=================================================*/ + +//------------------ FUNCOES ---------------------- + +void Setup_Motores(void){ pwm.period(0.01); pwm1.period(0.01); } + +void Vai_Demonio (float VelocidadeEsquerda, float VelocidadeDireita ){ //Função drive, controla a velocidade de cada roda + + if (VelocidadeEsquerda <= 0){ + VelocidadeEsquerda = abs(VelocidadeEsquerda); + dir = 0; + pwm.write(VelocidadeEsquerda); + } + else { dir = 1; pwm.write(VelocidadeEsquerda); } + + if (VelocidadeDireita <= 0){ + VelocidadeDireita = abs(VelocidadeDireita); + dir1 = 0; + pwm1.write(VelocidadeDireita); + } + else { dir1 = 1; pwm1.write(VelocidadeDireita); } +} + +/*void Para_Demonio (void){ dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } //Mantém o LF parado + +void Volta_Pra_Linha (){ //Faz o LF voltar para o último lado em que leu a linha, caso tenha pertido (função não funcional, precisa alterar a velocidade) + if (Position == 0){ + if (LastSensor > 0) { Esquerda_Esperada = -50; Direita_Esperada = 80; } + else { Esquerda_Esperada = 80; Direita_Esperada = -50; } + } + else { LastSensor = Error; } +} +*/ +void Final_Stop(){ while(1) { dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } } //Função de parada do LF depois do percurso +/*================================================= +--------------- PID DOS SENSORES ----------------- +=================================================*/ + +float PID_Sensores(int Position){ //Função que aplica o PID e calcula a correção + Error = Position - SET_POINT; + //bt.printf("\n\rErro: %d", Error); + + if (Error < 20 && Error > -20){ + Correcao_Sensor = Kp_Sensor * (float)Error + Kd_Sensor * ((float)Error - (float)LastError); + } + else{ + Correcao_Sensor = ParametroCorrecao * Kp_Sensor * (float)Error + ParametroCorrecao * Kd_Sensor * ((float)Error - (float)LastError); + } + + //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 + + LastError = Error; + + return Correcao_Sensor; +} + +/*================================================= +---------------- PID DOS MOTORES ------------------ +=================================================*/ + +/* + +void rightEncoderEvent (){ + if (B.read() == 0){ + Direita_Count++; + } + + else{ + Direita_Count--; + } +} + +void leftEncoderEvent(){ + if (C.read() == 0) { Esquerda_Count++; } + else { Esquerda_Count--; } +} + +void Speed () { + + Esquerda_realSpeed = (Esquerda_Count - Esquerda_LastCount); //A realSpeed é o numero de voltas/giros por leitura. Comparar o maximo e minimo do Afro com a leitura do encoder. + Esquerda_LastCount = Esquerda_Count; + + Direita_realSpeed = (Direita_Count - Direita_LastCount); + Direita_LastCount = Direita_Count; + //bt.printf("\n\rCount: %d ## Speed: %d",rightCount,realSpeed); + +} + +int Converte_Velocidade (int realSpeed){ + int realPWM; + + realPWM = (VMAX_PWM*realSpeed)/VMAX_Encoder; + + return realPWM; +} + +int PID_Roda (int Lado_realPWM, int Lado_VelocidadeEsperada, int Lado_LastError, int Contador_LadoMotor, int Integral_Lado_Roda, int Lado_lastExpectedSpeed){ + int proporcional, derivativo, Correcao, Velocidade_Corrigida; + + proporcional = Lado_realPWM - Lado_VelocidadeEsperada; + //bt.printf("\n\r%d",proporcional); + derivativo = proporcional - Lado_LastError; + //bt.printf("\n\rDerivativo: %d",derivativo); + + Correcao = Kp_Roda*proporcional + Kd_Roda*derivativo + Ki_Roda*Integral_Lado_Roda; + //bt.printf("\n\r%d",Correcao); + + Lado_LastError = proporcional; + Integral_Lado_Roda += proporcional; + + if (Lado_lastExpectedSpeed == Lado_VelocidadeEsperada) { return Velocidade_Corrigida; } + + if (proporcional == 0){ Lado_LastError = 0; } + else{ + if (Integral_Lado_Roda >= MAX_INTEGRAL) { Integral_Lado_Roda = MAX_INTEGRAL; } + else { Integral_Lado_Roda = MIN_INTEGRAL; } + + } + Lado_lastExpectedSpeed = Lado_VelocidadeEsperada; + + Velocidade_Corrigida = (Lado_realPWM - Correcao); + + return Velocidade_Corrigida; +} + +*/ + +/*================================================= +--------------- FUNCAO PRINCIPAL ----------------- +=================================================*/ + +int main(void){ + + //A.rise(&rightEncoderEvent); + //END.attach(&Speed, 1.0); + //C.rise(&leftEncoderEvent); + + //END.attach(&Speed, 1.0); + //END1.attach(&Speed, 1.0); //TESTAR VER SE ISSO FUNCIONA + + Setup_Sensores(); //Faz o setup dos sensores + Setup_Motores(); //Faz o setup dos motores + + //while (button.read()) { led = 1; wait(0.2); led=0; wait(0.2); } + + wait(2); //Delay para estabilizar + + pwm.write(0.00); //Pulso inicial para cada motor + pwm1.write(0.00); + + END.attach(&Final_Stop, 20); //Faz o LF parar depois de 20 segundos + + while(1){ + //Para_Demonio(); + Leitura_Sensores(); //Lê os sensores + + Correction = PID_Sensores(Position); //Calcula a correção + + Esquerda_Esperada = BaseSpeed + (Correction * 0.01); //Aplica a correção + Direita_Esperada = BaseSpeed - (Correction * 0.01); + + //bt.printf("\n\rEsquerda: %.2f | Direita: %.2f", Esquerda_Esperada, Direita_Esperada); + //Print das velocidades que estão chegando nas rodas após o PID + + //Volta_Pra_Linha(); //Volta para a linha caso tenha perdido + + Vai_Demonio(Esquerda_Esperada, Direita_Esperada); //Drive + + } +} \ No newline at end of file