Robô móvel com FREDOM KL25Z
Dependencies: Servo mbed HC-SR04
Diff: main.cpp
- Revision:
- 2:2e759e231fb6
- Parent:
- 1:027b210e631d
- Child:
- 3:e5d8eb3e3951
diff -r 027b210e631d -r 2e759e231fb6 main.cpp --- a/main.cpp Thu Jan 29 00:35:29 2015 +0000 +++ b/main.cpp Thu Feb 05 20:06:06 2015 +0000 @@ -42,62 +42,79 @@ void iniP(void); void finP(void); -void lerSonarF(void); -void lerSonarD(void); -void lerSonarE(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; +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); -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.48, 0.48); + Led = 1; + + //printf("COMECA O PROGRAMA\n"); frente(); + velocidade(Vd, Ve); + wait_ms(500); while(1){ - lerSonarF(); - dist_fre = distcm; - printf("Distancia detectada pelo sensor Frente %.2f cm \n",dist_fre); + 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); - lerSonarE(); - dist_esq = distcm; - printf("Distancia detectada pelo sensor esquerdo %.2f cm \n",dist_esq); + dist_esq = lerSonarE(); + //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); + + dist_dir = lerSonarD(); + //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)){ - sonarFrente(); - } + 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(razDE > 1){ - velocidade(0.22, 0.48); + if(Ve > 0.38){ + Ve = 0.38; + } + if(Vd > 0.38){ + Vd = 0.38; } - else if(razDE < 1){ - velocidade(0.48, 0.22); - } + if(Ve < 0.18){ + Ve = 0.18; + } + if(Vd < 0.18){ + Vd = 0.18; + } + velocidade(Vd, Ve); } } -void lerSonarD(void) +float lerSonarD(void) { - trigD=1; //Inicio do trigger - wait_us(10); //10us de pulso - trigD=0; //Fim do trigger + trigD = 1; //Inicio do trigger + wait_us(10); //10us de pulso + trigD = 0; //Fim do trigger while(!echoD); tempo.start(); while(echoD); @@ -106,13 +123,14 @@ //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 lerSonarF(void) +float lerSonarF(void) { - trigF=1; //Inicio do trigger - wait_us(10); //10us de pulso - trigF=0; //Fim do trigger + trigF=1; //Inicio do trigger + wait_us(10); //10us de pulso + trigF=0; //Fim do trigger while(!echoF); tempo.start(); while(echoF); @@ -121,13 +139,14 @@ //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 lerSonarE(void) +float lerSonarE(void) { - trigE = 1; //Inicio do trigger - wait_us(10); //10us de pulso - trigE = 0; //Fim do trigger + trigE = 1; //Inicio do trigger + wait_us(10); //10us de pulso + trigE = 0; //Fim do trigger while(!echoE); tempo.start(); while(echoE); @@ -136,6 +155,7 @@ //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; }