Diff: encoder.cpp

Revision:

6:5551834026ef

Parent:

1:573dc2b44591

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