mitsuo ozawa
/
programa_carro
seguidor de linea
main.cpp@0:3e68940a3405, 2014-12-03 (annotated)
- Committer:
- robert996
- Date:
- Wed Dec 03 23:35:13 2014 +0000
- Revision:
- 0:3e68940a3405
carrito seguidor de linea;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
robert996 | 0:3e68940a3405 | 1 | #include "mbed.h" |
robert996 | 0:3e68940a3405 | 2 | Serial pc (USBTX,USBRX); |
robert996 | 0:3e68940a3405 | 3 | DigitalOut myled1(LED1); |
robert996 | 0:3e68940a3405 | 4 | DigitalOut myled2(LED2); |
robert996 | 0:3e68940a3405 | 5 | DigitalOut myled3(LED3); |
robert996 | 0:3e68940a3405 | 6 | PwmOut motor1(PTD4); /* apagado - 0.0 |
robert996 | 0:3e68940a3405 | 7 | prendido - 1.0 (porcentaje de potencia)*/ |
robert996 | 0:3e68940a3405 | 8 | AnalogIn izquierda(PTB0);// 0 - 0 volts |
robert996 | 0:3e68940a3405 | 9 | AnalogIn derecha(PTB1); //1.0 - 3.3 volts |
robert996 | 0:3e68940a3405 | 10 | |
robert996 | 0:3e68940a3405 | 11 | PwmOut servo(PTA1); //600 - izquierda |
robert996 | 0:3e68940a3405 | 12 | //1400 - recto |
robert996 | 0:3e68940a3405 | 13 | //2200 - derecha |
robert996 | 0:3e68940a3405 | 14 | |
robert996 | 0:3e68940a3405 | 15 | int main(){ |
robert996 | 0:3e68940a3405 | 16 | float foto1; |
robert996 | 0:3e68940a3405 | 17 | float foto2; |
robert996 | 0:3e68940a3405 | 18 | int ultima=0; |
robert996 | 0:3e68940a3405 | 19 | float rango_foto1= .15; |
robert996 | 0:3e68940a3405 | 20 | float rango_foto2=.2; |
robert996 | 0:3e68940a3405 | 21 | float sev=1400; |
robert996 | 0:3e68940a3405 | 22 | servo.period_ms(20); |
robert996 | 0:3e68940a3405 | 23 | float motor; |
robert996 | 0:3e68940a3405 | 24 | while(1){ |
robert996 | 0:3e68940a3405 | 25 | motor=motor1; |
robert996 | 0:3e68940a3405 | 26 | foto1=izquierda; |
robert996 | 0:3e68940a3405 | 27 | foto2=derecha; |
robert996 | 0:3e68940a3405 | 28 | pc.printf("foto1 = %f\n\r",foto1 ); |
robert996 | 0:3e68940a3405 | 29 | pc.printf("foto2 = %f\n\r", foto2); |
robert996 | 0:3e68940a3405 | 30 | pc.printf("motor = %f\n\r", motor); |
robert996 | 0:3e68940a3405 | 31 | pc.printf("servo = %f\n\r", sev); |
robert996 | 0:3e68940a3405 | 32 | if(foto1<rango_foto1 and foto2<rango_foto2 ){ //linea recta |
robert996 | 0:3e68940a3405 | 33 | motor1=.40; |
robert996 | 0:3e68940a3405 | 34 | pc.printf("recto \n\n"); |
robert996 | 0:3e68940a3405 | 35 | |
robert996 | 0:3e68940a3405 | 36 | } |
robert996 | 0:3e68940a3405 | 37 | else if(foto1>rango_foto1 and foto2<rango_foto2 ){//derecha |
robert996 | 0:3e68940a3405 | 38 | motor1=0.175; |
robert996 | 0:3e68940a3405 | 39 | if(sev<1900){ |
robert996 | 0:3e68940a3405 | 40 | sev=sev+36; |
robert996 | 0:3e68940a3405 | 41 | } |
robert996 | 0:3e68940a3405 | 42 | else if (sev>=1900) |
robert996 | 0:3e68940a3405 | 43 | sev=1900; |
robert996 | 0:3e68940a3405 | 44 | servo.pulsewidth_us(sev); |
robert996 | 0:3e68940a3405 | 45 | ultima=1; |
robert996 | 0:3e68940a3405 | 46 | pc.printf("derecha \n"); |
robert996 | 0:3e68940a3405 | 47 | } |
robert996 | 0:3e68940a3405 | 48 | else if(foto2>rango_foto2 and foto1<rango_foto1 ){//izquierda |
robert996 | 0:3e68940a3405 | 49 | motor1=0.175; |
robert996 | 0:3e68940a3405 | 50 | if(sev>900){ |
robert996 | 0:3e68940a3405 | 51 | sev=sev-60; |
robert996 | 0:3e68940a3405 | 52 | } |
robert996 | 0:3e68940a3405 | 53 | else if (sev<=900) |
robert996 | 0:3e68940a3405 | 54 | sev=900; |
robert996 | 0:3e68940a3405 | 55 | servo.pulsewidth_us(sev); |
robert996 | 0:3e68940a3405 | 56 | ultima=2; |
robert996 | 0:3e68940a3405 | 57 | pc.printf("izquierda \n" ); |
robert996 | 0:3e68940a3405 | 58 | } |
robert996 | 0:3e68940a3405 | 59 | else if(foto2>rango_foto2 and foto1>rango_foto1){ |
robert996 | 0:3e68940a3405 | 60 | if(ultima==2){ |
robert996 | 0:3e68940a3405 | 61 | if(sev>900) |
robert996 | 0:3e68940a3405 | 62 | sev=sev-70; |
robert996 | 0:3e68940a3405 | 63 | else if (sev<=900) |
robert996 | 0:3e68940a3405 | 64 | sev=900; |
robert996 | 0:3e68940a3405 | 65 | servo.pulsewidth_us(sev);} |
robert996 | 0:3e68940a3405 | 66 | else if(ultima==1){ |
robert996 | 0:3e68940a3405 | 67 | if(sev<1900) |
robert996 | 0:3e68940a3405 | 68 | sev=sev+70; |
robert996 | 0:3e68940a3405 | 69 | else if (sev>=1900) |
robert996 | 0:3e68940a3405 | 70 | sev=1900; |
robert996 | 0:3e68940a3405 | 71 | servo.pulsewidth_us(sev); |
robert996 | 0:3e68940a3405 | 72 | } |
robert996 | 0:3e68940a3405 | 73 | wait(.0001); |
robert996 | 0:3e68940a3405 | 74 | } |
robert996 | 0:3e68940a3405 | 75 | } |
robert996 | 0:3e68940a3405 | 76 | } |