Diff: main.cpp

Revision:

0:b803244146f7

Child:

1:444af82e35cb

diff -r 000000000000 -r b803244146f7 main.cpp
--- /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