seguidor de linea

Dependencies:   mbed

main.cpp

Committer:
robert996
Date:
2014-12-03
Revision:
0:3e68940a3405

File content as of revision 0:3e68940a3405:

#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);
         }
    }
    }