Codigo do seguidor de linha V2 2020.2

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
rperoba
Date:
Sat Oct 10 19:08:11 2020 +0000
Parent:
1:573dc2b44591
Commit message:
Criado e implementado as funcoes mara escrita e leitura de arquivo

Changed in this revision

PID.cpp Show annotated file Show diff for this revision Revisions of this file
PID.h Show annotated file Show diff for this revision Revisions of this file
bluetooth.cpp Show annotated file Show diff for this revision Revisions of this file
encoder.cpp Show annotated file Show diff for this revision Revisions of this file
encoder.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
motors.cpp Show annotated file Show diff for this revision Revisions of this file
motors.h Show annotated file Show diff for this revision Revisions of this file
diff -r 573dc2b44591 -r 5551834026ef PID.cpp
--- a/PID.cpp	Sat Sep 19 21:31:41 2020 +0000
+++ b/PID.cpp	Sat Oct 10 19:08:11 2020 +0000
@@ -1,6 +1,6 @@
 #include "PID.h"
 #include "mbed.h"
-#include "encoder.h"
+
 
 
 float ultimoErro = 0;
diff -r 573dc2b44591 -r 5551834026ef PID.h
--- a/PID.h	Sat Sep 19 21:31:41 2020 +0000
+++ b/PID.h	Sat Oct 10 19:08:11 2020 +0000
@@ -1,4 +1,3 @@
-#include "mbed.h"
 
 float PID_Sensores(float Erro);
 float PID_rodaDireita(float velocidadeEsperada, float velocidadeReal);
diff -r 573dc2b44591 -r 5551834026ef bluetooth.cpp
--- a/bluetooth.cpp	Sat Sep 19 21:31:41 2020 +0000
+++ b/bluetooth.cpp	Sat Oct 10 19:08:11 2020 +0000
@@ -1,4 +1,4 @@
 #include "mbed.h"
 #include "bluetooth.h"
 
-BufferedSerial bt (p9,p10,9600);
\ No newline at end of file
+Serial bt (p9,p10,9600);
\ No newline at end of file
diff -r 573dc2b44591 -r 5551834026ef encoder.cpp
--- a/encoder.cpp	Sat Sep 19 21:31:41 2020 +0000
+++ b/encoder.cpp	Sat Oct 10 19:08:11 2020 +0000
@@ -1,34 +1,43 @@
 //--------------------------------BIBLIOTECAS-----------------------------------
 #include "mbed.h"
 #include "encoder.h"
-#include "Line_Sensor.h"
+#include "LineSensor.h"
+
+#define TAMANHODOMAPA 50
+#define ACELERACAO  2 // Deve ser alterado na fase de teste
+
 //------------------------------------------------------------------------------
 
 //---------------------------------ENCODERS-------------------------------------
-InterruptIn OUTA_RIGHT(p26);
+/* InterruptIn OUTA_RIGHT(p26);
 DigitalIn OUTB_RIGHT(p11);
 
 InterruptIn OUTA_LEFT(p21);
-DigitalIn OUTB_LEFT(p7);
+DigitalIn OUTB_LEFT(p7);*/
 //------------------------------------------------------------------------------
 
 //-----------------------------------VARIAVEIS----------------------------------
 float diametro_roda = 26; // Diametro da roda
 float reducao = 10; // Redução do motor 10:1
-float perimetro_pulso = (3.14*diametro_roda/(12*reducao); // Perimetro da roda por pulso do encoder
+float perimetro_pulso = 3.14*diametro_roda/(12*reducao); // Perimetro da roda por pulso do encoder
 float D = 149; // Distancia entre as rodas do line (mm)
 float mi = 1.45; // Coeficiente de atrito (Valor aproximado, testar em lab)
-float mapa[50][3]; /* Matriz de dados:
-mapa[i][0]-> distância percorrida pela roda direita no trecho i;
+float mapa[TAMANHODOMAPA][4]; /* Matriz de dados:
+mapa[i][0]-> distancia percorrida pela roda direita no trecho i;
 mapa[i][1]-> distancia percorrida pela roda esquerda no trecho i;
-mapa[i][2]-> velocidade maxima do line do trecho i;*/
+mapa[i][2]-> velocidade maxima do line do trecho i;
+mapa[i][3]-> Distancia antes da curva em que precisa comecar a desacelerar*/
 
 int i = 0;
 int pulso_direito = 0; // Contador de pulsos da roda direita
 int pulso_esquerdo = 0; // Contador de pulsos da roda esquerda
+float velAtual_direita = 0;
+float velAtual_esquerda = 0;
 
 Timer timer_direito; // Armazena o tempo de cada pulso na roda direita
 Timer timer_esquerdo; // Armazena o tempo de cada pulso na roda esquerda
+
+LocalFileSystem local("local"); // Cria o local fylesystem com o nome de "local"
 //------------------------------------------------------------------------------
 
 //------------------------------------FUNÇÕES-----------------------------------
@@ -46,6 +55,11 @@
     timer_direito.start();
 }
 
