Codigo do seguidor de linha V2 2020.2
Dependencies: mbed
LineSensor.cpp
- Committer:
- rperoba
- Date:
- 2020-10-10
- Revision:
- 6:5551834026ef
- Parent:
- 0:9efe13b5d868
File content as of revision 6:5551834026ef:
#include "mbed.h" #include "LineSensor.h" #include "bluetooth.h" #define DEBUG true int comprimentoDoRobo = 20 ; //Comprimento do seguidor em mm AnalogIn AsaDireitaInterno (p16); AnalogIn AsaDireitaExterno (p15); AnalogIn AsaEsquerdaInterno (p17); AnalogIn AsaEsquerdaExterno (p18); AnalogIn MultiplexadorIn (p19); DigitalIn SensorCentral (p25); DigitalOut Multiplexador0 (p12); DigitalOut Multiplexador1 (p13); DigitalOut Multiplexador2 (p14); PwmOut Buzzer (p8); float leituraNaoCalibrada; float leituraMaxima [12] = {0,0,0,0,0,0,0,0,0,0,0,0}; float leituraMinima [12] = {1,1,1,1,1,1,1,1,1,1,1,1}; float leituraCalibrada [8]; float parametroDeLeituraAceitavel = 0.7; float posicaoDosSensores [8] = {-37.5,-27.5,-17.5,-7.5,7.5,17.5,27.5,37.5}; int tabelaMultiplexador0 [8] = {0,1,0,1,0,1,0,1}; int tabelaMultiplexador1 [8] = {0,0,1,1,0,0,1,1}; int tabelaMultiplexador2 [8] = {0,0,0,0,1,1,1,1}; void AlocaLeitura (int posicaoSensor,float leitura) //Adiciona a leitura nao calibrada aos vetores de leitura maxima ou minima { if (leitura > leituraMaxima[posicaoSensor]) { leituraMaxima[posicaoSensor] = leitura; } else if (leitura < leituraMinima[posicaoSensor]) { leituraMinima[posicaoSensor] = leitura; } } void CalibragemDosSensores() //Registra as leituras minimas e maximas feitas por cada sensor //para funcionar precisa rodar a função continuamente e passar os sensores por cima da linha branca e do fundo preto da pista { for(int i = 0;i<8;i++){ //Calibrando sensores frontais //Ajustando o multiplexador Multiplexador0 = tabelaMultiplexador0[i]; Multiplexador1 = tabelaMultiplexador1[i]; Multiplexador2 = tabelaMultiplexador2[i]; //Lendo o sensor a adicionando na leitura leituraNaoCalibrada = MultiplexadorIn; //Salvando as leituras máximas e minimas AlocaLeitura(i,leituraNaoCalibrada); } //Calibragem das asas leituraNaoCalibrada = AsaEsquerdaExterno; AlocaLeitura(8, leituraNaoCalibrada); leituraNaoCalibrada = AsaEsquerdaInterno; AlocaLeitura(9, leituraNaoCalibrada); leituraNaoCalibrada = AsaDireitaInterno; AlocaLeitura(10, leituraNaoCalibrada); leituraNaoCalibrada = AsaDireitaExterno; AlocaLeitura(11, leituraNaoCalibrada); } float CalibragemDeLeitura (int numeroDoSensor,float leitura) // Calibra cada leitura individualmente baseado nas leituras maximas e minimas ja cadastradas de cada sensor { return (leitura - leituraMinima[numeroDoSensor])/(leituraMaxima[numeroDoSensor]-leituraMinima[numeroDoSensor]) ; } void LerSensoresFrontais () //Faz a leitura dos sensores frontais e salva no vetor leituraCalibrada { int contador = 1; for(int i = 0;i < 8;i++){ Multiplexador0 = tabelaMultiplexador0[i]; Multiplexador1 = tabelaMultiplexador1[i]; Multiplexador2 = tabelaMultiplexador2[i]; leituraNaoCalibrada = MultiplexadorIn; leituraCalibrada[i] = CalibragemDeLeitura(i,leituraNaoCalibrada); } } float CalculaErro () //Retorna o erro em graus para o calculo do PID { float distancia = 0; for(int i = 0;i < 8;i++){//Calcula a media ponderada das leituras pra achar a distancia da linha para o setpoint distancia += ( leituraCalibrada[i] + posicaoDosSensores [i] )/ leituraCalibrada [i]; } return atan(distancia/comprimentoDoRobo); } bool LerAsaDireita () //Retorna True se os sensores da asa direita lerem a linha { leituraNaoCalibrada = AsaDireitaExterno; float leituraExterna = CalibragemDeLeitura(11,leituraNaoCalibrada); leituraNaoCalibrada = AsaDireitaInterno; float leituraInterna = CalibragemDeLeitura(10,leituraNaoCalibrada); if((leituraExterna > parametroDeLeituraAceitavel) || (leituraInterna > parametroDeLeituraAceitavel)){ return true; } return false ; } bool LerAsaEsquerda () //Retorna True se os sensores da asa esquerda lerem a linha { leituraNaoCalibrada = AsaEsquerdaExterno; float leituraExterna = CalibragemDeLeitura(8,leituraNaoCalibrada); leituraNaoCalibrada = AsaEsquerdaInterno; float leituraInterna = CalibragemDeLeitura(9,leituraNaoCalibrada); if((leituraExterna > parametroDeLeituraAceitavel) || (leituraInterna > parametroDeLeituraAceitavel)){ return true; } return false ; } bool LerTurbo() { return SensorCentral.read(); }