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?
User | Revision | Line number | New 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 |