Raciel Flores Benítez
/
Ecuador_2017
sumo
main.cpp
- Committer:
- Sumobot
- Date:
- 2017-03-04
- Revision:
- 0:91600a6decef
File content as of revision 0:91600a6decef:
#include "mbed.h" #include "rtos.h" /********Declaracion de pines********************/ //Motores(1-derecha y 2 izquierda) DigitalOut enable2(P0_8);//S1 DigitalOut M2A(P0_13);//PWM4 DigitalOut M2B(P0_14);//PWM3 DigitalOut enable1(P1_13);//S2 DigitalOut M1A(P0_10);//PWM1 DigitalOut M1B(P0_9);//PWM2 //Sensores de presencia InterruptIn der(P1_22); InterruptIn izq(P1_19); InterruptIn fder(P1_14); InterruptIn fizq(P0_21); //Sensores de linea InterruptIn ld(P1_25); InterruptIn li(P0_15); //LEDs DigitalOut led1(P0_18);//D8-L4 DigitalOut led2(P0_19);//D9-L5 DigitalOut led3(P1_26);//D10-L6 DigitalOut led4(P1_27);//D11-L7 DigitalOut led5(P0_4);//D12-L8 DigitalOut led6(P0_17);//D14-L9 int m = 0; void girar_derecha() { M2A=0; M2B=1; M1A=1; M1B=0; } void stop2() { M2A = 1; M2B = 1; M1A = 1; M1B = 1; } void avanzar() { M2A=0; M2B=1; M1A=0; M1B=1; } void reversa() { M2A=1; M2B=0; M1A=1; M1B=0; } void girar_izquierda() { M2A=1; M2B=0; M1A=0; M1B=1; } void stop() { M2A=0; M2B=0; M1A=0; M1B=0; } /************************INTERRUPT********************/ int h = 0; void linea1() { if(h == 0) { stop(); reversa(); wait(0.3); girar_derecha(); wait(0.2); avanzar(); wait(0.1); h = h + 1; } else { girar_derecha(); wait(0.2); avanzar(); wait(0.1); h = 0; } } void linea2() { if(h == 1) { girar_derecha(); wait(0.1); avanzar(); wait(0.1); h = 0; } } void derecha() { led1=1; while(izq == 0) { girar_derecha(); wait(0.1); stop(); wait(0.01); } led1=0; } void izquierda() { led2=1; while(der == 0) { girar_izquierda(); wait(0.1); stop(); wait(0.01); } led2=0; } void frented() { while(fder == 0 && fizq == 1) { girar_izquierda(); wait(0.01); stop(); wait(0.01); } while(fder == 0 && fizq == 0) { avanzar(); wait(0.01); //stop(); //wait(0.01); } } void frenteizq() { while(fizq == 0 && fder == 1) { girar_derecha(); wait(0.01); stop(); wait(0.01); } while(fizq == 0 && fder == 0) { avanzar(); wait(0.01); //stop(); //wait(0.01); } } //*************************************************/ int main() { //configuracion en pullup o pulldown der.mode(PullDown); izq.mode(PullDown); fder.mode(PullDown); fizq.mode(PullDown); li.mode(PullUp); ld.mode(PullUp); wait(5); ld.rise(&linea1); li.rise(&linea2); fder.fall(&frented); fizq.fall(&frenteizq); der.fall(&izquierda); izq.fall(&derecha); while(1) { avanzar(); wait(0.3); stop(); wait(0.08); } }