main.cpp

00001 #include "mbed.h"
00002 Serial pc (USBTX,USBRX);
00003 DigitalOut myled1(LED1);
00004 DigitalOut myled2(LED2);
00005 DigitalOut myled3(LED3);
00006 PwmOut motor1(PTD4); /* apagado - 0.0
00007                        prendido - 1.0 (porcentaje de potencia)*/
00008 AnalogIn izquierda(PTB0);// 0 - 0 volts
00009 AnalogIn derecha(PTB1); //1.0 - 3.3 volts
00010 
00011 PwmOut servo(PTA1); //600 - izquierda
00012                     //1400 - recto
00013                     //2200 - derecha
00014 
00015 int main(){
00016     float foto1;
00017     float foto2;
00018     int ultima=0;
00019     float rango_foto1= .15;
00020     float rango_foto2=.2;
00021     float sev=1400;
00022     servo.period_ms(20);
00023     float motor;
00024     while(1){
00025         motor=motor1;
00026         foto1=izquierda;
00027         foto2=derecha;
00028         pc.printf("foto1 = %f\n\r",foto1 );
00029         pc.printf("foto2 = %f\n\r", foto2);
00030         pc.printf("motor = %f\n\r", motor);
00031         pc.printf("servo = %f\n\r", sev);
00032         if(foto1<rango_foto1 and foto2<rango_foto2 ){ //linea recta
00033             motor1=.40;
00034             pc.printf("recto \n\n");
00035             
00036             }
00037         else if(foto1>rango_foto1 and foto2<rango_foto2 ){//derecha
00038             motor1=0.175;
00039             if(sev<1900){
00040                 sev=sev+36;
00041                 }
00042             else if (sev>=1900)
00043                 sev=1900;
00044             servo.pulsewidth_us(sev);
00045             ultima=1;
00046             pc.printf("derecha \n");
00047             }
00048         else if(foto2>rango_foto2 and foto1<rango_foto1 ){//izquierda
00049             motor1=0.175;
00050             if(sev>900){
00051                 sev=sev-60;
00052               }
00053             else if (sev<=900)
00054                 sev=900;
00055             servo.pulsewidth_us(sev);
00056             ultima=2;
00057             pc.printf("izquierda \n" );
00058             }
00059         else if(foto2>rango_foto2 and foto1>rango_foto1){
00060             if(ultima==2){
00061                 if(sev>900)
00062                     sev=sev-70;
00063                 else if (sev<=900)
00064                      sev=900;
00065                 servo.pulsewidth_us(sev);}
00066             else if(ultima==1){
00067                 if(sev<1900)
00068                     sev=sev+70;
00069                 else if (sev>=1900)
00070                      sev=1900;
00071                 servo.pulsewidth_us(sev);
00072             }
00073         wait(.0001);
00074          }
00075     }
00076     }