![](/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-01-27
- Revision:
- 0:182a12372dd4
- Child:
- 1:027b210e631d
File content as of revision 0:182a12372dd4:
#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 //Sonar Esquerdo //DigitalOut trig(A0,0); //Configuração do pino de Trigger //InterruptIn echo(D11); //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 //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 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); void lerSonar(void); float tdist=0, distcm=0, distin=0, dist0=0; float dist_esq = 0, dist_dir = 0, dist_fre = 0; int main() { printf("INICIA\n"); myservo.calibrate(0.0013, 45.0); wait_ms(3000); //myservo.position(-33.0); todo pra direita //myservo.position(30.8); todo pra esquerda myservo.position(-3.0); Led = 1; printf("COMECA O PROGRAMA\n"); velocidade(0.38, 0.38); frente(); while(1){ lerSonar(); printf("Distancia detectada pelo sensor %.2f cm \n",distcm); dist_fre = distcm; if((dist_fre <= 30) && (dist_fre > 3)){ bip = 1; printf("PARA\n"); re(); wait_ms(100); velocidade(0.0, 0.0); bip = 0; myservo.position(-33.0); wait_ms(1000); lerSonar(); printf("Distancia direita %.2f cm \n",distcm); dist_dir = distcm; myservo.position(30.8); wait_ms(1000); lerSonar(); printf("Distancia esquerda %.2f cm \n",distcm); dist_esq = distcm; if(dist_dir > dist_esq){ printf("VIRA ESQUERDA \n"); direita(); velocidade(0.38, 0.38); wait_ms(250); velocidade(0.0, 0.0); myservo.position(-3.0); wait_ms(500); velocidade(0.38, 0.38); frente(); } else if(dist_dir < dist_esq){ printf("VIRA ESQUERDA \n"); esquerda(); velocidade(0.38, 0.38); wait_ms(250); velocidade(0.0, 0.0); myservo.position(-3.0); wait_ms(500); velocidade(0.38, 0.38); frente(); } //myservo.position(-3.0); //wait_ms(100); } } /*while(1){ M1direct = 1; M2direct = 1; bip = 0; for(float p=0; p<=1.0; p += 0.1){ myservo = p; trig=1; //Inicio do trigger wait_us(10); // 10us de pulso trig=0; //Fim do trigger echo.rise(&iniP); //leitura do inicio do pulso de retorno echo.fall(&finP); //Leitura do final do pulso de retorno printf("Distancia detectada pelo sensor %.2f cm \n",distcm); wait(0.05); } wait(0.13); M1direct = 1; M2direct = 1; bip = 0; for(float p=1.0; p>=0.0; p -= 0.1) { myservo = p; trig=1; //Inicio do trigger wait_us(10); // 10us de pulso trig=0; //Fim do trigger echo.rise(&iniP); //leitura do inicio do pulso de retorno echo.fall(&finP); //Leitura do final do pulso de retorno printf("Distancia detectada pelo sensor %.2f cm \n",distcm); wait(0.05); } wait(0.13); }*/ } //Rotina para receber o pulso inicial do pino Echo void iniP(void) { tempo.start(); //Rotina para iniciar o contador return; } //Rotina para pegar o tempo final do pulso void finP(void) { 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; } void lerSonar(void) { trig=1; //Inicio do trigger wait_us(10); //10us de pulso trig=0; //Fim do trigger echo.rise(&iniP); //leitura do inicio do pulso de retorno echo.fall(&finP); //Leitura do final do pulso de retorno wait_ms(100); return; }