Nestor Pereira-Neto / Mbed 2 deprecated CarrinhoLabirinto

Robô móvel com FREDOM KL25Z

Dependencies:   Servo mbed HC-SR04

main.cpp@3:e5d8eb3e3951, 2015-02-05 (annotated)

Committer:
Nestordp
Date:
Thu Feb 05 23:38:30 2015 +0000
Revision:
3:e5d8eb3e3951
Parent:
2:2e759e231fb6 
carrinho labirinto;

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 3:e5d8eb3e3951 3 #include "HCSR04.h"
Nestordp 0:182a12372dd4 4
Nestordp 0:182a12372dd4 5 #define velocidade(a, b) MDPWM = a; MEPWM = b
Nestordp 3:e5d8eb3e3951 6 #define sentido(a, b) MDdirect = a; MEdirect = b
Nestordp 3:e5d8eb3e3951 7
Nestordp 0:182a12372dd4 8
Nestordp 0:182a12372dd4 9 //Sonar Frente
Nestordp 0:182a12372dd4 10 //DigitalOut trig(PTB2,0); //Configuração do pino de Trigger
Nestordp 0:182a12372dd4 11 //InterruptIn echo(D12); //Configuração da interrupção por pino de Echo
Nestordp 1:027b210e631d 12
Nestordp 0:182a12372dd4 13
Nestordp 0:182a12372dd4 14
Nestordp 0:182a12372dd4 15
Nestordp 0:182a12372dd4 16 DigitalOut bip(D2); //
Nestordp 3:e5d8eb3e3951 17 Servo myservo(PTB0); //
Nestordp 3:e5d8eb3e3951 18
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 3:e5d8eb3e3951 24 HCSR04 sonarD(D9, D8);
Nestordp 3:e5d8eb3e3951 25 HCSR04 sonarE(D11, D10);
Nestordp 3:e5d8eb3e3951 26 HCSR04 sonarF(D13, D12);
Nestordp 1:027b210e631d 27
Nestordp 3:e5d8eb3e3951 28 DigitalOut Led(LED2);
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
Nestordp 1:027b210e631d 32 void sonarFrente(void);
Nestordp 0:182a12372dd4 33
Nestordp 3:e5d8eb3e3951 34 float dist_esq = 0, dist_dir = 0, dist_fre = 0, razDE = 0, Xmeio = 0, Ve = 0.3, Vd = 0.3, Vg = 0.0, Kp = 0.050, ed = 0, ee = 0;
Nestordp 3:e5d8eb3e3951 35
Nestordp 2:2e759e231fb6 36
Nestordp 2:2e759e231fb6 37 int main() {
Nestordp 3:e5d8eb3e3951 38 printf("INICIA\n");
Nestordp 3:e5d8eb3e3951 39 myservo.calibrate(0.0013, 45.0);
Nestordp 3:e5d8eb3e3951 40
Nestordp 0:182a12372dd4 41 wait_ms(3000);
Nestordp 0:182a12372dd4 42 myservo.position(-3.0);
Nestordp 2:2e759e231fb6 43 Led = 1;
Nestordp 2:2e759e231fb6 44
Nestordp 2:2e759e231fb6 45 //printf("COMECA O PROGRAMA\n");
Nestordp 0:182a12372dd4 46 frente();
Nestordp 2:2e759e231fb6 47 velocidade(Vd, Ve);
Nestordp 2:2e759e231fb6 48 wait_ms(500);
Nestordp 0:182a12372dd4 49
Nestordp 0:182a12372dd4 50 while(1){
Nestordp 3:e5d8eb3e3951 51 dist_fre = sonarF.getCm();
Nestordp 3:e5d8eb3e3951 52 /*if (dist_fre <= 20){
Nestordp 2:2e759e231fb6 53 velocidade(0.0, 0.0);
Nestordp 3:e5d8eb3e3951 54 }*/
Nestordp 2:2e759e231fb6 55 //printf("Distancia detectada pelo sensor Frente %.2f cm \n",dist_fre);
Nestordp 1:027b210e631d 56 //wait_ms(1000);
Nestordp 1:027b210e631d 57
Nestordp 3:e5d8eb3e3951 58 dist_esq = sonarE.getCm();
Nestordp 2:2e759e231fb6 59 //printf("Distancia detectada pelo sensor esquerdo %.2f cm \n",dist_esq);
Nestordp 1:027b210e631d 60 //wait_ms(1000);
Nestordp 2:2e759e231fb6 61
Nestordp 3:e5d8eb3e3951 62 dist_dir = sonarD.getCm();
Nestordp 2:2e759e231fb6 63 //printf("Distancia detectada pelo sensor direito %.2f cm \n",dist_dir);
Nestordp 1:027b210e631d 64 //wait_ms(1000);
Nestordp 1:027b210e631d 65
Nestordp 2:2e759e231fb6 66 Xmeio = (dist_dir + dist_esq) / 2; //Caucula o SetPoint
Nestordp 2:2e759e231fb6 67
Nestordp 2:2e759e231fb6 68 ed = Xmeio - dist_dir; //
Nestordp 2:2e759e231fb6 69 ee = Xmeio - dist_esq; //
Nestordp 2:2e759e231fb6 70
Nestordp 2:2e759e231fb6 71 Ve = Vg + (ee * Kp); //
Nestordp 2:2e759e231fb6 72 Vd = Vg + (ed * Kp); //
Nestordp 2:2e759e231fb6 73
Nestordp 2:2e759e231fb6 74 //Ve =(ee * Kp); //
Nestordp 2:2e759e231fb6 75 //Vd =(ed * Kp);
Nestordp 1:027b210e631d 76
Nestordp 3:e5d8eb3e3951 77
Nestordp 3:e5d8eb3e3951 78
Nestordp 3:e5d8eb3e3951 79 if(Ve > 0.31){
Nestordp 3:e5d8eb3e3951 80 Ve = 0.31;
Nestordp 2:2e759e231fb6 81 }
Nestordp 3:e5d8eb3e3951 82 if(Vd > 0.31){
Nestordp 3:e5d8eb3e3951 83 Vd = 0.31;
Nestordp 0:182a12372dd4 84 }
Nestordp 2:2e759e231fb6 85 if(Ve < 0.18){
Nestordp 2:2e759e231fb6 86 Ve = 0.18;
Nestordp 2:2e759e231fb6 87 }
Nestordp 2:2e759e231fb6 88 if(Vd < 0.18){
Nestordp 2:2e759e231fb6 89 Vd = 0.18;
Nestordp 2:2e759e231fb6 90 }
Nestordp 2:2e759e231fb6 91 velocidade(Vd, Ve);
Nestordp 0:182a12372dd4 92 }
Nestordp 1:027b210e631d 93 }
Nestordp 1:027b210e631d 94
Nestordp 0:182a12372dd4 95
Nestordp 1:027b210e631d 96
Nestordp 3:e5d8eb3e3951 97 //******************************************************************************
Nestordp 3:e5d8eb3e3951 98 /*void sonarFrente(void){
Nestordp 1:027b210e631d 99 bip = 1;
Nestordp 1:027b210e631d 100 printf("PARA\n");
Nestordp 1:027b210e631d 101 re();
Nestordp 0:182a12372dd4 102 wait_ms(100);
Nestordp 1:027b210e631d 103 velocidade(0.0, 0.0);
Nestordp 1:027b210e631d 104 bip = 0;
Nestordp 1:027b210e631d 105
Nestordp 1:027b210e631d 106 myservo.position(-33.0);
Nestordp 1:027b210e631d 107 wait_ms(1000);
Nestordp 1:027b210e631d 108 lerSonarF();
Nestordp 1:027b210e631d 109 printf("Distancia direita %.2f cm \n",distcm);
Nestordp 1:027b210e631d 110 dist_dir = distcm;
Nestordp 1:027b210e631d 111
Nestordp 1:027b210e631d 112 myservo.position(30.8);
Nestordp 1:027b210e631d 113 wait_ms(1000);
Nestordp 1:027b210e631d 114 lerSonarF();
Nestordp 1:027b210e631d 115 printf("Distancia esquerda %.2f cm \n",distcm);
Nestordp 1:027b210e631d 116 dist_esq = distcm;
Nestordp 1:027b210e631d 117
Nestordp 1:027b210e631d 118 if(dist_dir > dist_esq){
Nestordp 1:027b210e631d 119 printf("VIRA ESQUERDA \n");
Nestordp 1:027b210e631d 120 direita();
Nestordp 1:027b210e631d 121 velocidade(0.48, 0.48);
Nestordp 1:027b210e631d 122 wait_ms(250);
Nestordp 1:027b210e631d 123 velocidade(0.0, 0.0);
Nestordp 1:027b210e631d 124 myservo.position(-3.0);
Nestordp 1:027b210e631d 125 wait_ms(500);
Nestordp 1:027b210e631d 126 velocidade(0.48, 0.48);
Nestordp 1:027b210e631d 127 frente();
Nestordp 1:027b210e631d 128 }
Nestordp 1:027b210e631d 129 else if(dist_dir < dist_esq){
Nestordp 1:027b210e631d 130 printf("VIRA ESQUERDA \n");
Nestordp 1:027b210e631d 131 esquerda();
Nestordp 1:027b210e631d 132 velocidade(0.48, 0.48);
Nestordp 1:027b210e631d 133 wait_ms(250);
Nestordp 1:027b210e631d 134 velocidade(0.0, 0.0);
Nestordp 1:027b210e631d 135 myservo.position(-3.0);
Nestordp 1:027b210e631d 136 wait_ms(500);
Nestordp 1:027b210e631d 137 velocidade(0.48, 0.48);
Nestordp 1:027b210e631d 138 frente();
Nestordp 1:027b210e631d 139 }
Nestordp 3:e5d8eb3e3951 140 }*/