![](/media/cache/profiles/603ec80e671c74a8641d332712c458e8.jpg.50x50_q85.jpg)
Robô móvel com FREDOM KL25Z
Dependencies: Servo mbed HC-SR04
Diff: main.cpp
- Revision:
- 1:027b210e631d
- Parent:
- 0:182a12372dd4
- Child:
- 2:2e759e231fb6
--- a/main.cpp Tue Jan 27 21:57:22 2015 +0000 +++ b/main.cpp Thu Jan 29 00:35:29 2015 +0000 @@ -10,9 +10,7 @@ //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 + @@ -22,9 +20,19 @@ 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 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 @@ -34,10 +42,14 @@ void iniP(void); void finP(void); -void lerSonar(void); +void lerSonarF(void); +void lerSonarD(void); +void lerSonarE(void); +void sonarFrente(void); float tdist=0, distcm=0, distin=0, dist0=0; -float dist_esq = 0, dist_dir = 0, dist_fre = 0; +float dist_esq = 0, dist_dir = 0, dist_fre = 0, razDE = 0; +char flagTempo = 0; int main() { printf("INICIA\n"); @@ -48,123 +60,125 @@ myservo.position(-3.0); Led = 1; printf("COMECA O PROGRAMA\n"); - velocidade(0.38, 0.38); + velocidade(0.48, 0.48); frente(); while(1){ - lerSonar(); - printf("Distancia detectada pelo sensor %.2f cm \n",distcm); + lerSonarF(); dist_fre = distcm; + printf("Distancia detectada pelo sensor Frente %.2f cm \n",dist_fre); + //wait_ms(1000); + + lerSonarE(); + dist_esq = distcm; + printf("Distancia detectada pelo sensor esquerdo %.2f cm \n",dist_esq); + //wait_ms(1000); + lerSonarD(); + dist_dir = distcm; + printf("Distancia detectada pelo sensor direito %.2f cm \n",dist_dir); + //wait_ms(1000); + + razDE = dist_dir / dist_esq; 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); + sonarFrente(); + } + + if(razDE > 1){ + velocidade(0.22, 0.48); } + else if(razDE < 1){ + velocidade(0.48, 0.22); + } } - - - - /*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); - }*/ +} + +void 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 +} + +void 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 } -//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) -{ +void 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" - + //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 + +void sonarFrente(void){ + bip = 1; + printf("PARA\n"); + re(); wait_ms(100); - return; -} + 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(); + } +} \ No newline at end of file