+int retornaPulsoDireito ()
+{
+    return pulso_direito;
+}
+
 void contaPulso_esquerdo()
 {
     pulso_esquerdo++;
@@ -54,7 +68,17 @@
     timer_esquerdo.reset();
     timer_esquerdo.start();
 }
+/*==============================================================================
+Nome: retornaVelDireita; retornaVelEsquerda;
+Objetivo: Retorna as velocidades reais das rodas para podermos usa-las na main
+==============================================================================*/
+float retornaVelDireita (){
+    return velAtual_direita;
+}
 
+float retornaVelEsquerda (){
+    return velAtual_esquerda;
+}
 /*==============================================================================
 Nome: calcula_velmax;
 Objetivo: Calcula a velocidade máxima em cada trecho sem que o line derrape;
@@ -64,16 +88,16 @@
     float raio; // Raio da curva
     float dif = mapa[i][0] - mapa[i][1]; // Diferença entra a distancia percorrida pela roda direita e a roda esquerda
     float soma = mapa[i][0] + mapa[i][1]; // Soma entra a distancia percorrida pela roda direita e a roda esquerda
-    if (abs dif > 0.1) // Margem de erro de 0.1 na reta (A ser testada)
+    if (abs(dif) > 0.1) // Margem de erro de 0.1 na reta (A ser testada)
     {
-        abs raio = (D/2) * (soma/dif);
+        raio = abs((D/2) * (soma/dif));
         return sqrt(raio * 9810 * mi);
     }
     else
         return 0; // Se retornar 0 será feito um novo calculo maximo da velocidade
         
     
-    
+}   
 /*==============================================================================
 Nome: mapeamento;
 Objetivo: Faz o mapeamento da pista e armazena na matriz de dados;
@@ -87,20 +111,73 @@
     {
         i++;
         pulso_direito = 0;
-        pulso_esquero = 0; 
+        pulso_esquerdo = 0; 
     }
     else if (dir && !esq) // Zera o contador ao passar pela marcação de inicio da pista
     {
         pulso_direito = 0;
-        pulso_esquero = 0;
+        pulso_esquerdo = 0;
     }
 }
 
+/*==============================================================================
+Nome: DistanciaPraDesacelerar;
+Objetivo: Calcula o 4o item do mapa, a distancia para desacelerar para chegar na proxima parte da pista (curva) com a velocidade certa;
+==============================================================================*/
+    
 
+float DistanciaPraDesacelerar (float velAtual, float proxVel,float aceleracao)
+{   
+    if (proxVel == NULL)//para evitar problema na ultima parte da pista,em que nao teremos uma proxima velocidade
+    {
+        proxVel = 0;
+    }
+    float distancia = (pow(proxVel,2) - pow(velAtual,2))/2*aceleracao ;
+    return distancia;
+}
+
+/*==============================================================================
+Nome: CriaParteQuatro;
+Objetivo: Adiciona a distancia para comecar a desaceleracao no mapa da pista;
+==============================================================================*/
+
+void CriaParteQuatro ()
+{
+    for(int i = 0;i<TAMANHODOMAPA;i++){
+        if (mapa[i][0] > mapa[i][1] + 1 || mapa[i][0] + 1 < mapa[i][1] ) // +1 é um parametro arbitrario para evitar que seja uma diferenca minima dos periodos de cada roda
+        {
+            mapa[i][3] = DistanciaPraDesacelerar(velAtual_direita,mapa[i+1][2],ACELERACAO);
+        }
+        else 
+        {
+            mapa[i][3] = 0;
+        }
+    }
+}
+/*==============================================================================
+Nome: criaArquivo;
+Objetivo: Cria e coloca as informacoes do mapa da pista dentro de um arquivo;
+==============================================================================*/
     
