RioBotz / Mbed 2 deprecated SeguidorDeLinhaV2

Codigo do seguidor de linha V2 2020.2

Dependencies:   mbed

main.cpp@0:9efe13b5d868, 2020-09-19 (annotated)

Committer:
rperoba
Date:
Sat Sep 19 20:43:59 2020 +0000
Revision:
0:9efe13b5d868
Child:
2:4db1486af4d5
Child:
6:5551834026ef
Primeira versao do codigo do seguidor de linha v2,utilizando bibliotecas para melhor organizar suas funcoes

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 0:9efe13b5d868 8 InterruptIn Botao (p5);
rperoba 0:9efe13b5d868 9 DigitalOut Led (p6);
rperoba 0:9efe13b5d868 10
rperoba 0:9efe13b5d868 11 int botao = 0;
rperoba 0:9efe13b5d868 12 int comando = 1;
rperoba 0:9efe13b5d868 13
rperoba 0:9efe13b5d868 14 float correcaoPD;
rperoba 0:9efe13b5d868 15 double erroSensores;
rperoba 0:9efe13b5d868 16
rperoba 0:9efe13b5d868 17 float velocidadeEsquerda;
rperoba 0:9efe13b5d868 18 float velocidadeDireita;
rperoba 0:9efe13b5d868 19 float velocidadeRealEsquerda;
rperoba 0:9efe13b5d868 20 float velocidadeRealDireita;
rperoba 0:9efe13b5d868 21 float velocidadeBase = 0.18;
rperoba 0:9efe13b5d868 22
rperoba 0:9efe13b5d868 23 Callback<void()> AlteraComando(){
rperoba 0:9efe13b5d868 24 botao++;
rperoba 0:9efe13b5d868 25 }
rperoba 0:9efe13b5d868 26
rperoba 0:9efe13b5d868 27 void PiscaLed(){
rperoba 0:9efe13b5d868 28 if(Led){
rperoba 0:9efe13b5d868 29 Led = 0;
rperoba 0:9efe13b5d868 30 }
rperoba 0:9efe13b5d868 31 else{
rperoba 0:9efe13b5d868 32 Led = 1;
rperoba 0:9efe13b5d868 33 }
rperoba 0:9efe13b5d868 34 }
rperoba 0:9efe13b5d868 35
rperoba 0:9efe13b5d868 36 int main()
rperoba 0:9efe13b5d868 37 {
rperoba 0:9efe13b5d868 38 Botao.rise(AlteraComando());
rperoba 0:9efe13b5d868 39 Setup_Motores();
rperoba 0:9efe13b5d868 40 while(botao == 0){ //Espera o aperto do botao para iniciar a calibragem, o led permanecera acesso para indicar esta fase
rperoba 0:9efe13b5d868 41 wait_us(10);
rperoba 0:9efe13b5d868 42 Led = 1;
rperoba 0:9efe13b5d868 43 }
rperoba 0:9efe13b5d868 44 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 45 { //Espera que se aperte o botao para terminar a calibragem
rperoba 0:9efe13b5d868 46 CalibragemDosSensores();
rperoba 0:9efe13b5d868 47 PiscaLed();
rperoba 0:9efe13b5d868 48 }
rperoba 0:9efe13b5d868 49
rperoba 0:9efe13b5d868 50 Led = 0; //Certificando que o led esta apagado depois da calibragem
rperoba 0:9efe13b5d868 51
rperoba 0:9efe13b5d868 52 wait_us(5000); //Espera 5 segundos para poder mudar a posição de calibrar os sensores para a de correr
rperoba 0:9efe13b5d868 53 //Tocar o buzzer parar avisar q ai começar a correr
rperoba 0:9efe13b5d868 54 while(1){
rperoba 0:9efe13b5d868 55 switch(comando){
rperoba 0:9efe13b5d868 56 //Comando = 1 -> Faz a pista devagar e mapeando
rperoba 0:9efe13b5d868 57 //Comando = 2 -> Faz a pista usando a leitura do mapa
rperoba 0:9efe13b5d868 58 //Comando = 3 -> Faz a usando a leitura do mapa e ultrapassando os limites de velocidade
rperoba 0:9efe13b5d868 59 case 1:
rperoba 0:9efe13b5d868 60 LerSensoresFrontais(); //Leitura dos sensores frontais
rperoba 0:9efe13b5d868 61 erroSensores = CalculaErro(); //Calcula o arco do erro dos sensores
rperoba 0:9efe13b5d868 62 correcaoPD = PID_Sensores(erroSensores); //Calcula a correção do PD angular
rperoba 0:9efe13b5d868 63 velocidadeEsquerda = velocidadeBase + correcaoPD; //Ajusta a inicialmente a velocidade das rodas para manter na linha
rperoba 0:9efe13b5d868 64 velocidadeDireita = velocidadeBase - correcaoPD;
rperoba 0:9efe13b5d868 65 //velocidadeRealDireita = ; //Pega a velocidade do seguidor lida pelos encoderes
rperoba 0:9efe13b5d868 66 //velocidadeRealEsquerda = ;
rperoba 0:9efe13b5d868 67 velocidadeEsquerda = PID_rodaEsquerda(velocidadeEsquerda,velocidadeRealEsquerda); //Faz o ajusta final da velocidade das rodas
rperoba 0:9efe13b5d868 68 velocidadeDireita = PID_rodaDireita(velocidadeDireita,velocidadeRealDireita);
rperoba 0:9efe13b5d868 69 ControleMotores(velocidadeEsquerda,velocidadeDireita); //Muda a velocidade das rodas para as velocidades calculadas anteriormente
rperoba 0:9efe13b5d868 70 //Mapear a pista
rperoba 0:9efe13b5d868 71
rperoba 0:9efe13b5d868 72 break;
rperoba 0:9efe13b5d868 73
rperoba 0:9efe13b5d868 74 case 2:
rperoba 0:9efe13b5d868 75 break;
rperoba 0:9efe13b5d868 76
rperoba 0:9efe13b5d868 77 case 3:
rperoba 0:9efe13b5d868 78 break;
rperoba 0:9efe13b5d868 79
rperoba 0:9efe13b5d868 80 default:
rperoba 0:9efe13b5d868 81 ParadaFinal();
rperoba 0:9efe13b5d868 82 break;
rperoba 0:9efe13b5d868 83 }
rperoba 0:9efe13b5d868 84
rperoba 0:9efe13b5d868 85
rperoba 0:9efe13b5d868 86 }
rperoba 0:9efe13b5d868 87
rperoba 0:9efe13b5d868 88 }
rperoba 0:9efe13b5d868 89