RioBotz / Mbed 2 deprecated SeguidorDeLinhaV2

Codigo do seguidor de linha V2 2020.2

Dependencies:   mbed

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rperoba 0:9efe13b5d868 1
rperoba 0:9efe13b5d868 2 #include "mbed.h"
rperoba 0:9efe13b5d868 3 #include "LineSensor.h"
rperoba 0:9efe13b5d868 4 #include "bluetooth.h"
rperoba 0:9efe13b5d868 5
rperoba 0:9efe13b5d868 6 #define DEBUG true
rperoba 0:9efe13b5d868 7
rperoba 0:9efe13b5d868 8 int comprimentoDoRobo = 20 ; //Comprimento do seguidor em mm
rperoba 0:9efe13b5d868 9
rperoba 0:9efe13b5d868 10 AnalogIn AsaDireitaInterno (p16);
rperoba 0:9efe13b5d868 11 AnalogIn AsaDireitaExterno (p15);
rperoba 0:9efe13b5d868 12 AnalogIn AsaEsquerdaInterno (p17);
rperoba 0:9efe13b5d868 13 AnalogIn AsaEsquerdaExterno (p18);
rperoba 0:9efe13b5d868 14 AnalogIn MultiplexadorIn (p19);
rperoba 0:9efe13b5d868 15 DigitalIn SensorCentral (p25);
rperoba 0:9efe13b5d868 16
rperoba 0:9efe13b5d868 17 DigitalOut Multiplexador0 (p12);
rperoba 0:9efe13b5d868 18 DigitalOut Multiplexador1 (p13);
rperoba 0:9efe13b5d868 19 DigitalOut Multiplexador2 (p14);
rperoba 0:9efe13b5d868 20
rperoba 0:9efe13b5d868 21 PwmOut Buzzer (p8);
rperoba 0:9efe13b5d868 22
rperoba 0:9efe13b5d868 23 float leituraNaoCalibrada;
rperoba 0:9efe13b5d868 24 float leituraMaxima [12] = {0,0,0,0,0,0,0,0,0,0,0,0};
rperoba 0:9efe13b5d868 25 float leituraMinima [12] = {1,1,1,1,1,1,1,1,1,1,1,1};
rperoba 0:9efe13b5d868 26 float leituraCalibrada [8];
rperoba 0:9efe13b5d868 27 float parametroDeLeituraAceitavel = 0.7;
rperoba 0:9efe13b5d868 28
rperoba 0:9efe13b5d868 29 float posicaoDosSensores [8] = {-37.5,-27.5,-17.5,-7.5,7.5,17.5,27.5,37.5};
rperoba 0:9efe13b5d868 30
rperoba 0:9efe13b5d868 31 int tabelaMultiplexador0 [8] = {0,1,0,1,0,1,0,1};
rperoba 0:9efe13b5d868 32 int tabelaMultiplexador1 [8] = {0,0,1,1,0,0,1,1};
rperoba 0:9efe13b5d868 33 int tabelaMultiplexador2 [8] = {0,0,0,0,1,1,1,1};
rperoba 0:9efe13b5d868 34
rperoba 0:9efe13b5d868 35
rperoba 0:9efe13b5d868 36 void AlocaLeitura (int posicaoSensor,float leitura) //Adiciona a leitura nao calibrada aos vetores de leitura maxima ou minima
rperoba 0:9efe13b5d868 37 {
rperoba 0:9efe13b5d868 38 if (leitura > leituraMaxima[posicaoSensor])
rperoba 0:9efe13b5d868 39 {
rperoba 0:9efe13b5d868 40 leituraMaxima[posicaoSensor] = leitura;
rperoba 0:9efe13b5d868 41 }
rperoba 0:9efe13b5d868 42 else if (leitura < leituraMinima[posicaoSensor])
rperoba 0:9efe13b5d868 43 {
rperoba 0:9efe13b5d868 44 leituraMinima[posicaoSensor] = leitura;
rperoba 0:9efe13b5d868 45 }
rperoba 0:9efe13b5d868 46 }
rperoba 0:9efe13b5d868 47
rperoba 0:9efe13b5d868 48 void CalibragemDosSensores() //Registra as leituras minimas e maximas feitas por cada sensor
rperoba 0:9efe13b5d868 49 //para funcionar precisa rodar a função continuamente e passar os sensores por cima da linha branca e do fundo preto da pista
rperoba 0:9efe13b5d868 50 {
rperoba 0:9efe13b5d868 51 for(int i = 0;i<8;i++){ //Calibrando sensores frontais
rperoba 0:9efe13b5d868 52 //Ajustando o multiplexador
rperoba 0:9efe13b5d868 53 Multiplexador0 = tabelaMultiplexador0[i];
rperoba 0:9efe13b5d868 54 Multiplexador1 = tabelaMultiplexador1[i];
rperoba 0:9efe13b5d868 55 Multiplexador2 = tabelaMultiplexador2[i];
rperoba 0:9efe13b5d868 56
rperoba 0:9efe13b5d868 57 //Lendo o sensor a adicionando na leitura
rperoba 0:9efe13b5d868 58 leituraNaoCalibrada = MultiplexadorIn;
rperoba 0:9efe13b5d868 59
rperoba 0:9efe13b5d868 60 //Salvando as leituras máximas e minimas
rperoba 0:9efe13b5d868 61 AlocaLeitura(i,leituraNaoCalibrada);
rperoba 0:9efe13b5d868 62 }
rperoba 0:9efe13b5d868 63 //Calibragem das asas
rperoba 0:9efe13b5d868 64 leituraNaoCalibrada = AsaEsquerdaExterno;
rperoba 0:9efe13b5d868 65 AlocaLeitura(8, leituraNaoCalibrada);
rperoba 0:9efe13b5d868 66
rperoba 0:9efe13b5d868 67 leituraNaoCalibrada = AsaEsquerdaInterno;
rperoba 0:9efe13b5d868 68 AlocaLeitura(9, leituraNaoCalibrada);
rperoba 0:9efe13b5d868 69
rperoba 0:9efe13b5d868 70 leituraNaoCalibrada = AsaDireitaInterno;
rperoba 0:9efe13b5d868 71 AlocaLeitura(10, leituraNaoCalibrada);
rperoba 0:9efe13b5d868 72
rperoba 0:9efe13b5d868 73 leituraNaoCalibrada = AsaDireitaExterno;
rperoba 0:9efe13b5d868 74 AlocaLeitura(11, leituraNaoCalibrada);
rperoba 0:9efe13b5d868 75
rperoba 0:9efe13b5d868 76 }
rperoba 0:9efe13b5d868 77
rperoba 0:9efe13b5d868 78 float CalibragemDeLeitura (int numeroDoSensor,float leitura) // Calibra cada leitura individualmente baseado nas leituras maximas e minimas ja cadastradas de cada sensor
rperoba 0:9efe13b5d868 79 {
rperoba 0:9efe13b5d868 80 return (leitura - leituraMinima[numeroDoSensor])/(leituraMaxima[numeroDoSensor]-leituraMinima[numeroDoSensor]) ;
rperoba 0:9efe13b5d868 81 }
rperoba 0:9efe13b5d868 82
rperoba 0:9efe13b5d868 83 void LerSensoresFrontais () //Faz a leitura dos sensores frontais e salva no vetor leituraCalibrada
rperoba 0:9efe13b5d868 84 {
rperoba 0:9efe13b5d868 85 int contador = 1;
rperoba 0:9efe13b5d868 86
rperoba 0:9efe13b5d868 87 for(int i = 0;i < 8;i++){
rperoba 0:9efe13b5d868 88
rperoba 0:9efe13b5d868 89 Multiplexador0 = tabelaMultiplexador0[i];
rperoba 0:9efe13b5d868 90 Multiplexador1 = tabelaMultiplexador1[i];
rperoba 0:9efe13b5d868 91 Multiplexador2 = tabelaMultiplexador2[i];
rperoba 0:9efe13b5d868 92
rperoba 0:9efe13b5d868 93 leituraNaoCalibrada = MultiplexadorIn;
rperoba 0:9efe13b5d868 94
rperoba 0:9efe13b5d868 95 leituraCalibrada[i] = CalibragemDeLeitura(i,leituraNaoCalibrada);
rperoba 0:9efe13b5d868 96 }
rperoba 0:9efe13b5d868 97
rperoba 0:9efe13b5d868 98
rperoba 0:9efe13b5d868 99 }
rperoba 0:9efe13b5d868 100
rperoba 0:9efe13b5d868 101 float CalculaErro () //Retorna o erro em graus para o calculo do PID
rperoba 0:9efe13b5d868 102 {
rperoba 0:9efe13b5d868 103 float distancia = 0;
rperoba 0:9efe13b5d868 104
rperoba 0:9efe13b5d868 105 for(int i = 0;i < 8;i++){//Calcula a media ponderada das leituras pra achar a distancia da linha para o setpoint
rperoba 0:9efe13b5d868 106
rperoba 0:9efe13b5d868 107 distancia += ( leituraCalibrada[i] + posicaoDosSensores [i] )/ leituraCalibrada [i];
rperoba 0:9efe13b5d868 108 }
rperoba 0:9efe13b5d868 109 return atan(distancia/comprimentoDoRobo);
rperoba 0:9efe13b5d868 110 }
rperoba 0:9efe13b5d868 111
rperoba 0:9efe13b5d868 112 bool LerAsaDireita () //Retorna True se os sensores da asa direita lerem a linha
rperoba 0:9efe13b5d868 113 {
rperoba 0:9efe13b5d868 114 leituraNaoCalibrada = AsaDireitaExterno;
rperoba 0:9efe13b5d868 115 float leituraExterna = CalibragemDeLeitura(11,leituraNaoCalibrada);
rperoba 0:9efe13b5d868 116
rperoba 0:9efe13b5d868 117 leituraNaoCalibrada = AsaDireitaInterno;
rperoba 0:9efe13b5d868 118 float leituraInterna = CalibragemDeLeitura(10,leituraNaoCalibrada);
rperoba 0:9efe13b5d868 119
rperoba 0:9efe13b5d868 120 if((leituraExterna > parametroDeLeituraAceitavel) || (leituraInterna > parametroDeLeituraAceitavel)){
rperoba 0:9efe13b5d868 121 return true;
rperoba 0:9efe13b5d868 122 }
rperoba 0:9efe13b5d868 123 return false ;
rperoba 0:9efe13b5d868 124 }
rperoba 0:9efe13b5d868 125
rperoba 0:9efe13b5d868 126 bool LerAsaEsquerda () //Retorna True se os sensores da asa esquerda lerem a linha
rperoba 0:9efe13b5d868 127 {
rperoba 0:9efe13b5d868 128 leituraNaoCalibrada = AsaEsquerdaExterno;
rperoba 0:9efe13b5d868 129 float leituraExterna = CalibragemDeLeitura(8,leituraNaoCalibrada);
rperoba 0:9efe13b5d868 130
rperoba 0:9efe13b5d868 131 leituraNaoCalibrada = AsaEsquerdaInterno;
rperoba 0:9efe13b5d868 132 float leituraInterna = CalibragemDeLeitura(9,leituraNaoCalibrada);
rperoba 0:9efe13b5d868 133
rperoba 0:9efe13b5d868 134 if((leituraExterna > parametroDeLeituraAceitavel) || (leituraInterna > parametroDeLeituraAceitavel)){
rperoba 0:9efe13b5d868 135 return true;
rperoba 0:9efe13b5d868 136 }
rperoba 0:9efe13b5d868 137 return false ;
rperoba 0:9efe13b5d868 138 }
rperoba 0:9efe13b5d868 139
rperoba 0:9efe13b5d868 140 bool LerTurbo()
rperoba 0:9efe13b5d868 141 {
rperoba 0:9efe13b5d868 142 return SensorCentral.read();
rperoba 0:9efe13b5d868 143 }