Nestor Pereira-Neto / Mbed 2 deprecated CarrinhoLabirinto

Robô móvel com FREDOM KL25Z

Dependencies:   Servo mbed HC-SR04

main.cpp@2:2e759e231fb6, 2015-02-05 (annotated)

Committer:
Nestordp
Date:
Thu Feb 05 20:06:06 2015 +0000
Revision:
2:2e759e231fb6
Parent:
1:027b210e631d
Child:
3:e5d8eb3e3951
Ultima vers?o sem classe sonar

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 2:2e759e231fb6 45 float lerSonarF(void);
Nestordp 2:2e759e231fb6 46 float lerSonarD(void);
Nestordp 2:2e759e231fb6 47 float 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 2:2e759e231fb6 51 float dist_esq = 0, dist_dir = 0, dist_fre = 0, razDE = 0, Xmeio = 0, Ve = 0.3, Vd = 0.3, Vg = 0.23, Kp = 0.080, ed = 0, ee = 0;
Nestordp 1:027b210e631d 52 char flagTempo = 0;
Nestordp 2:2e759e231fb6 53
Nestordp 2:2e759e231fb6 54 int main() {
Nestordp 2:2e759e231fb6 55 //printf("INICIA\n");
Nestordp 2:2e759e231fb6 56 //myservo.calibrate(0.0013, 45.0);
Nestordp 0:182a12372dd4 57
Nestordp 2:2e759e231fb6 58 //
Nestordp 2:2e759e231fb6 59
Nestordp 0:182a12372dd4 60 wait_ms(3000);
Nestordp 0:182a12372dd4 61 myservo.position(-3.0);
Nestordp 2:2e759e231fb6 62 Led = 1;
Nestordp 2:2e759e231fb6 63
Nestordp 2:2e759e231fb6 64 //printf("COMECA O PROGRAMA\n");
Nestordp 0:182a12372dd4 65 frente();
Nestordp 2:2e759e231fb6 66 velocidade(Vd, Ve);
Nestordp 2:2e759e231fb6 67 wait_ms(500);
Nestordp 0:182a12372dd4 68
Nestordp 0:182a12372dd4 69 while(1){
Nestordp 2:2e759e231fb6 70 dist_fre = lerSonarF();
Nestordp 2:2e759e231fb6 71 if (dist_fre <= 20){
Nestordp 2:2e759e231fb6 72 velocidade(0.0, 0.0);
Nestordp 2:2e759e231fb6 73 }
Nestordp 2:2e759e231fb6 74 //sonarFrente();
Nestordp 2:2e759e231fb6 75 //printf("Distancia detectada pelo sensor Frente %.2f cm \n",dist_fre);
Nestordp 1:027b210e631d 76 //wait_ms(1000);
Nestordp 1:027b210e631d 77
Nestordp 2:2e759e231fb6 78 dist_esq = lerSonarE();
Nestordp 2:2e759e231fb6 79 //printf("Distancia detectada pelo sensor esquerdo %.2f cm \n",dist_esq);
Nestordp 1:027b210e631d 80 //wait_ms(1000);
Nestordp 2:2e759e231fb6 81
Nestordp 2:2e759e231fb6 82 dist_dir = lerSonarD();
Nestordp 2:2e759e231fb6 83 //printf("Distancia detectada pelo sensor direito %.2f cm \n",dist_dir);
Nestordp 1:027b210e631d 84 //wait_ms(1000);
Nestordp 1:027b210e631d 85
Nestordp 2:2e759e231fb6 86 Xmeio = (dist_dir + dist_esq) / 2; //Caucula o SetPoint
Nestordp 2:2e759e231fb6 87
Nestordp 2:2e759e231fb6 88 ed = Xmeio - dist_dir; //
Nestordp 2:2e759e231fb6 89 ee = Xmeio - dist_esq; //
Nestordp 2:2e759e231fb6 90
Nestordp 2:2e759e231fb6 91 Ve = Vg + (ee * Kp); //
Nestordp 2:2e759e231fb6 92 Vd = Vg + (ed * Kp); //
Nestordp 2:2e759e231fb6 93
Nestordp 2:2e759e231fb6 94 //Ve =(ee * Kp); //
Nestordp 2:2e759e231fb6 95 //Vd =(ed * Kp);
Nestordp 1:027b210e631d 96
Nestordp 2:2e759e231fb6 97 if(Ve > 0.38){
Nestordp 2:2e759e231fb6 98 Ve = 0.38;
Nestordp 2:2e759e231fb6 99 }
Nestordp 2:2e759e231fb6 100 if(Vd > 0.38){
Nestordp 2:2e759e231fb6 101 Vd = 0.38;
Nestordp 0:182a12372dd4 102 }
Nestordp 2:2e759e231fb6 103 if(Ve < 0.18){
Nestordp 2:2e759e231fb6 104 Ve = 0.18;
Nestordp 2:2e759e231fb6 105 }
Nestordp 2:2e759e231fb6 106 if(Vd < 0.18){
Nestordp 2:2e759e231fb6 107 Vd = 0.18;
Nestordp 2:2e759e231fb6 108 }
Nestordp 2:2e759e231fb6 109 velocidade(Vd, Ve);
Nestordp 0:182a12372dd4 110 }
Nestordp 1:027b210e631d 111 }
Nestordp 1:027b210e631d 112
Nestordp 2:2e759e231fb6 113 float lerSonarD(void)
Nestordp 1:027b210e631d 114 {
Nestordp 2:2e759e231fb6 115 trigD = 1; //Inicio do trigger
Nestordp 2:2e759e231fb6 116 wait_us(10); //10us de pulso
Nestordp 2:2e759e231fb6 117 trigD = 0; //Fim do trigger
Nestordp 1:027b210e631d 118 while(!echoD);
Nestordp 1:027b210e631d 119 tempo.start();
Nestordp 1:027b210e631d 120 while(echoD);
Nestordp 1:027b210e631d 121 tdist = tempo.read_us(); //Leitura do tempo transcorrido
Nestordp 1:027b210e631d 122 distcm = tdist/58; //Cálculo da distância detectada em "cm"
Nestordp 1:027b210e631d 123 //distin = tdist/148; //Cálculo da distância detectada em "in"
Nestordp 1:027b210e631d 124 tempo.stop(); //Paro o temporizador
Nestordp 1:027b210e631d 125 tempo.reset(); //Reset para o próximo ciclo
Nestordp 2:2e759e231fb6 126 return distcm;
Nestordp 1:027b210e631d 127 }
Nestordp 1:027b210e631d 128
Nestordp 2:2e759e231fb6 129 float lerSonarF(void)
Nestordp 1:027b210e631d 130 {
Nestordp 2:2e759e231fb6 131 trigF=1; //Inicio do trigger
Nestordp 2:2e759e231fb6 132 wait_us(10); //10us de pulso
Nestordp 2:2e759e231fb6 133 trigF=0; //Fim do trigger
Nestordp 1:027b210e631d 134 while(!echoF);
Nestordp 1:027b210e631d 135 tempo.start();
Nestordp 1:027b210e631d 136 while(echoF);
Nestordp 1:027b210e631d 137 tdist = tempo.read_us(); //Leitura do tempo transcorrido
Nestordp 1:027b210e631d 138 distcm = tdist/58; //Cálculo da distância detectada em "cm"
Nestordp 1:027b210e631d 139 //distin = tdist/148; //Cálculo da distância detectada em "in"
Nestordp 1:027b210e631d 140 tempo.stop(); //Paro o temporizador
Nestordp 1:027b210e631d 141 tempo.reset(); //Reset para o próximo ciclo
Nestordp 2:2e759e231fb6 142 return distcm;
Nestordp 0:182a12372dd4 143 }
Nestordp 0:182a12372dd4 144
Nestordp 2:2e759e231fb6 145 float lerSonarE(void)
Nestordp 1:027b210e631d 146 {
Nestordp 2:2e759e231fb6 147 trigE = 1; //Inicio do trigger
Nestordp 2:2e759e231fb6 148 wait_us(10); //10us de pulso
Nestordp 2:2e759e231fb6 149 trigE = 0; //Fim do trigger
Nestordp 1:027b210e631d 150 while(!echoE);
Nestordp 1:027b210e631d 151 tempo.start();
Nestordp 1:027b210e631d 152 while(echoE);
Nestordp 0:182a12372dd4 153 tdist = tempo.read_us(); //Leitura do tempo transcorrido
Nestordp 0:182a12372dd4 154 distcm = tdist/58; //Cálculo da distância detectada em "cm"
Nestordp 1:027b210e631d 155 //distin = tdist/148; //Cálculo da distância detectada em "in"
Nestordp 0:182a12372dd4 156 tempo.stop(); //Paro o temporizador
Nestordp 0:182a12372dd4 157 tempo.reset(); //Reset para o próximo ciclo
Nestordp 2:2e759e231fb6 158 return distcm;
Nestordp 0:182a12372dd4 159 }
Nestordp 0:182a12372dd4 160
Nestordp 1:027b210e631d 161
Nestordp 1:027b210e631d 162 void sonarFrente(void){
Nestordp 1:027b210e631d 163 bip = 1;
Nestordp 1:027b210e631d 164 printf("PARA\n");
Nestordp 1:027b210e631d 165 re();
Nestordp 0:182a12372dd4 166 wait_ms(100);
Nestordp 1:027b210e631d 167 velocidade(0.0, 0.0);
Nestordp 1:027b210e631d 168 bip = 0;
Nestordp 1:027b210e631d 169
Nestordp 1:027b210e631d 170 myservo.position(-33.0);
Nestordp 1:027b210e631d 171 wait_ms(1000);
Nestordp 1:027b210e631d 172 lerSonarF();
Nestordp 1:027b210e631d 173 printf("Distancia direita %.2f cm \n",distcm);
Nestordp 1:027b210e631d 174 dist_dir = distcm;
Nestordp 1:027b210e631d 175
Nestordp 1:027b210e631d 176 myservo.position(30.8);
Nestordp 1:027b210e631d 177 wait_ms(1000);
Nestordp 1:027b210e631d 178 lerSonarF();
Nestordp 1:027b210e631d 179 printf("Distancia esquerda %.2f cm \n",distcm);
Nestordp 1:027b210e631d 180 dist_esq = distcm;
Nestordp 1:027b210e631d 181
Nestordp 1:027b210e631d 182 if(dist_dir > dist_esq){
Nestordp 1:027b210e631d 183 printf("VIRA ESQUERDA \n");
Nestordp 1:027b210e631d 184 direita();
Nestordp 1:027b210e631d 185 velocidade(0.48, 0.48);
Nestordp 1:027b210e631d 186 wait_ms(250);
Nestordp 1:027b210e631d 187 velocidade(0.0, 0.0);
Nestordp 1:027b210e631d 188 myservo.position(-3.0);
Nestordp 1:027b210e631d 189 wait_ms(500);
Nestordp 1:027b210e631d 190 velocidade(0.48, 0.48);
Nestordp 1:027b210e631d 191 frente();
Nestordp 1:027b210e631d 192 }
Nestordp 1:027b210e631d 193 else if(dist_dir < dist_esq){
Nestordp 1:027b210e631d 194 printf("VIRA ESQUERDA \n");
Nestordp 1:027b210e631d 195 esquerda();
Nestordp 1:027b210e631d 196 velocidade(0.48, 0.48);
Nestordp 1:027b210e631d 197 wait_ms(250);
Nestordp 1:027b210e631d 198 velocidade(0.0, 0.0);
Nestordp 1:027b210e631d 199 myservo.position(-3.0);
Nestordp 1:027b210e631d 200 wait_ms(500);
Nestordp 1:027b210e631d 201 velocidade(0.48, 0.48);
Nestordp 1:027b210e631d 202 frente();
Nestordp 1:027b210e631d 203 }
Nestordp 1:027b210e631d 204 }