+void criaArquivo ()
+{
+
+    FILE *mapaArquivo = fopen("/local/mapaTeste.txt","w");
+    if(mapaArquivo == NULL){
+        printf("Impossivel criar o arquivo");
+        return;
+    }
+    //fprintf(mapaArquivo,"Teste\n");
+    for(int i = 0;i<TAMANHODOMAPA;i++)
+    {
+        fprintf (mapaArquivo,"%f-%f-%f-%f\n",mapa[i][0],mapa[i][1],mapa[i][2],mapa[i][3]);
+    }
+    
+    fclose(mapaArquivo);
+
+}
 
 
 
 
 
-
diff -r 573dc2b44591 -r 5551834026ef encoder.h
--- a/encoder.h	Sat Sep 19 21:31:41 2020 +0000
+++ b/encoder.h	Sat Oct 10 19:08:11 2020 +0000
@@ -1,7 +1,14 @@
 //----------------------------------ENCODERS------------------------------------
 
 //---------------------------------PROTOTIPOS-----------------------------------
-void contaPulso_direito() //Conta os pulsos do encoder e calcula a velocidade atual
-void contaPulso_esquerdo() //Conta os pulsos do encoder e calcula a velocidade atual
-float calcula_velmax() //Calcula a velocidade máxima em cada trecho sem que o line derrape;
-void mapeamento() //Faz o mapeamento da pista e armazena na matriz de dados;
\ No newline at end of file
+
+float retornaVelEsquerda ();
+float retornaVelDireita ();
+int retornaPulsoDireito ();
+void contaPulso_direito(); //Conta os pulsos do encoder e calcula a velocidade atual
+void contaPulso_esquerdo(); //Conta os pulsos do encoder e calcula a velocidade atual
+float calcula_velmax(); //Calcula a velocidade máxima em cada trecho sem que o line derrape;
+void mapeamento(bool esq, bool dir); //Faz o mapeamento da pista e armazena na matriz de dados;
+float DistanciaPraDesacelerar (float velAtual, float proxVel,float aceleracao); // Calcula a distancia para desacelerar para chegar na curva com a velocidade certa;
+void CriaParteQuatro ();//Cria o quarto item do mapa;
+void criaArquivo ();//Cria e escreve o arquivo txt do mapa da pista;
\ No newline at end of file
diff -r 573dc2b44591 -r 5551834026ef main.cpp
--- a/main.cpp	Sat Sep 19 21:31:41 2020 +0000
+++ b/main.cpp	Sat Oct 10 19:08:11 2020 +0000
@@ -5,12 +5,24 @@
 #include "motors.h"
 #include "PID.h"
 
+#define TAMANHODOMAPA 50
+#define PERIMETRODARODA 26
+
 InterruptIn Botao (p5);
 DigitalOut Led (p6);
 
+InterruptIn OUTA_RIGHT(p26);
+DigitalIn OUTB_RIGHT(p11);
+
+InterruptIn OUTA_LEFT(p21);
+DigitalIn OUTB_LEFT(p7);
+
 int botao = 0;
 int comando = 1;
- 
+int contadorAsaDireita = 0;
+int posicaoNoMapa = 0;
+float distanciaPraCurva;
+
 float correcaoPD;
 double erroSensores; 
 
@@ -18,9 +30,32 @@
 float velocidadeDireita;
 float velocidadeRealEsquerda;
 float velocidadeRealDireita;
-float velocidadeBase = 0.18;
+float velocidadeAtual = 0.18;
+float novaVelocidade;
+float correcaoEsquerda;
+float correcaoDireita;
+
+bool estadoAsaDireita = false;
+bool estadoAsaEsquerda = false;
+
+float mapaPraCorrer[TAMANHODOMAPA][4];
 
