Nestor Pereira-Neto / Mbed 2 deprecated CarrinhoLabirinto

Robô móvel com FREDOM KL25Z

Dependencies:   Servo mbed HC-SR04

main.cpp@0:182a12372dd4, 2015-01-27 (annotated)

Committer:
Nestordp
Date:
Tue Jan 27 21:57:22 2015 +0000
Revision:
0:182a12372dd4
Child:
1:027b210e631d
Rob? m?vel.

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 0:182a12372dd4 13 //Sonar Esquerdo
Nestordp 0:182a12372dd4 14 //DigitalOut trig(A0,0); //Configuração do pino de Trigger
Nestordp 0:182a12372dd4 15 //InterruptIn echo(D11); //Configuração da interrupção por pino de Echo
Nestordp 0:182a12372dd4 16
Nestordp 0:182a12372dd4 17
Nestordp 0:182a12372dd4 18
Nestordp 0:182a12372dd4 19 DigitalOut bip(D2); //
Nestordp 0:182a12372dd4 20 Servo myservo(D3); //
Nestordp 0:182a12372dd4 21 DigitalOut MEdirect(D4); //Motor 2 Direction control
Nestordp 0:182a12372dd4 22 PwmOut MEPWM(D5); //Motor 2 PWM control
Nestordp 0:182a12372dd4 23 PwmOut MDPWM(D6); //Motor 1 PWM control
Nestordp 0:182a12372dd4 24 DigitalOut MDdirect(D7); //Motor 1 Direction control
Nestordp 0:182a12372dd4 25 //Sonar Frente
Nestordp 0:182a12372dd4 26 DigitalOut trig(PTB2,0); //Configuração do pino de Trigger
Nestordp 0:182a12372dd4 27 InterruptIn echo(D12); //Configuração da interrupção por pino de Echo
Nestordp 0:182a12372dd4 28 DigitalOut Led(LED1);
Nestordp 0:182a12372dd4 29
Nestordp 0:182a12372dd4 30 Serial pc(USBTX,USBRX); //Configuração da comunicação serial para enviar o valor do sensor
Nestordp 0:182a12372dd4 31 Timer tempo; //Para usar rotinas de tempo
Nestordp 0:182a12372dd4 32
Nestordp 0:182a12372dd4 33
Nestordp 0:182a12372dd4 34
Nestordp 0:182a12372dd4 35 void iniP(void);
Nestordp 0:182a12372dd4 36 void finP(void);
Nestordp 0:182a12372dd4 37 void lerSonar(void);
Nestordp 0:182a12372dd4 38
Nestordp 0:182a12372dd4 39 float tdist=0, distcm=0, distin=0, dist0=0;
Nestordp 0:182a12372dd4 40 float dist_esq = 0, dist_dir = 0, dist_fre = 0;
Nestordp 0:182a12372dd4 41
Nestordp 0:182a12372dd4 42 int main() {
Nestordp 0:182a12372dd4 43 printf("INICIA\n");
Nestordp 0:182a12372dd4 44 myservo.calibrate(0.0013, 45.0);
Nestordp 0:182a12372dd4 45 wait_ms(3000);
Nestordp 0:182a12372dd4 46 //myservo.position(-33.0); todo pra direita
Nestordp 0:182a12372dd4 47 //myservo.position(30.8); todo pra esquerda
Nestordp 0:182a12372dd4 48 myservo.position(-3.0);
Nestordp 0:182a12372dd4 49 Led = 1;
Nestordp 0:182a12372dd4 50 printf("COMECA O PROGRAMA\n");
Nestordp 0:182a12372dd4 51 velocidade(0.38, 0.38);
Nestordp 0:182a12372dd4 52 frente();
Nestordp 0:182a12372dd4 53
Nestordp 0:182a12372dd4 54 while(1){
Nestordp 0:182a12372dd4 55 lerSonar();
Nestordp 0:182a12372dd4 56 printf("Distancia detectada pelo sensor %.2f cm \n",distcm);
Nestordp 0:182a12372dd4 57 dist_fre = distcm;
Nestordp 0:182a12372dd4 58
Nestordp 0:182a12372dd4 59 if((dist_fre <= 30) && (dist_fre > 3)){
Nestordp 0:182a12372dd4 60 bip = 1;
Nestordp 0:182a12372dd4 61 printf("PARA\n");
Nestordp 0:182a12372dd4 62 re();
Nestordp 0:182a12372dd4 63 wait_ms(100);
Nestordp 0:182a12372dd4 64 velocidade(0.0, 0.0);
Nestordp 0:182a12372dd4 65 bip = 0;
Nestordp 0:182a12372dd4 66
Nestordp 0:182a12372dd4 67 myservo.position(-33.0);
Nestordp 0:182a12372dd4 68 wait_ms(1000);
Nestordp 0:182a12372dd4 69 lerSonar();
Nestordp 0:182a12372dd4 70 printf("Distancia direita %.2f cm \n",distcm);
Nestordp 0:182a12372dd4 71 dist_dir = distcm;
Nestordp 0:182a12372dd4 72
Nestordp 0:182a12372dd4 73 myservo.position(30.8);
Nestordp 0:182a12372dd4 74 wait_ms(1000);
Nestordp 0:182a12372dd4 75 lerSonar();
Nestordp 0:182a12372dd4 76 printf("Distancia esquerda %.2f cm \n",distcm);
Nestordp 0:182a12372dd4 77 dist_esq = distcm;
Nestordp 0:182a12372dd4 78
Nestordp 0:182a12372dd4 79 if(dist_dir > dist_esq){
Nestordp 0:182a12372dd4 80 printf("VIRA ESQUERDA \n");
Nestordp 0:182a12372dd4 81 direita();
Nestordp 0:182a12372dd4 82 velocidade(0.38, 0.38);
Nestordp 0:182a12372dd4 83 wait_ms(250);
Nestordp 0:182a12372dd4 84 velocidade(0.0, 0.0);
Nestordp 0:182a12372dd4 85 myservo.position(-3.0);
Nestordp 0:182a12372dd4 86 wait_ms(500);
Nestordp 0:182a12372dd4 87 velocidade(0.38, 0.38);
Nestordp 0:182a12372dd4 88 frente();
Nestordp 0:182a12372dd4 89 }
Nestordp 0:182a12372dd4 90 else if(dist_dir < dist_esq){
Nestordp 0:182a12372dd4 91 printf("VIRA ESQUERDA \n");
Nestordp 0:182a12372dd4 92 esquerda();
Nestordp 0:182a12372dd4 93 velocidade(0.38, 0.38);
Nestordp 0:182a12372dd4 94 wait_ms(250);
Nestordp 0:182a12372dd4 95 velocidade(0.0, 0.0);
Nestordp 0:182a12372dd4 96 myservo.position(-3.0);
Nestordp 0:182a12372dd4 97 wait_ms(500);
Nestordp 0:182a12372dd4 98 velocidade(0.38, 0.38);
Nestordp 0:182a12372dd4 99 frente();
Nestordp 0:182a12372dd4 100 }
Nestordp 0:182a12372dd4 101 //myservo.position(-3.0);
Nestordp 0:182a12372dd4 102 //wait_ms(100);
Nestordp 0:182a12372dd4 103 }
Nestordp 0:182a12372dd4 104 }
Nestordp 0:182a12372dd4 105
Nestordp 0:182a12372dd4 106
Nestordp 0:182a12372dd4 107
Nestordp 0:182a12372dd4 108 /*while(1){
Nestordp 0:182a12372dd4 109 M1direct = 1;
Nestordp 0:182a12372dd4 110 M2direct = 1;
Nestordp 0:182a12372dd4 111 bip = 0;
Nestordp 0:182a12372dd4 112
Nestordp 0:182a12372dd4 113 for(float p=0; p<=1.0; p += 0.1){
Nestordp 0:182a12372dd4 114 myservo = p;
Nestordp 0:182a12372dd4 115 trig=1; //Inicio do trigger
Nestordp 0:182a12372dd4 116 wait_us(10); // 10us de pulso
Nestordp 0:182a12372dd4 117 trig=0; //Fim do trigger
Nestordp 0:182a12372dd4 118 echo.rise(&iniP); //leitura do inicio do pulso de retorno
Nestordp 0:182a12372dd4 119 echo.fall(&finP); //Leitura do final do pulso de retorno
Nestordp 0:182a12372dd4 120 printf("Distancia detectada pelo sensor %.2f cm \n",distcm);
Nestordp 0:182a12372dd4 121 wait(0.05);
Nestordp 0:182a12372dd4 122 }
Nestordp 0:182a12372dd4 123 wait(0.13);
Nestordp 0:182a12372dd4 124
Nestordp 0:182a12372dd4 125 M1direct = 1;
Nestordp 0:182a12372dd4 126 M2direct = 1;
Nestordp 0:182a12372dd4 127 bip = 0;
Nestordp 0:182a12372dd4 128
Nestordp 0:182a12372dd4 129 for(float p=1.0; p>=0.0; p -= 0.1) {
Nestordp 0:182a12372dd4 130 myservo = p;
Nestordp 0:182a12372dd4 131 trig=1; //Inicio do trigger
Nestordp 0:182a12372dd4 132 wait_us(10); // 10us de pulso
Nestordp 0:182a12372dd4 133 trig=0; //Fim do trigger
Nestordp 0:182a12372dd4 134 echo.rise(&iniP); //leitura do inicio do pulso de retorno
Nestordp 0:182a12372dd4 135 echo.fall(&finP); //Leitura do final do pulso de retorno
Nestordp 0:182a12372dd4 136 printf("Distancia detectada pelo sensor %.2f cm \n",distcm);
Nestordp 0:182a12372dd4 137 wait(0.05);
Nestordp 0:182a12372dd4 138 }
Nestordp 0:182a12372dd4 139 wait(0.13);
Nestordp 0:182a12372dd4 140 }*/
Nestordp 0:182a12372dd4 141 }
Nestordp 0:182a12372dd4 142
Nestordp 0:182a12372dd4 143 //Rotina para receber o pulso inicial do pino Echo
Nestordp 0:182a12372dd4 144 void iniP(void)
Nestordp 0:182a12372dd4 145 {
Nestordp 0:182a12372dd4 146 tempo.start(); //Rotina para iniciar o contador
Nestordp 0:182a12372dd4 147 return;
Nestordp 0:182a12372dd4 148 }
Nestordp 0:182a12372dd4 149 //Rotina para pegar o tempo final do pulso
Nestordp 0:182a12372dd4 150 void finP(void)
Nestordp 0:182a12372dd4 151 {
Nestordp 0:182a12372dd4 152 tdist = tempo.read_us(); //Leitura do tempo transcorrido
Nestordp 0:182a12372dd4 153 distcm = tdist/58; //Cálculo da distância detectada em "cm"
Nestordp 0:182a12372dd4 154 //distin = tdist/148; //Cálculo da distância detectada em "in"
Nestordp 0:182a12372dd4 155
Nestordp 0:182a12372dd4 156 tempo.stop(); //Paro o temporizador
Nestordp 0:182a12372dd4 157 tempo.reset(); //Reset para o próximo ciclo
Nestordp 0:182a12372dd4 158 return;
Nestordp 0:182a12372dd4 159 }
Nestordp 0:182a12372dd4 160
Nestordp 0:182a12372dd4 161 void lerSonar(void)
Nestordp 0:182a12372dd4 162 {
Nestordp 0:182a12372dd4 163 trig=1; //Inicio do trigger
Nestordp 0:182a12372dd4 164 wait_us(10); //10us de pulso
Nestordp 0:182a12372dd4 165 trig=0; //Fim do trigger
Nestordp 0:182a12372dd4 166 echo.rise(&iniP); //leitura do inicio do pulso de retorno
Nestordp 0:182a12372dd4 167 echo.fall(&finP); //Leitura do final do pulso de retorno
Nestordp 0:182a12372dd4 168 wait_ms(100);
Nestordp 0:182a12372dd4 169 return;
Nestordp 0:182a12372dd4 170 }