Revision 6:5551834026ef, committed 2020-10-10

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

--- 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;

--- 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);

--- 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

--- 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);
+
+}
 
 
 
 
 
-

--- 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

--- 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 @@
     }    
 
 
-    }
+    
     
 }
 

--- 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)

--- 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