Nestor Pereira-Neto / Mbed 2 deprecated CarrinhoLabirinto

Robô móvel com FREDOM KL25Z

Dependencies:   Servo mbed HC-SR04

main.cpp@1:027b210e631d, 2015-01-29 (annotated)

Committer:
Nestordp
Date:
Thu Jan 29 00:35:29 2015 +0000
Revision:
1:027b210e631d
Parent:
0:182a12372dd4
Child:
2:2e759e231fb6
Acrescentado controle proporcional e lineariza??o

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nestordp 0:182a12372dd4 1 #include "mbed.h"
Nestordp 0:182a12372dd4 2 #include "Servo.h"
Nestordp 0:182a12372dd4 3
Nestordp 0:182a12372dd4 4 #define velocidade(a, b) MDPWM = a; MEPWM = b
Nestordp 0:182a12372dd4 5 #define frente() MDdirect = 1; MEdirect = 1
Nestordp 0:182a12372dd4 6 #define re() MDdirect = 0; MEdirect = 0
Nestordp 0:182a12372dd4 7 #define esquerda() MDdirect = 1; MEdirect = 0
Nestordp 0:182a12372dd4 8 #define direita() MDdirect = 0; MEdirect = 1
Nestordp 0:182a12372dd4 9
Nestordp 0:182a12372dd4 10 //Sonar Frente
Nestordp 0:182a12372dd4 11 //DigitalOut trig(PTB2,0); //Configuração do pino de Trigger
Nestordp 0:182a12372dd4 12 //InterruptIn echo(D12); //Configuração da interrupção por pino de Echo
Nestordp 1:027b210e631d 13
Nestordp 0:182a12372dd4 14
Nestordp 0:182a12372dd4 15
Nestordp 0:182a12372dd4 16
Nestordp 0:182a12372dd4 17 DigitalOut bip(D2); //
Nestordp 0:182a12372dd4 18 Servo myservo(D3); //
Nestordp 0:182a12372dd4 19 DigitalOut MEdirect(D4); //Motor 2 Direction control
Nestordp 0:182a12372dd4 20 PwmOut MEPWM(D5); //Motor 2 PWM control
Nestordp 0:182a12372dd4 21 PwmOut MDPWM(D6); //Motor 1 PWM control
Nestordp 0:182a12372dd4 22 DigitalOut MDdirect(D7); //Motor 1 Direction control
Nestordp 1:027b210e631d 23
Nestordp 1:027b210e631d 24
Nestordp 1:027b210e631d 25 DigitalOut trigF(A2,0); //Configuração do pino de Trigger
Nestordp 1:027b210e631d 26 DigitalIn echoF(D12); //Configuração da interrupção por pino de Echo
Nestordp 1:027b210e631d 27
Nestordp 1:027b210e631d 28 //Sonar Direito
Nestordp 1:027b210e631d 29 DigitalOut trigD(A1, 0); //Configuração do pino de Trigger
Nestordp 1:027b210e631d 30 DigitalIn echoD(D10); //Configuração da interrupção por pino de Echo
Nestordp 1:027b210e631d 31
Nestordp 1:027b210e631d 32 //Sonar Esqerdo
Nestordp 1:027b210e631d 33 DigitalOut trigE(A0,0); //Configuração do pino de Trigger
Nestordp 1:027b210e631d 34 InterruptIn echoE(D11); //Configuração da interrupção por pino de Echo
Nestordp 1:027b210e631d 35
Nestordp 0:182a12372dd4 36 DigitalOut Led(LED1);
Nestordp 0:182a12372dd4 37
Nestordp 0:182a12372dd4 38 Serial pc(USBTX,USBRX); //Configuração da comunicação serial para enviar o valor do sensor
Nestordp 0:182a12372dd4 39 Timer tempo; //Para usar rotinas de tempo
Nestordp 0:182a12372dd4 40
Nestordp 0:182a12372dd4 41
Nestordp 0:182a12372dd4 42
Nestordp 0:182a12372dd4 43 void iniP(void);
Nestordp 0:182a12372dd4 44 void finP(void);
Nestordp 1:027b210e631d 45 void lerSonarF(void);
Nestordp 1:027b210e631d 46 void lerSonarD(void);
Nestordp 1:027b210e631d 47 void lerSonarE(void);
Nestordp 1:027b210e631d 48 void sonarFrente(void);
Nestordp 0:182a12372dd4 49
Nestordp 0:182a12372dd4 50 float tdist=0, distcm=0, distin=0, dist0=0;
Nestordp 1:027b210e631d 51 float dist_esq = 0, dist_dir = 0, dist_fre = 0, razDE = 0;
Nestordp 1:027b210e631d 52 char flagTempo = 0;
Nestordp 0:182a12372dd4 53
Nestordp 0:182a12372dd4 54 int main() {
Nestordp 0:182a12372dd4 55 printf("INICIA\n");
Nestordp 0:182a12372dd4 56 myservo.calibrate(0.0013, 45.0);
Nestordp 0:182a12372dd4 57 wait_ms(3000);
Nestordp 0:182a12372dd4 58 //myservo.position(-33.0); todo pra direita
Nestordp 0:182a12372dd4 59 //myservo.position(30.8); todo pra esquerda
Nestordp 0:182a12372dd4 60 myservo.position(-3.0);
Nestordp 0:182a12372dd4 61 Led = 1;
Nestordp 0:182a12372dd4 62 printf("COMECA O PROGRAMA\n");
Nestordp 1:027b210e631d 63 velocidade(0.48, 0.48);
Nestordp 0:182a12372dd4 64 frente();
Nestordp 0:182a12372dd4 65
Nestordp 0:182a12372dd4 66 while(1){
Nestordp 1:027b210e631d 67 lerSonarF();
Nestordp 0:182a12372dd4 68 dist_fre = distcm;
Nestordp 1:027b210e631d 69 printf("Distancia detectada pelo sensor Frente %.2f cm \n",dist_fre);
Nestordp 1:027b210e631d 70 //wait_ms(1000);
Nestordp 1:027b210e631d 71
Nestordp 1:027b210e631d 72 lerSonarE();
Nestordp 1:027b210e631d 73 dist_esq = distcm;
Nestordp 1:027b210e631d 74 printf("Distancia detectada pelo sensor esquerdo %.2f cm \n",dist_esq);
Nestordp 1:027b210e631d 75 //wait_ms(1000);
Nestordp 1:027b210e631d 76 lerSonarD();
Nestordp 1:027b210e631d 77 dist_dir = distcm;
Nestordp 1:027b210e631d 78 printf("Distancia detectada pelo sensor direito %.2f cm \n",dist_dir);
Nestordp 1:027b210e631d 79 //wait_ms(1000);
Nestordp 1:027b210e631d 80
Nestordp 1:027b210e631d 81 razDE = dist_dir / dist_esq;
Nestordp 0:182a12372dd4 82
Nestordp 0:182a12372dd4 83 if((dist_fre <= 30) && (dist_fre > 3)){
Nestordp 1:027b210e631d 84 sonarFrente();
Nestordp 1:027b210e631d 85 }
Nestordp 1:027b210e631d 86
Nestordp 1:027b210e631d 87 if(razDE > 1){
Nestordp 1:027b210e631d 88 velocidade(0.22, 0.48);
Nestordp 0:182a12372dd4 89 }
Nestordp 1:027b210e631d 90 else if(razDE < 1){
Nestordp 1:027b210e631d 91 velocidade(0.48, 0.22);
Nestordp 1:027b210e631d 92 }
Nestordp 0:182a12372dd4 93 }
Nestordp 1:027b210e631d 94 }
Nestordp 1:027b210e631d 95
Nestordp 1:027b210e631d 96 void lerSonarD(void)
Nestordp 1:027b210e631d 97 {
Nestordp 1:027b210e631d 98 trigD=1; //Inicio do trigger
Nestordp 1:027b210e631d 99 wait_us(10); //10us de pulso
Nestordp 1:027b210e631d 100 trigD=0; //Fim do trigger
Nestordp 1:027b210e631d 101 while(!echoD);
Nestordp 1:027b210e631d 102 tempo.start();
Nestordp 1:027b210e631d 103 while(echoD);
Nestordp 1:027b210e631d 104 tdist = tempo.read_us(); //Leitura do tempo transcorrido
Nestordp 1:027b210e631d 105 distcm = tdist/58; //Cálculo da distância detectada em "cm"
Nestordp 1:027b210e631d 106 //distin = tdist/148; //Cálculo da distância detectada em "in"
Nestordp 1:027b210e631d 107 tempo.stop(); //Paro o temporizador
Nestordp 1:027b210e631d 108 tempo.reset(); //Reset para o próximo ciclo
Nestordp 1:027b210e631d 109 }
Nestordp 1:027b210e631d 110
Nestordp 1:027b210e631d 111 void lerSonarF(void)
Nestordp 1:027b210e631d 112 {
Nestordp 1:027b210e631d 113 trigF=1; //Inicio do trigger
Nestordp 1:027b210e631d 114 wait_us(10); //10us de pulso
Nestordp 1:027b210e631d 115 trigF=0; //Fim do trigger
Nestordp 1:027b210e631d 116 while(!echoF);
Nestordp 1:027b210e631d 117 tempo.start();
Nestordp 1:027b210e631d 118 while(echoF);
Nestordp 1:027b210e631d 119 tdist = tempo.read_us(); //Leitura do tempo transcorrido
Nestordp 1:027b210e631d 120 distcm = tdist/58; //Cálculo da distância detectada em "cm"
Nestordp 1:027b210e631d 121 //distin = tdist/148; //Cálculo da distância detectada em "in"
Nestordp 1:027b210e631d 122 tempo.stop(); //Paro o temporizador
Nestordp 1:027b210e631d 123 tempo.reset(); //Reset para o próximo ciclo
Nestordp 0:182a12372dd4 124 }
Nestordp 0:182a12372dd4 125
Nestordp 1:027b210e631d 126 void lerSonarE(void)
Nestordp 1:027b210e631d 127 {
Nestordp 1:027b210e631d 128 trigE = 1; //Inicio do trigger
Nestordp 1:027b210e631d 129 wait_us(10); //10us de pulso
Nestordp 1:027b210e631d 130 trigE = 0; //Fim do trigger
Nestordp 1:027b210e631d 131 while(!echoE);
Nestordp 1:027b210e631d 132 tempo.start();
Nestordp 1:027b210e631d 133 while(echoE);
Nestordp 0:182a12372dd4 134 tdist = tempo.read_us(); //Leitura do tempo transcorrido
Nestordp 0:182a12372dd4 135 distcm = tdist/58; //Cálculo da distância detectada em "cm"
Nestordp 1:027b210e631d 136 //distin = tdist/148; //Cálculo da distância detectada em "in"
Nestordp 0:182a12372dd4 137 tempo.stop(); //Paro o temporizador
Nestordp 0:182a12372dd4 138 tempo.reset(); //Reset para o próximo ciclo
Nestordp 0:182a12372dd4 139 }
Nestordp 0:182a12372dd4 140
Nestordp 1:027b210e631d 141
Nestordp 1:027b210e631d 142 void sonarFrente(void){
Nestordp 1:027b210e631d 143 bip = 1;
Nestordp 1:027b210e631d 144 printf("PARA\n");
Nestordp 1:027b210e631d 145 re();
Nestordp 0:182a12372dd4 146 wait_ms(100);
Nestordp 1:027b210e631d 147 velocidade(0.0, 0.0);
Nestordp 1:027b210e631d 148 bip = 0;
Nestordp 1:027b210e631d 149
Nestordp 1:027b210e631d 150 myservo.position(-33.0);
Nestordp 1:027b210e631d 151 wait_ms(1000);
Nestordp 1:027b210e631d 152 lerSonarF();
Nestordp 1:027b210e631d 153 printf("Distancia direita %.2f cm \n",distcm);
Nestordp 1:027b210e631d 154 dist_dir = distcm;
Nestordp 1:027b210e631d 155
Nestordp 1:027b210e631d 156 myservo.position(30.8);
Nestordp 1:027b210e631d 157 wait_ms(1000);
Nestordp 1:027b210e631d 158 lerSonarF();
Nestordp 1:027b210e631d 159 printf("Distancia esquerda %.2f cm \n",distcm);
Nestordp 1:027b210e631d 160 dist_esq = distcm;
Nestordp 1:027b210e631d 161
Nestordp 1:027b210e631d 162 if(dist_dir > dist_esq){
Nestordp 1:027b210e631d 163 printf("VIRA ESQUERDA \n");
Nestordp 1:027b210e631d 164 direita();
Nestordp 1:027b210e631d 165 velocidade(0.48, 0.48);
Nestordp 1:027b210e631d 166 wait_ms(250);
Nestordp 1:027b210e631d 167 velocidade(0.0, 0.0);
Nestordp 1:027b210e631d 168 myservo.position(-3.0);
Nestordp 1:027b210e631d 169 wait_ms(500);
Nestordp 1:027b210e631d 170 velocidade(0.48, 0.48);
Nestordp 1:027b210e631d 171 frente();
Nestordp 1:027b210e631d 172 }
Nestordp 1:027b210e631d 173 else if(dist_dir < dist_esq){
Nestordp 1:027b210e631d 174 printf("VIRA ESQUERDA \n");
Nestordp 1:027b210e631d 175 esquerda();
Nestordp 1:027b210e631d 176 velocidade(0.48, 0.48);
Nestordp 1:027b210e631d 177 wait_ms(250);
Nestordp 1:027b210e631d 178 velocidade(0.0, 0.0);
Nestordp 1:027b210e631d 179 myservo.position(-3.0);
Nestordp 1:027b210e631d 180 wait_ms(500);
Nestordp 1:027b210e631d 181 velocidade(0.48, 0.48);
Nestordp 1:027b210e631d 182 frente();
Nestordp 1:027b210e631d 183 }
Nestordp 1:027b210e631d 184 }