RioBotz / Mbed 2 deprecated SeguidorDeLinhaV2

Codigo do seguidor de linha V2 2020.2

Dependencies:   mbed

main.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 #include "mbed.h"
rperoba 0:9efe13b5d868 2 #include "LineSensor.h"
rperoba 0:9efe13b5d868 3 #include "bluetooth.h"
rperoba 0:9efe13b5d868 4 #include "encoder.h"
rperoba 0:9efe13b5d868 5 #include "motors.h"
rperoba 0:9efe13b5d868 6 #include "PID.h"
rperoba 0:9efe13b5d868 7
rperoba 6:5551834026ef 8 #define TAMANHODOMAPA 50
rperoba 6:5551834026ef 9 #define PERIMETRODARODA 26
rperoba 6:5551834026ef 10
rperoba 0:9efe13b5d868 11 InterruptIn Botao (p5);
rperoba 0:9efe13b5d868 12 DigitalOut Led (p6);
rperoba 0:9efe13b5d868 13
rperoba 6:5551834026ef 14 InterruptIn OUTA_RIGHT(p26);
rperoba 6:5551834026ef 15 DigitalIn OUTB_RIGHT(p11);
rperoba 6:5551834026ef 16
rperoba 6:5551834026ef 17 InterruptIn OUTA_LEFT(p21);
rperoba 6:5551834026ef 18 DigitalIn OUTB_LEFT(p7);
rperoba 6:5551834026ef 19
rperoba 0:9efe13b5d868 20 int botao = 0;
rperoba 0:9efe13b5d868 21 int comando = 1;
rperoba 6:5551834026ef 22 int contadorAsaDireita = 0;
rperoba 6:5551834026ef 23 int posicaoNoMapa = 0;
rperoba 6:5551834026ef 24 float distanciaPraCurva;
rperoba 6:5551834026ef 25
rperoba 0:9efe13b5d868 26 float correcaoPD;
rperoba 0:9efe13b5d868 27 double erroSensores;
rperoba 0:9efe13b5d868 28
rperoba 0:9efe13b5d868 29 float velocidadeEsquerda;
rperoba 0:9efe13b5d868 30 float velocidadeDireita;
rperoba 0:9efe13b5d868 31 float velocidadeRealEsquerda;
rperoba 0:9efe13b5d868 32 float velocidadeRealDireita;
rperoba 6:5551834026ef 33 float velocidadeAtual = 0.18;
rperoba 6:5551834026ef 34 float novaVelocidade;
rperoba 6:5551834026ef 35 float correcaoEsquerda;
rperoba 6:5551834026ef 36 float correcaoDireita;
rperoba 6:5551834026ef 37
rperoba 6:5551834026ef 38 bool estadoAsaDireita = false;
rperoba 6:5551834026ef 39 bool estadoAsaEsquerda = false;
rperoba 6:5551834026ef 40
rperoba 6:5551834026ef 41 float mapaPraCorrer[TAMANHODOMAPA][4];
rperoba 0:9efe13b5d868 42
rperoba 6:5551834026ef 43 void leArquivo ()
rperoba 6:5551834026ef 44 {
rperoba 6:5551834026ef 45 FILE *mapaArquivo = fopen("/local/mapaTeste.txt","r");
rperoba 6:5551834026ef 46 if(mapaArquivo == NULL){
rperoba 6:5551834026ef 47 printf("Impossivel criar o arquivo");
rperoba 6:5551834026ef 48 return;
rperoba 6:5551834026ef 49 }
rperoba 6:5551834026ef 50 for(int i = 0;i<TAMANHODOMAPA;i++)
rperoba 6:5551834026ef 51 {
rperoba 6:5551834026ef 52 fscanf(mapaArquivo,"%f-%f-%f-%f\n",&mapaPraCorrer[i][0],&mapaPraCorrer[i][1],&mapaPraCorrer[i][2],&mapaPraCorrer[i][3]); // talvez tenha q salvar numa variavel, igualar a um vetor
rperoba 6:5551834026ef 53 }
rperoba 6:5551834026ef 54
rperoba 6:5551834026ef 55 fclose(mapaArquivo);
rperoba 6:5551834026ef 56 }
rperoba 6:5551834026ef 57
rperoba 6:5551834026ef 58 void AlteraComando(){
rperoba 0:9efe13b5d868 59 botao++;
rperoba 0:9efe13b5d868 60 }
rperoba 0:9efe13b5d868 61
rperoba 0:9efe13b5d868 62 void PiscaLed(){
rperoba 0:9efe13b5d868 63 if(Led){
rperoba 0:9efe13b5d868 64 Led = 0;
rperoba 0:9efe13b5d868 65 }
rperoba 0:9efe13b5d868 66 else{
rperoba 0:9efe13b5d868 67 Led = 1;
rperoba 0:9efe13b5d868 68 }
rperoba 0:9efe13b5d868 69 }
rperoba 0:9efe13b5d868 70
rperoba 0:9efe13b5d868 71 int main()
rperoba 0:9efe13b5d868 72 {
rperoba 6:5551834026ef 73 Botao.rise(&AlteraComando);
rperoba 6:5551834026ef 74 OUTA_RIGHT.rise(&contaPulso_direito);
rperoba 6:5551834026ef 75 OUTA_LEFT.rise(&contaPulso_esquerdo);
rperoba 6:5551834026ef 76 leArquivo();
rperoba 6:5551834026ef 77
rperoba 0:9efe13b5d868 78 Setup_Motores();
rperoba 0:9efe13b5d868 79 while(botao == 0){ //Espera o aperto do botao para iniciar a calibragem, o led permanecera acesso para indicar esta fase
rperoba 0:9efe13b5d868 80 wait_us(10);
rperoba 0:9efe13b5d868 81 Led = 1;
rperoba 0:9efe13b5d868 82 }
rperoba 0:9efe13b5d868 83 while (botao == 1) //Inicialmente calibra os sensores ao ligar o seguidor,o led fica piscando para mostrar que a calibragem esta acontecendo
rperoba 0:9efe13b5d868 84 { //Espera que se aperte o botao para terminar a calibragem
rperoba 0:9efe13b5d868 85 CalibragemDosSensores();
rperoba 0:9efe13b5d868 86 PiscaLed();
rperoba 0:9efe13b5d868 87 }
rperoba 0:9efe13b5d868 88
rperoba 0:9efe13b5d868 89 Led = 0; //Certificando que o led esta apagado depois da calibragem
rperoba 0:9efe13b5d868 90
rperoba 0:9efe13b5d868 91 wait_us(5000); //Espera 5 segundos para poder mudar a posição de calibrar os sensores para a de correr
rperoba 6:5551834026ef 92 //Tocar o buzzer parar avisar q ai começar a correr
rperoba 0:9efe13b5d868 93 switch(comando){
rperoba 0:9efe13b5d868 94 //Comando = 1 -> Faz a pista devagar e mapeando
rperoba 0:9efe13b5d868 95 //Comando = 2 -> Faz a pista usando a leitura do mapa
rperoba 0:9efe13b5d868 96 //Comando = 3 -> Faz a usando a leitura do mapa e ultrapassando os limites de velocidade
rperoba 0:9efe13b5d868 97 case 1:
rperoba 6:5551834026ef 98 while(1){
rperoba 6:5551834026ef 99 velocidadeAtual = VelocidadeBateria(velocidadeAtual); //Ajuste da velocidade por causa da descarga da bateria
rperoba 0:9efe13b5d868 100 LerSensoresFrontais(); //Leitura dos sensores frontais
rperoba 0:9efe13b5d868 101 erroSensores = CalculaErro(); //Calcula o arco do erro dos sensores
rperoba 0:9efe13b5d868 102 correcaoPD = PID_Sensores(erroSensores); //Calcula a correção do PD angular
rperoba 6:5551834026ef 103 velocidadeEsquerda = velocidadeAtual + correcaoPD; //Ajusta a inicialmente a velocidade das rodas para manter na linha
rperoba 6:5551834026ef 104 velocidadeDireita = velocidadeAtual - correcaoPD;
rperoba 6:5551834026ef 105 velocidadeRealDireita = retornaVelDireita(); //velocidade em /m/s
rperoba 6:5551834026ef 106 velocidadeRealEsquerda = retornaVelEsquerda();
rperoba 6:5551834026ef 107 velocidadeEsquerda = transformaPWM(velocidadeEsquerda); //Converte velocidade PWM em m/s
rperoba 6:5551834026ef 108 velocidadeDireita = transformaPWM(velocidadeDireita);
rperoba 6:5551834026ef 109 correcaoEsquerda = PID_rodaEsquerda(velocidadeEsquerda,velocidadeRealEsquerda); //Faz o ajusta final da velocidade das rodas
rperoba 6:5551834026ef 110 correcaoDireita = PID_rodaDireita(velocidadeDireita,velocidadeRealDireita);
rperoba 6:5551834026ef 111 velocidadeEsquerda += correcaoEsquerda;
rperoba 6:5551834026ef 112 velocidadeDireita += correcaoDireita;
rperoba 0:9efe13b5d868 113 ControleMotores(velocidadeEsquerda,velocidadeDireita); //Muda a velocidade das rodas para as velocidades calculadas anteriormente
rperoba 6:5551834026ef 114 estadoAsaDireita = LerAsaDireita();
rperoba 6:5551834026ef 115 estadoAsaEsquerda = LerAsaEsquerda();
rperoba 6:5551834026ef 116 if(estadoAsaEsquerda || estadoAsaDireita ){ //Checa os sensores laterais
rperoba 6:5551834026ef 117 mapeamento(estadoAsaEsquerda,estadoAsaDireita); //faz o mapeamento da pista
rperoba 6:5551834026ef 118 if (estadoAsaDireita && !estadoAsaEsquerda){
rperoba 6:5551834026ef 119 contadorAsaDireita++; //Faz as contagens das marcacoes a direita
rperoba 6:5551834026ef 120 if(contadorAsaDireita >=2){//ARRUMAR UM JEITO DE DAR UM TEMPO ENTRE A LEITURA E O SEGUIDOR PARAR PARA ELE PASSAR TODO PELA LINHA DE CHAGADA
rperoba 6:5551834026ef 121 Aceleracao(0,0);
rperoba 6:5551834026ef 122 CriaParteQuatro();
rperoba 6:5551834026ef 123 criaArquivo();
rperoba 6:5551834026ef 124 ParadaFinal(); //Para o seguidor na segunda marcacao
rperoba 6:5551834026ef 125 }
rperoba 6:5551834026ef 126 }
rperoba 6:5551834026ef 127 }
rperoba 6:5551834026ef 128 }
rperoba 0:9efe13b5d868 129
rperoba 0:9efe13b5d868 130 break;
rperoba 0:9efe13b5d868 131
rperoba 6:5551834026ef 132 case 2: //Percorre a pista com o uso do mapa
rperoba 6:5551834026ef 133 while(1){
rperoba 6:5551834026ef 134 distanciaPraCurva =retornaPulsoDireito() * PERIMETRODARODA ;
rperoba 6:5551834026ef 135 if(distanciaPraCurva < mapaPraCorrer[posicaoNoMapa][1] - mapaPraCorrer[posicaoNoMapa][3]){ //Seleciona a velocidade
rperoba 6:5551834026ef 136 novaVelocidade = VelocidadeBateria (mapaPraCorrer[posicaoNoMapa+1][2]);
rperoba 6:5551834026ef 137 }
rperoba 6:5551834026ef 138 else{
rperoba 6:5551834026ef 139 novaVelocidade = VelocidadeBateria(mapaPraCorrer[posicaoNoMapa][2]);
rperoba 6:5551834026ef 140 }
rperoba 6:5551834026ef 141 Aceleracao(novaVelocidade,1); //Acelera ou desacelera o robo
rperoba 6:5551834026ef 142 LerSensoresFrontais(); //Leitura dos sensores frontais
rperoba 6:5551834026ef 143 erroSensores = CalculaErro(); //Calcula o arco do erro dos sensores
rperoba 6:5551834026ef 144 correcaoPD = PID_Sensores(erroSensores); //Calcula a correção do PD angular
rperoba 6:5551834026ef 145 velocidadeEsquerda = velocidadeAtual + correcaoPD; //Ajusta a inicialmente a velocidade das rodas para manter na linha
rperoba 6:5551834026ef 146 velocidadeDireita = velocidadeAtual - correcaoPD;
rperoba 6:5551834026ef 147 velocidadeRealDireita = retornaVelDireita();
rperoba 6:5551834026ef 148 velocidadeRealEsquerda = retornaVelEsquerda();
rperoba 6:5551834026ef 149 velocidadeEsquerda = transformaPWM(velocidadeEsquerda); //Converte velocidade PWM em m/s
rperoba 6:5551834026ef 150 velocidadeDireita = transformaPWM(velocidadeDireita);
rperoba 6:5551834026ef 151 correcaoEsquerda = PID_rodaEsquerda(velocidadeEsquerda,velocidadeRealEsquerda); //Faz o ajusta final da velocidade das rodas
rperoba 6:5551834026ef 152 correcaoDireita = PID_rodaDireita(velocidadeDireita,velocidadeRealDireita);
rperoba 6:5551834026ef 153 velocidadeEsquerda += correcaoEsquerda;
rperoba 6:5551834026ef 154 velocidadeDireita += correcaoDireita;
rperoba 6:5551834026ef 155 ControleMotores(velocidadeEsquerda,velocidadeDireita); //Muda a velocidade das rodas para as velocidades calculadas anteriormente
rperoba 6:5551834026ef 156 estadoAsaDireita = LerAsaDireita();
rperoba 6:5551834026ef 157 estadoAsaEsquerda = LerAsaEsquerda();
rperoba 6:5551834026ef 158 if(estadoAsaEsquerda || estadoAsaDireita ){ //Checa os sensores laterais
rperoba 6:5551834026ef 159 if(!estadoAsaEsquerda || estadoAsaDireita){ //Leu somente o asa direita
rperoba 6:5551834026ef 160 posicaoNoMapa++; //Atualiza a etapa do mapa da pista que esta sendo lido
rperoba 6:5551834026ef 161 }
rperoba 6:5551834026ef 162 if(estadoAsaDireita && !estadoAsaEsquerda){ //Leu somente a asa esquerda
rperoba 6:5551834026ef 163 contadorAsaDireita++; //Faz as contagens das marcacoes a direita
rperoba 6:5551834026ef 164 if(contadorAsaDireita >=2){
rperoba 6:5551834026ef 165 Aceleracao(0,0);
rperoba 6:5551834026ef 166 ParadaFinal(); //Para o seguidor na segunda marcacao
rperoba 6:5551834026ef 167 }
rperoba 6:5551834026ef 168 }
rperoba 6:5551834026ef 169 }
rperoba 6:5551834026ef 170 }
rperoba 6:5551834026ef 171
rperoba 0:9efe13b5d868 172 break;
rperoba 0:9efe13b5d868 173
rperoba 6:5551834026ef 174 case 3:
rperoba 6:5551834026ef 175 while(1){
rperoba 6:5551834026ef 176
rperoba 6:5551834026ef 177 }
rperoba 0:9efe13b5d868 178 break;
rperoba 0:9efe13b5d868 179
rperoba 0:9efe13b5d868 180 default:
rperoba 0:9efe13b5d868 181 ParadaFinal();
rperoba 0:9efe13b5d868 182 break;
rperoba 0:9efe13b5d868 183 }
rperoba 0:9efe13b5d868 184
rperoba 0:9efe13b5d868 185
rperoba 6:5551834026ef 186
rperoba 0:9efe13b5d868 187
rperoba 0:9efe13b5d868 188 }
rperoba 0:9efe13b5d868 189