RioBotz / Mbed 2 deprecated SeguidorDeLinhaV2

Codigo do seguidor de linha V2 2020.2

Dependencies:   mbed

encoder.cpp@1:573dc2b44591, 2020-09-19 (annotated)

Committer:
raylaneleite
Date:
Sat Sep 19 21:31:41 2020 +0000
Revision:
1:573dc2b44591
Parent:
0:9efe13b5d868
Child:
2:4db1486af4d5
Child:
6:5551834026ef
Codigo do encoder atualizado

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"
raylaneleite 1:573dc2b44591 4 #include "Line_Sensor.h"
raylaneleite 1:573dc2b44591 5 //------------------------------------------------------------------------------
raylaneleite 1:573dc2b44591 6
raylaneleite 1:573dc2b44591 7 //---------------------------------ENCODERS-------------------------------------
raylaneleite 1:573dc2b44591 8 InterruptIn OUTA_RIGHT(p26);
raylaneleite 1:573dc2b44591 9 DigitalIn OUTB_RIGHT(p11);
raylaneleite 1:573dc2b44591 10
raylaneleite 1:573dc2b44591 11 InterruptIn OUTA_LEFT(p21);
raylaneleite 1:573dc2b44591 12 DigitalIn OUTB_LEFT(p7);
raylaneleite 1:573dc2b44591 13 //------------------------------------------------------------------------------
raylaneleite 1:573dc2b44591 14
raylaneleite 1:573dc2b44591 15 //-----------------------------------VARIAVEIS----------------------------------
raylaneleite 1:573dc2b44591 16 float diametro_roda = 26; // Diametro da roda
raylaneleite 1:573dc2b44591 17 float reducao = 10; // Redução do motor 10:1
raylaneleite 1:573dc2b44591 18 float perimetro_pulso = (3.14*diametro_roda/(12*reducao); // Perimetro da roda por pulso do encoder
raylaneleite 1:573dc2b44591 19 float D = 149; // Distancia entre as rodas do line (mm)
raylaneleite 1:573dc2b44591 20 float mi = 1.45; // Coeficiente de atrito (Valor aproximado, testar em lab)
raylaneleite 1:573dc2b44591 21 float mapa[50][3]; /* Matriz de dados:
raylaneleite 1:573dc2b44591 22 mapa[i][0]-> distância percorrida pela roda direita no trecho i;
raylaneleite 1:573dc2b44591 23 mapa[i][1]-> distancia percorrida pela roda esquerda no trecho i;
raylaneleite 1:573dc2b44591 24 mapa[i][2]-> velocidade maxima do line do trecho i;*/
raylaneleite 1:573dc2b44591 25
raylaneleite 1:573dc2b44591 26 int i = 0;
raylaneleite 1:573dc2b44591 27 int pulso_direito = 0; // Contador de pulsos da roda direita
raylaneleite 1:573dc2b44591 28 int pulso_esquerdo = 0; // Contador de pulsos da roda esquerda
raylaneleite 1:573dc2b44591 29
raylaneleite 1:573dc2b44591 30 Timer timer_direito; // Armazena o tempo de cada pulso na roda direita
raylaneleite 1:573dc2b44591 31 Timer timer_esquerdo; // Armazena o tempo de cada pulso na roda esquerda
raylaneleite 1:573dc2b44591 32 //------------------------------------------------------------------------------
raylaneleite 1:573dc2b44591 33
raylaneleite 1:573dc2b44591 34 //------------------------------------FUNÇÕES-----------------------------------
raylaneleite 1:573dc2b44591 35
raylaneleite 1:573dc2b44591 36 /*==============================================================================
raylaneleite 1:573dc2b44591 37 Nome: contaPulso_direito; contaPulso_esquerdo;
raylaneleite 1:573dc2b44591 38 Objetivo: Conta os pulsos do encoder e calcula a velocidade atual;
raylaneleite 1:573dc2b44591 39 ==============================================================================*/
raylaneleite 1:573dc2b44591 40 void contaPulso_direito()
raylaneleite 1:573dc2b44591 41 {
raylaneleite 1:573dc2b44591 42 pulso_direito++;
raylaneleite 1:573dc2b44591 43 timer_direito.stop();
raylaneleite 1:573dc2b44591 44 velAtual_direita = perimetro_pulso/timer_direito.read();
raylaneleite 1:573dc2b44591 45 timer_direito.reset();
raylaneleite 1:573dc2b44591 46 timer_direito.start();
raylaneleite 1:573dc2b44591 47 }
raylaneleite 1:573dc2b44591 48
raylaneleite 1:573dc2b44591 49 void contaPulso_esquerdo()
raylaneleite 1:573dc2b44591 50 {
raylaneleite 1:573dc2b44591 51 pulso_esquerdo++;
raylaneleite 1:573dc2b44591 52 timer_esquerdo.stop();
raylaneleite 1:573dc2b44591 53 velAtual_esquerda = perimetro_pulso/timer_esquerdo.read();
raylaneleite 1:573dc2b44591 54 timer_esquerdo.reset();
raylaneleite 1:573dc2b44591 55 timer_esquerdo.start();
raylaneleite 1:573dc2b44591 56 }
raylaneleite 1:573dc2b44591 57
raylaneleite 1:573dc2b44591 58 /*==============================================================================
raylaneleite 1:573dc2b44591 59 Nome: calcula_velmax;
raylaneleite 1:573dc2b44591 60 Objetivo: Calcula a velocidade máxima em cada trecho sem que o line derrape;
raylaneleite 1:573dc2b44591 61 ==============================================================================*/
raylaneleite 1:573dc2b44591 62 float calcula_velmax()
raylaneleite 1:573dc2b44591 63 {
raylaneleite 1:573dc2b44591 64 float raio; // Raio da curva
raylaneleite 1:573dc2b44591 65 float dif = mapa[i][0] - mapa[i][1]; // Diferença entra a distancia percorrida pela roda direita e a roda esquerda
raylaneleite 1:573dc2b44591 66 float soma = mapa[i][0] + mapa[i][1]; // Soma entra a distancia percorrida pela roda direita e a roda esquerda
raylaneleite 1:573dc2b44591 67 if (abs dif > 0.1) // Margem de erro de 0.1 na reta (A ser testada)
raylaneleite 1:573dc2b44591 68 {
raylaneleite 1:573dc2b44591 69 abs raio = (D/2) * (soma/dif);
raylaneleite 1:573dc2b44591 70 return sqrt(raio * 9810 * mi);
raylaneleite 1:573dc2b44591 71 }
raylaneleite 1:573dc2b44591 72 else
raylaneleite 1:573dc2b44591 73 return 0; // Se retornar 0 será feito um novo calculo maximo da velocidade
raylaneleite 1:573dc2b44591 74
raylaneleite 1:573dc2b44591 75
raylaneleite 1:573dc2b44591 76
raylaneleite 1:573dc2b44591 77 /*==============================================================================
raylaneleite 1:573dc2b44591 78 Nome: mapeamento;
raylaneleite 1:573dc2b44591 79 Objetivo: Faz o mapeamento da pista e armazena na matriz de dados;
raylaneleite 1:573dc2b44591 80 ==============================================================================*/
raylaneleite 1:573dc2b44591 81 void mapeamento(bool esq, bool dir) // Variaveis esq e dir estarão gravadas o estado do sensor da asa
raylaneleite 1:573dc2b44591 82 {
raylaneleite 1:573dc2b44591 83 mapa[i][0] = pulso_direito*perimetro_pulso;
raylaneleite 1:573dc2b44591 84 mapa[i][1] = pulso_esquerdo*perimetro_pulso;
raylaneleite 1:573dc2b44591 85 mapa[i][2] = calcula_velmax();
raylaneleite 1:573dc2b44591 86 if (esq && !dir) // Verifica se não está passando por um cruzamento
raylaneleite 1:573dc2b44591 87 {
raylaneleite 1:573dc2b44591 88 i++;
raylaneleite 1:573dc2b44591 89 pulso_direito = 0;
raylaneleite 1:573dc2b44591 90 pulso_esquero = 0;
raylaneleite 1:573dc2b44591 91 }
raylaneleite 1:573dc2b44591 92 else if (dir && !esq) // Zera o contador ao passar pela marcação de inicio da pista
raylaneleite 1:573dc2b44591 93 {
raylaneleite 1:573dc2b44591 94 pulso_direito = 0;
raylaneleite 1:573dc2b44591 95 pulso_esquero = 0;
raylaneleite 1:573dc2b44591 96 }
raylaneleite 1:573dc2b44591 97 }
raylaneleite 1:573dc2b44591 98
raylaneleite 1:573dc2b44591 99
raylaneleite 1:573dc2b44591 100
raylaneleite 1:573dc2b44591 101
raylaneleite 1:573dc2b44591 102
raylaneleite 1:573dc2b44591 103
raylaneleite 1:573dc2b44591 104
raylaneleite 1:573dc2b44591 105
raylaneleite 1:573dc2b44591 106