RioBotz / Mbed 2 deprecated SeguidorDeLinhaV2

Codigo do seguidor de linha V2 2020.2

Dependencies:   mbed

encoder.cpp@6:5551834026ef, 2020-10-10 (annotated)

Committer:
rperoba
Date:
Sat Oct 10 19:08:11 2020 +0000
Revision:
6:5551834026ef
Parent:
1:573dc2b44591 
Criado e implementado as funcoes mara escrita e leitura de arquivo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
raylaneleite 1:573dc2b44591 1 //--------------------------------BIBLIOTECAS-----------------------------------
raylaneleite 1:573dc2b44591 2 #include "mbed.h"
raylaneleite 1:573dc2b44591 3 #include "encoder.h"
rperoba 6:5551834026ef 4 #include "LineSensor.h"
rperoba 6:5551834026ef 5
rperoba 6:5551834026ef 6 #define TAMANHODOMAPA 50
rperoba 6:5551834026ef 7 #define ACELERACAO 2 // Deve ser alterado na fase de teste
rperoba 6:5551834026ef 8
raylaneleite 1:573dc2b44591 9 //------------------------------------------------------------------------------
raylaneleite 1:573dc2b44591 10
raylaneleite 1:573dc2b44591 11 //---------------------------------ENCODERS-------------------------------------
rperoba 6:5551834026ef 12 /* InterruptIn OUTA_RIGHT(p26);
raylaneleite 1:573dc2b44591 13 DigitalIn OUTB_RIGHT(p11);
raylaneleite 1:573dc2b44591 14
raylaneleite 1:573dc2b44591 15 InterruptIn OUTA_LEFT(p21);
rperoba 6:5551834026ef 16 DigitalIn OUTB_LEFT(p7);*/
raylaneleite 1:573dc2b44591 17 //------------------------------------------------------------------------------
raylaneleite 1:573dc2b44591 18
raylaneleite 1:573dc2b44591 19 //-----------------------------------VARIAVEIS----------------------------------
raylaneleite 1:573dc2b44591 20 float diametro_roda = 26; // Diametro da roda
raylaneleite 1:573dc2b44591 21 float reducao = 10; // Redução do motor 10:1
rperoba 6:5551834026ef 22 float perimetro_pulso = 3.14*diametro_roda/(12*reducao); // Perimetro da roda por pulso do encoder
raylaneleite 1:573dc2b44591 23 float D = 149; // Distancia entre as rodas do line (mm)
raylaneleite 1:573dc2b44591 24 float mi = 1.45; // Coeficiente de atrito (Valor aproximado, testar em lab)
rperoba 6:5551834026ef 25 float mapa[TAMANHODOMAPA][4]; /* Matriz de dados:
rperoba 6:5551834026ef 26 mapa[i][0]-> distancia percorrida pela roda direita no trecho i;
raylaneleite 1:573dc2b44591 27 mapa[i][1]-> distancia percorrida pela roda esquerda no trecho i;
rperoba 6:5551834026ef 28 mapa[i][2]-> velocidade maxima do line do trecho i;
rperoba 6:5551834026ef 29 mapa[i][3]-> Distancia antes da curva em que precisa comecar a desacelerar*/
raylaneleite 1:573dc2b44591 30
raylaneleite 1:573dc2b44591 31 int i = 0;
raylaneleite 1:573dc2b44591 32 int pulso_direito = 0; // Contador de pulsos da roda direita
raylaneleite 1:573dc2b44591 33 int pulso_esquerdo = 0; // Contador de pulsos da roda esquerda
rperoba 6:5551834026ef 34 float velAtual_direita = 0;
rperoba 6:5551834026ef 35 float velAtual_esquerda = 0;
raylaneleite 1:573dc2b44591 36
raylaneleite 1:573dc2b44591 37 Timer timer_direito; // Armazena o tempo de cada pulso na roda direita
raylaneleite 1:573dc2b44591 38 Timer timer_esquerdo; // Armazena o tempo de cada pulso na roda esquerda
rperoba 6:5551834026ef 39
rperoba 6:5551834026ef 40 LocalFileSystem local("local"); // Cria o local fylesystem com o nome de "local"
raylaneleite 1:573dc2b44591 41 //------------------------------------------------------------------------------
raylaneleite 1:573dc2b44591 42
raylaneleite 1:573dc2b44591 43 //------------------------------------FUNÇÕES-----------------------------------
raylaneleite 1:573dc2b44591 44
raylaneleite 1:573dc2b44591 45 /*==============================================================================
raylaneleite 1:573dc2b44591 46 Nome: contaPulso_direito; contaPulso_esquerdo;
raylaneleite 1:573dc2b44591 47 Objetivo: Conta os pulsos do encoder e calcula a velocidade atual;
raylaneleite 1:573dc2b44591 48 ==============================================================================*/
raylaneleite 1:573dc2b44591 49 void contaPulso_direito()
raylaneleite 1:573dc2b44591 50 {
raylaneleite 1:573dc2b44591 51 pulso_direito++;
raylaneleite 1:573dc2b44591 52 timer_direito.stop();
raylaneleite 1:573dc2b44591 53 velAtual_direita = perimetro_pulso/timer_direito.read();
raylaneleite 1:573dc2b44591 54 timer_direito.reset();
raylaneleite 1:573dc2b44591 55 timer_direito.start();
raylaneleite 1:573dc2b44591 56 }
raylaneleite 1:573dc2b44591 57
rperoba 6:5551834026ef 58 int retornaPulsoDireito ()
rperoba 6:5551834026ef 59 {
rperoba 6:5551834026ef 60 return pulso_direito;
rperoba 6:5551834026ef 61 }
rperoba 6:5551834026ef 62
raylaneleite 1:573dc2b44591 63 void contaPulso_esquerdo()
raylaneleite 1:573dc2b44591 64 {
raylaneleite 1:573dc2b44591 65 pulso_esquerdo++;
raylaneleite 1:573dc2b44591 66 timer_esquerdo.stop();
raylaneleite 1:573dc2b44591 67 velAtual_esquerda = perimetro_pulso/timer_esquerdo.read();
raylaneleite 1:573dc2b44591 68 timer_esquerdo.reset();
raylaneleite 1:573dc2b44591 69 timer_esquerdo.start();
raylaneleite 1:573dc2b44591 70 }
rperoba 6:5551834026ef 71 /*==============================================================================
rperoba 6:5551834026ef 72 Nome: retornaVelDireita; retornaVelEsquerda;
rperoba 6:5551834026ef 73 Objetivo: Retorna as velocidades reais das rodas para podermos usa-las na main
rperoba 6:5551834026ef 74 ==============================================================================*/
rperoba 6:5551834026ef 75 float retornaVelDireita (){
rperoba 6:5551834026ef 76 return velAtual_direita;
rperoba 6:5551834026ef 77 }
raylaneleite 1:573dc2b44591 78
rperoba 6:5551834026ef 79 float retornaVelEsquerda (){
rperoba 6:5551834026ef 80 return velAtual_esquerda;
rperoba 6:5551834026ef 81 }
raylaneleite 1:573dc2b44591 82 /*==============================================================================
raylaneleite 1:573dc2b44591 83 Nome: calcula_velmax;
raylaneleite 1:573dc2b44591 84 Objetivo: Calcula a velocidade máxima em cada trecho sem que o line derrape;
raylaneleite 1:573dc2b44591 85 ==============================================================================*/
raylaneleite 1:573dc2b44591 86 float calcula_velmax()
raylaneleite 1:573dc2b44591 87 {
raylaneleite 1:573dc2b44591 88 float raio; // Raio da curva
raylaneleite 1:573dc2b44591 89 float dif = mapa[i][0] - mapa[i][1]; // Diferença entra a distancia percorrida pela roda direita e a roda esquerda
raylaneleite 1:573dc2b44591 90 float soma = mapa[i][0] + mapa[i][1]; // Soma entra a distancia percorrida pela roda direita e a roda esquerda
rperoba 6:5551834026ef 91 if (abs(dif) > 0.1) // Margem de erro de 0.1 na reta (A ser testada)
raylaneleite 1:573dc2b44591 92 {
rperoba 6:5551834026ef 93 raio = abs((D/2) * (soma/dif));
raylaneleite 1:573dc2b44591 94 return sqrt(raio * 9810 * mi);
raylaneleite 1:573dc2b44591 95 }
raylaneleite 1:573dc2b44591 96 else
raylaneleite 1:573dc2b44591 97 return 0; // Se retornar 0 será feito um novo calculo maximo da velocidade
raylaneleite 1:573dc2b44591 98
raylaneleite 1:573dc2b44591 99
rperoba 6:5551834026ef 100 }
raylaneleite 1:573dc2b44591 101 /*==============================================================================
raylaneleite 1:573dc2b44591 102 Nome: mapeamento;
raylaneleite 1:573dc2b44591 103 Objetivo: Faz o mapeamento da pista e armazena na matriz de dados;
raylaneleite 1:573dc2b44591 104 ==============================================================================*/
raylaneleite 1:573dc2b44591 105 void mapeamento(bool esq, bool dir) // Variaveis esq e dir estarão gravadas o estado do sensor da asa
raylaneleite 1:573dc2b44591 106 {
raylaneleite 1:573dc2b44591 107 mapa[i][0] = pulso_direito*perimetro_pulso;
raylaneleite 1:573dc2b44591 108 mapa[i][1] = pulso_esquerdo*perimetro_pulso;
raylaneleite 1:573dc2b44591 109 mapa[i][2] = calcula_velmax();
raylaneleite 1:573dc2b44591 110 if (esq && !dir) // Verifica se não está passando por um cruzamento
raylaneleite 1:573dc2b44591 111 {
raylaneleite 1:573dc2b44591 112 i++;
raylaneleite 1:573dc2b44591 113 pulso_direito = 0;
rperoba 6:5551834026ef 114 pulso_esquerdo = 0;
raylaneleite 1:573dc2b44591 115 }
raylaneleite 1:573dc2b44591 116 else if (dir && !esq) // Zera o contador ao passar pela marcação de inicio da pista
raylaneleite 1:573dc2b44591 117 {
raylaneleite 1:573dc2b44591 118 pulso_direito = 0;
rperoba 6:5551834026ef 119 pulso_esquerdo = 0;
raylaneleite 1:573dc2b44591 120 }
raylaneleite 1:573dc2b44591 121 }
raylaneleite 1:573dc2b44591 122
rperoba 6:5551834026ef 123 /*==============================================================================
rperoba 6:5551834026ef 124 Nome: DistanciaPraDesacelerar;
rperoba 6:5551834026ef 125 Objetivo: Calcula o 4o item do mapa, a distancia para desacelerar para chegar na proxima parte da pista (curva) com a velocidade certa;
rperoba 6:5551834026ef 126 ==============================================================================*/
rperoba 6:5551834026ef 127
raylaneleite 1:573dc2b44591 128
rperoba 6:5551834026ef 129 float DistanciaPraDesacelerar (float velAtual, float proxVel,float aceleracao)
rperoba 6:5551834026ef 130 {
rperoba 6:5551834026ef 131 if (proxVel == NULL)//para evitar problema na ultima parte da pista,em que nao teremos uma proxima velocidade
rperoba 6:5551834026ef 132 {
rperoba 6:5551834026ef 133 proxVel = 0;
rperoba 6:5551834026ef 134 }
rperoba 6:5551834026ef 135 float distancia = (pow(proxVel,2) - pow(velAtual,2))/2*aceleracao ;
rperoba 6:5551834026ef 136 return distancia;
rperoba 6:5551834026ef 137 }
rperoba 6:5551834026ef 138
rperoba 6:5551834026ef 139 /*==============================================================================
rperoba 6:5551834026ef 140 Nome: CriaParteQuatro;
rperoba 6:5551834026ef 141 Objetivo: Adiciona a distancia para comecar a desaceleracao no mapa da pista;
rperoba 6:5551834026ef 142 ==============================================================================*/
rperoba 6:5551834026ef 143
rperoba 6:5551834026ef 144 void CriaParteQuatro ()
rperoba 6:5551834026ef 145 {
rperoba 6:5551834026ef 146 for(int i = 0;i<TAMANHODOMAPA;i++){
rperoba 6:5551834026ef 147 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
rperoba 6:5551834026ef 148 {
rperoba 6:5551834026ef 149 mapa[i][3] = DistanciaPraDesacelerar(velAtual_direita,mapa[i+1][2],ACELERACAO);
rperoba 6:5551834026ef 150 }
rperoba 6:5551834026ef 151 else
rperoba 6:5551834026ef 152 {
rperoba 6:5551834026ef 153 mapa[i][3] = 0;
rperoba 6:5551834026ef 154 }
rperoba 6:5551834026ef 155 }
rperoba 6:5551834026ef 156 }
rperoba 6:5551834026ef 157 /*==============================================================================
rperoba 6:5551834026ef 158 Nome: criaArquivo;
rperoba 6:5551834026ef 159 Objetivo: Cria e coloca as informacoes do mapa da pista dentro de um arquivo;
rperoba 6:5551834026ef 160 ==============================================================================*/
raylaneleite 1:573dc2b44591 161
rperoba 6:5551834026ef 162 void criaArquivo ()
rperoba 6:5551834026ef 163 {
rperoba 6:5551834026ef 164
rperoba 6:5551834026ef 165 FILE *mapaArquivo = fopen("/local/mapaTeste.txt","w");
rperoba 6:5551834026ef 166 if(mapaArquivo == NULL){
rperoba 6:5551834026ef 167 printf("Impossivel criar o arquivo");
rperoba 6:5551834026ef 168 return;
rperoba 6:5551834026ef 169 }
rperoba 6:5551834026ef 170 //fprintf(mapaArquivo,"Teste\n");
rperoba 6:5551834026ef 171 for(int i = 0;i<TAMANHODOMAPA;i++)
rperoba 6:5551834026ef 172 {
rperoba 6:5551834026ef 173 fprintf (mapaArquivo,"%f-%f-%f-%f\n",mapa[i][0],mapa[i][1],mapa[i][2],mapa[i][3]);
rperoba 6:5551834026ef 174 }
rperoba 6:5551834026ef 175
rperoba 6:5551834026ef 176 fclose(mapaArquivo);
rperoba 6:5551834026ef 177
rperoba 6:5551834026ef 178 }
raylaneleite 1:573dc2b44591 179
raylaneleite 1:573dc2b44591 180
raylaneleite 1:573dc2b44591 181
raylaneleite 1:573dc2b44591 182
raylaneleite 1:573dc2b44591 183