![](/media/cache/profiles/603ec80e671c74a8641d332712c458e8.jpg.50x50_q85.jpg)
Robô móvel com FREDOM KL25Z
Dependencies: Servo mbed HC-SR04
main.cpp
- Committer:
- Nestordp
- Date:
- 2015-02-05
- Revision:
- 2:2e759e231fb6
- Parent:
- 1:027b210e631d
- Child:
- 3:e5d8eb3e3951
File content as of revision 2:2e759e231fb6:
#include "mbed.h" #include "Servo.h" #define velocidade(a, b) MDPWM = a; MEPWM = b #define frente() MDdirect = 1; MEdirect = 1 #define re() MDdirect = 0; MEdirect = 0 #define esquerda() MDdirect = 1; MEdirect = 0 #define direita() MDdirect = 0; MEdirect = 1 //Sonar Frente //DigitalOut trig(PTB2,0); //Configuração do pino de Trigger //InterruptIn echo(D12); //Configuração da interrupção por pino de Echo DigitalOut bip(D2); // Servo myservo(D3); // DigitalOut MEdirect(D4); //Motor 2 Direction control PwmOut MEPWM(D5); //Motor 2 PWM control PwmOut MDPWM(D6); //Motor 1 PWM control DigitalOut MDdirect(D7); //Motor 1 Direction control DigitalOut trigF(A2,0); //Configuração do pino de Trigger DigitalIn echoF(D12); //Configuração da interrupção por pino de Echo //Sonar Direito DigitalOut trigD(A1, 0); //Configuração do pino de Trigger DigitalIn echoD(D10); //Configuração da interrupção por pino de Echo //Sonar Esqerdo DigitalOut trigE(A0,0); //Configuração do pino de Trigger InterruptIn echoE(D11); //Configuração da interrupção por pino de Echo DigitalOut Led(LED1); Serial pc(USBTX,USBRX); //Configuração da comunicação serial para enviar o valor do sensor Timer tempo; //Para usar rotinas de tempo void iniP(void); void finP(void); float lerSonarF(void); float lerSonarD(void); float lerSonarE(void); void sonarFrente(void); float tdist=0, distcm=0, distin=0, dist0=0; 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; char flagTempo = 0; int main() { //printf("INICIA\n"); //myservo.calibrate(0.0013, 45.0); // wait_ms(3000); myservo.position(-3.0); Led = 1; //printf("COMECA O PROGRAMA\n"); frente(); velocidade(Vd, Ve); wait_ms(500); while(1){ dist_fre = lerSonarF(); if (dist_fre <= 20){ velocidade(0.0, 0.0); } //sonarFrente(); //printf("Distancia detectada pelo sensor Frente %.2f cm \n",dist_fre); //wait_ms(1000); dist_esq = lerSonarE(); //printf("Distancia detectada pelo sensor esquerdo %.2f cm \n",dist_esq); //wait_ms(1000); dist_dir = lerSonarD(); //printf("Distancia detectada pelo sensor direito %.2f cm \n",dist_dir); //wait_ms(1000); Xmeio = (dist_dir + dist_esq) / 2; //Caucula o SetPoint ed = Xmeio - dist_dir; // ee = Xmeio - dist_esq; // Ve = Vg + (ee * Kp); // Vd = Vg + (ed * Kp); // //Ve =(ee * Kp); // //Vd =(ed * Kp); if(Ve > 0.38){ Ve = 0.38; } if(Vd > 0.38){ Vd = 0.38; } if(Ve < 0.18){ Ve = 0.18; } if(Vd < 0.18){ Vd = 0.18; } velocidade(Vd, Ve); } } float lerSonarD(void) { trigD = 1; //Inicio do trigger wait_us(10); //10us de pulso trigD = 0; //Fim do trigger while(!echoD); tempo.start(); while(echoD); tdist = tempo.read_us(); //Leitura do tempo transcorrido distcm = tdist/58; //Cálculo da distância detectada em "cm" //distin = tdist/148; //Cálculo da distância detectada em "in" tempo.stop(); //Paro o temporizador tempo.reset(); //Reset para o próximo ciclo return distcm; } float lerSonarF(void) { trigF=1; //Inicio do trigger wait_us(10); //10us de pulso trigF=0; //Fim do trigger while(!echoF); tempo.start(); while(echoF); tdist = tempo.read_us(); //Leitura do tempo transcorrido distcm = tdist/58; //Cálculo da distância detectada em "cm" //distin = tdist/148; //Cálculo da distância detectada em "in" tempo.stop(); //Paro o temporizador tempo.reset(); //Reset para o próximo ciclo return distcm; } float lerSonarE(void) { trigE = 1; //Inicio do trigger wait_us(10); //10us de pulso trigE = 0; //Fim do trigger while(!echoE); tempo.start(); while(echoE); tdist = tempo.read_us(); //Leitura do tempo transcorrido distcm = tdist/58; //Cálculo da distância detectada em "cm" //distin = tdist/148; //Cálculo da distância detectada em "in" tempo.stop(); //Paro o temporizador tempo.reset(); //Reset para o próximo ciclo return distcm; } void sonarFrente(void){ bip = 1; printf("PARA\n"); re(); wait_ms(100); velocidade(0.0, 0.0); bip = 0; myservo.position(-33.0); wait_ms(1000); lerSonarF(); printf("Distancia direita %.2f cm \n",distcm); dist_dir = distcm; myservo.position(30.8); wait_ms(1000); lerSonarF(); printf("Distancia esquerda %.2f cm \n",distcm); dist_esq = distcm; if(dist_dir > dist_esq){ printf("VIRA ESQUERDA \n"); direita(); velocidade(0.48, 0.48); wait_ms(250); velocidade(0.0, 0.0); myservo.position(-3.0); wait_ms(500); velocidade(0.48, 0.48); frente(); } else if(dist_dir < dist_esq){ printf("VIRA ESQUERDA \n"); esquerda(); velocidade(0.48, 0.48); wait_ms(250); velocidade(0.0, 0.0); myservo.position(-3.0); wait_ms(500); velocidade(0.48, 0.48); frente(); } }