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