Nestor Pereira-Neto
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