-Callback<void()> AlteraComando(){
+void leArquivo ()
+{
+    FILE *mapaArquivo = fopen("/local/mapaTeste.txt","r");
+    if(mapaArquivo == NULL){
+        printf("Impossivel criar o arquivo");
+        return;
+    }
+    for(int i = 0;i<TAMANHODOMAPA;i++)
+    {
+        fscanf(mapaArquivo,"%f-%f-%f-%f\n",&mapaPraCorrer[i][0],&mapaPraCorrer[i][1],&mapaPraCorrer[i][2],&mapaPraCorrer[i][3]); // talvez tenha q salvar numa variavel, igualar a um vetor
+    }
+    
+    fclose(mapaArquivo);
+}
+
+void AlteraComando(){
     botao++;
 }
 
@@ -35,7 +70,11 @@
 
 int main()
 {
-    Botao.rise(AlteraComando());
+    Botao.rise(&AlteraComando);
+    OUTA_RIGHT.rise(&contaPulso_direito);
+    OUTA_LEFT.rise(&contaPulso_esquerdo);
+    leArquivo();
+
     Setup_Motores();
     while(botao == 0){          //Espera o aperto do botao para iniciar a calibragem, o led permanecera acesso para indicar esta fase
         wait_us(10);
@@ -50,31 +89,92 @@
     Led = 0;                    //Certificando que o led esta apagado depois da calibragem
 
     wait_us(5000);              //Espera 5 segundos para poder mudar a posição de calibrar os sensores para a de correr
-    //Tocar o buzzer parar avisar q ai começar a correr
-    while(1){                   
+    //Tocar o buzzer parar avisar q ai começar a correr                  
         switch(comando){        
                                 //Comando = 1 -> Faz a pista devagar e mapeando 
                                 //Comando = 2 -> Faz a pista usando a leitura do mapa
                                 //Comando = 3 -> Faz a usando a leitura do mapa e ultrapassando os limites de velocidade
             case 1:
+              while(1){ 
+                velocidadeAtual = VelocidadeBateria(velocidadeAtual);                                     //Ajuste da velocidade por causa da descarga da bateria 
                 LerSensoresFrontais();                                                                  //Leitura dos sensores frontais
                 erroSensores = CalculaErro();                                                           //Calcula o arco do erro dos sensores
                 correcaoPD = PID_Sensores(erroSensores);                                                //Calcula a correção do PD angular
-                velocidadeEsquerda = velocidadeBase + correcaoPD;                                       //Ajusta a inicialmente a velocidade das rodas para manter na linha
-                velocidadeDireita = velocidadeBase - correcaoPD;
-                //velocidadeRealDireita = ;                                                             //Pega a velocidade do seguidor lida pelos encoderes
-                //velocidadeRealEsquerda = ;
-                velocidadeEsquerda = PID_rodaEsquerda(velocidadeEsquerda,velocidadeRealEsquerda);       //Faz o ajusta final da velocidade das rodas
-                velocidadeDireita = PID_rodaDireita(velocidadeDireita,velocidadeRealDireita);
+                velocidadeEsquerda = velocidadeAtual + correcaoPD;                                       //Ajusta a inicialmente a velocidade das rodas para manter na linha
+                velocidadeDireita = velocidadeAtual - correcaoPD;
+                velocidadeRealDireita = retornaVelDireita();                                            //velocidade em /m/s
+                velocidadeRealEsquerda = retornaVelEsquerda();
+                velocidadeEsquerda = transformaPWM(velocidadeEsquerda);                                  //Converte velocidade PWM em m/s
+                velocidadeDireita = transformaPWM(velocidadeDireita);
+                correcaoEsquerda = PID_rodaEsquerda(velocidadeEsquerda,velocidadeRealEsquerda);       //Faz o ajusta final da velocidade das rodas
+                correcaoDireita = PID_rodaDireita(velocidadeDireita,velocidadeRealDireita);
+                velocidadeEsquerda += correcaoEsquerda;
+                velocidadeDireita += correcaoDireita;
                 ControleMotores(velocidadeEsquerda,velocidadeDireita);                                  //Muda a velocidade das rodas para as velocidades calculadas anteriormente
-                //Mapear a pista
+                estadoAsaDireita = LerAsaDireita();
+                estadoAsaEsquerda = LerAsaEsquerda();
+                if(estadoAsaEsquerda || estadoAsaDireita ){                                             //Checa os sensores laterais
+                    mapeamento(estadoAsaEsquerda,estadoAsaDireita);                                     //faz o mapeamento da pista
+                    if (estadoAsaDireita && !estadoAsaEsquerda){
+                        contadorAsaDireita++;                                                           //Faz as contagens das marcacoes a direita
+                        if(contadorAsaDireita >=2){//ARRUMAR UM JEITO DE DAR UM TEMPO ENTRE A LEITURA E O SEGUIDOR PARAR PARA ELE PASSAR TODO PELA LINHA DE CHAGADA
+                            Aceleracao(0,0);
+                            CriaParteQuatro();
+                            criaArquivo();
+                            ParadaFinal();                                                              //Para o seguidor na segunda marcacao
+                        }
+                    }
+                }
+              }
 
             break;
 
-            case 2:
+            case 2:                                                                           //Percorre a pista com o uso do mapa
+              while(1){ 
+                distanciaPraCurva =retornaPulsoDireito() * PERIMETRODARODA  ;
+                if(distanciaPraCurva < mapaPraCorrer[posicaoNoMapa][1] - mapaPraCorrer[posicaoNoMapa][3]){                                     //Seleciona a velocidade
+                    novaVelocidade = VelocidadeBateria (mapaPraCorrer[posicaoNoMapa+1][2]);
+                }
+                else{
+                    novaVelocidade = VelocidadeBateria(mapaPraCorrer[posicaoNoMapa][2]);
+                }  
+                Aceleracao(novaVelocidade,1);                                                       //Acelera ou desacelera o robo
+                LerSensoresFrontais();                                                                  //Leitura dos sensores frontais
+                erroSensores = CalculaErro();                                                           //Calcula o arco do erro dos sensores
+                correcaoPD = PID_Sensores(erroSensores);                                                //Calcula a correção do PD angular
+                velocidadeEsquerda = velocidadeAtual + correcaoPD;                                       //Ajusta a inicialmente a velocidade das rodas para manter na linha
+                velocidadeDireita = velocidadeAtual - correcaoPD;
+                velocidadeRealDireita = retornaVelDireita(); 
+                velocidadeRealEsquerda = retornaVelEsquerda();
+                velocidadeEsquerda = transformaPWM(velocidadeEsquerda);                                  //Converte velocidade PWM em m/s
+                velocidadeDireita = transformaPWM(velocidadeDireita);
+                correcaoEsquerda = PID_rodaEsquerda(velocidadeEsquerda,velocidadeRealEsquerda);       //Faz o ajusta final da velocidade das rodas
+                correcaoDireita = PID_rodaDireita(velocidadeDireita,velocidadeRealDireita);
+                velocidadeEsquerda += correcaoEsquerda; 
+                velocidadeDireita += correcaoDireita;
+                ControleMotores(velocidadeEsquerda,velocidadeDireita);                                  //Muda a velocidade das rodas para as velocidades calculadas anteriormente
+                estadoAsaDireita = LerAsaDireita();
+                estadoAsaEsquerda = LerAsaEsquerda();
+                if(estadoAsaEsquerda || estadoAsaDireita ){                                             //Checa os sensores laterais
+                    if(!estadoAsaEsquerda || estadoAsaDireita){                                         //Leu somente o asa direita
+                        posicaoNoMapa++;                                                             //Atualiza a etapa do mapa da pista que esta sendo lido
+                    }                                                                                   
+                    if(estadoAsaDireita && !estadoAsaEsquerda){                                        //Leu somente a asa esquerda
+                        contadorAsaDireita++;                                                           //Faz as contagens das marcacoes a direita
+                        if(contadorAsaDireita >=2){
+                            Aceleracao(0,0);
+                            ParadaFinal();                                                              //Para o seguidor na segunda marcacao
+                        }
+                    }
+                }
+              }
+
             break;
 
-            case 3:
+            case 3:  
+                while(1){
+
+                 }
             break;
 
             default:
@@ -83,7 +183,7 @@
     }    
 
 
-    }
+    
     
 }
 
diff -r 573dc2b44591 -r 5551834026ef motors.cpp
--- a/motors.cpp	Sat Sep 19 21:31:41 2020 +0000
+++ b/motors.cpp	Sat Oct 10 19:08:11 2020 +0000
@@ -7,7 +7,7 @@
 =================================================*/
 
 //------------------ FUNCOES ----------------------
-float velocidadeBase = 0.18;
+float velocidadePadrao = 0.18;
 
 PwmOut rodaEsquerda(p25); // Potencias da roda e direções
 DigitalOut direcaoEsquerda (p26);
@@ -39,42 +39,54 @@
     else { direcaoDireita = 1; rodaDireita.write(VelocidadeDireita); }
 }
 
-void Acelerando(float velocidadeMax)//Acelera o seguidor de linha
+void Aceleracao(float velocidadeDesejada,int comando)
 {
-    int bandeira = 0;
-    float novaVelocidade = velocidadeBase;
+    int bandeira = 0; //bandeira para controle do while
+    float novaVelocidade = velocidadePadrao;
+    if(comando==1)
+    {
     while(bandeira == 0)
     {
         novaVelocidade = novaVelocidade + 0.01;
         wait_us(1);//Deve ser testado se e necessario esse wait
-        if (novaVelocidade <= velocidadeMax)
+        if (novaVelocidade <= velocidadeDesejada)
         {
             bandeira = 1;
         }
     }
-}
-
-void Desacelerando(float velocidadeMin)//Desacelera o seguidor de linha
-{
-    int bandeira = 0;
-    float novaVelocidade = velocidadeBase;
-    while(bandeira == 0)
+    }
+    else
+    {
+        while(bandeira == 0)
     {
         novaVelocidade = novaVelocidade - 0.01;
         wait_us(1);//Deve ser testado se e necessario esse wait
-        if (novaVelocidade <= velocidadeMin)
+        if (novaVelocidade <= velocidadeDesejada)
         {
             bandeira = 1;
         }
     }
+    }
 }
 
-void VelocidadeBateria() // Funcao de setup da velocidade base dos motores
+float transformaPWM(float velocidadePWM)
+{
+    float leituraBateria = batMeasure;
+    int kv=1100;
+    int R=13;                                           //Raio da roda
+    float rpm=leituraBateria*kv;
+    float velocidadeNova = (2*3.14*R/60)*rpm ;           //Ao calcular essa velocidade temos o equivalente da velocidadePWM=1
+                                                        
+    velocidadeNova=velocidadeNova*velocidadePWM;         //Logo em seguida multiplicamos pela velocidadePWM, dando a porcentagem desejada da nossa velocidade
+    return velocidadeNova;
+}
+
+float VelocidadeBateria(float velocidadeBase) // Funcao de setup da velocidade base dos motores
 {
     float leituraBateria = batMeasure; //Lendo a carga da bateria em um numero de 0 a 1
     float tensaoBateria = 12.6 * leituraBateria; // Carga atual da bateria em Volts
     float novaVelocidade = velocidadeBase * 8.4/tensaoBateria;// Calculando a nova velocidade de acordo com a medida de carga atual da bateria
-    return;
+    return novaVelocidade;
 }
 
 /*float ajustaVelocidade(float velocidadeEsperada, float velocidadeReal)
diff -r 573dc2b44591 -r 5551834026ef motors.h
--- a/motors.h	Sat Sep 19 21:31:41 2020 +0000
+++ b/motors.h	Sat Oct 10 19:08:11 2020 +0000
@@ -1,4 +1,3 @@
-#include "mbed.h"
 
 /*=================================================
 --------------- CODIGO DOS MOTORES ----------------
@@ -10,13 +9,13 @@
 
 void ControleMotores (float VelocidadeEsquerda, float VelocidadeDireita );//Controle geral dos motores
 
-void Acelerando(float velocidadeMax);//Acelera o seguidor de linha
-
-void Desacelerando(float velocidadeMin);//Desacelera o seguidor de linha
-
-void VelocidadeBateria(); // Funcao de setup da velocidade base dos motores
+float VelocidadeBateria(float velocidadeBase); // Funcao de setup da velocidade base dos motores
 
 void ParadaFinal(); //Para o seguidor de linha
 
+float transformaPWM(float velocidadePWM);//Converte velocidadePWM para m/s
+
+void Aceleracao(float velocidadeDesejada,int comando);//Acelera ou desacelera o line follower através do comando, comando=1 acelera else,desacelera
+
 //float ajustaVelocidade(float velocidadeEsperada, float velocidadeReal);//Ajusta a velocidade através da correcao PID das rodas