mitsuo ozawa
/
programa_carro
seguidor de linea
Diff: main.cpp
- Revision:
- 0:3e68940a3405
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Dec 03 23:35:13 2014 +0000 @@ -0,0 +1,76 @@ +#include "mbed.h" +Serial pc (USBTX,USBRX); +DigitalOut myled1(LED1); +DigitalOut myled2(LED2); +DigitalOut myled3(LED3); +PwmOut motor1(PTD4); /* apagado - 0.0 + prendido - 1.0 (porcentaje de potencia)*/ +AnalogIn izquierda(PTB0);// 0 - 0 volts +AnalogIn derecha(PTB1); //1.0 - 3.3 volts + +PwmOut servo(PTA1); //600 - izquierda + //1400 - recto + //2200 - derecha + +int main(){ + float foto1; + float foto2; + int ultima=0; + float rango_foto1= .15; + float rango_foto2=.2; + float sev=1400; + servo.period_ms(20); + float motor; + while(1){ + motor=motor1; + foto1=izquierda; + foto2=derecha; + pc.printf("foto1 = %f\n\r",foto1 ); + pc.printf("foto2 = %f\n\r", foto2); + pc.printf("motor = %f\n\r", motor); + pc.printf("servo = %f\n\r", sev); + if(foto1<rango_foto1 and foto2<rango_foto2 ){ //linea recta + motor1=.40; + pc.printf("recto \n\n"); + + } + else if(foto1>rango_foto1 and foto2<rango_foto2 ){//derecha + motor1=0.175; + if(sev<1900){ + sev=sev+36; + } + else if (sev>=1900) + sev=1900; + servo.pulsewidth_us(sev); + ultima=1; + pc.printf("derecha \n"); + } + else if(foto2>rango_foto2 and foto1<rango_foto1 ){//izquierda + motor1=0.175; + if(sev>900){ + sev=sev-60; + } + else if (sev<=900) + sev=900; + servo.pulsewidth_us(sev); + ultima=2; + pc.printf("izquierda \n" ); + } + else if(foto2>rango_foto2 and foto1>rango_foto1){ + if(ultima==2){ + if(sev>900) + sev=sev-70; + else if (sev<=900) + sev=900; + servo.pulsewidth_us(sev);} + else if(ultima==1){ + if(sev<1900) + sev=sev+70; + else if (sev>=1900) + sev=1900; + servo.pulsewidth_us(sev); + } + wait(.0001); + } + } + } \ No newline at end of file