Raciel Flores Benítez / Mbed 2 deprecated Ecuador_2017

sumo

Dependencies:   mbed-rtos mbed

main.cpp@0:91600a6decef, 2017-03-04 (annotated)

Committer:
Sumobot
Date:
Sat Mar 04 18:20:34 2017 +0000
Revision:
0:91600a6decef
sumo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sumobot 0:91600a6decef 1 #include "mbed.h"
Sumobot 0:91600a6decef 2 #include "rtos.h"
Sumobot 0:91600a6decef 3
Sumobot 0:91600a6decef 4 /********Declaracion de pines********************/
Sumobot 0:91600a6decef 5 //Motores(1-derecha y 2 izquierda)
Sumobot 0:91600a6decef 6 DigitalOut enable2(P0_8);//S1
Sumobot 0:91600a6decef 7 DigitalOut M2A(P0_13);//PWM4
Sumobot 0:91600a6decef 8 DigitalOut M2B(P0_14);//PWM3
Sumobot 0:91600a6decef 9
Sumobot 0:91600a6decef 10 DigitalOut enable1(P1_13);//S2
Sumobot 0:91600a6decef 11 DigitalOut M1A(P0_10);//PWM1
Sumobot 0:91600a6decef 12 DigitalOut M1B(P0_9);//PWM2
Sumobot 0:91600a6decef 13
Sumobot 0:91600a6decef 14
Sumobot 0:91600a6decef 15 //Sensores de presencia
Sumobot 0:91600a6decef 16 InterruptIn der(P1_22);
Sumobot 0:91600a6decef 17 InterruptIn izq(P1_19);
Sumobot 0:91600a6decef 18 InterruptIn fder(P1_14);
Sumobot 0:91600a6decef 19 InterruptIn fizq(P0_21);
Sumobot 0:91600a6decef 20
Sumobot 0:91600a6decef 21 //Sensores de linea
Sumobot 0:91600a6decef 22 InterruptIn ld(P1_25);
Sumobot 0:91600a6decef 23 InterruptIn li(P0_15);
Sumobot 0:91600a6decef 24
Sumobot 0:91600a6decef 25 //LEDs
Sumobot 0:91600a6decef 26 DigitalOut led1(P0_18);//D8-L4
Sumobot 0:91600a6decef 27 DigitalOut led2(P0_19);//D9-L5
Sumobot 0:91600a6decef 28 DigitalOut led3(P1_26);//D10-L6
Sumobot 0:91600a6decef 29 DigitalOut led4(P1_27);//D11-L7
Sumobot 0:91600a6decef 30 DigitalOut led5(P0_4);//D12-L8
Sumobot 0:91600a6decef 31 DigitalOut led6(P0_17);//D14-L9
Sumobot 0:91600a6decef 32
Sumobot 0:91600a6decef 33 int m = 0;
Sumobot 0:91600a6decef 34 void girar_derecha()
Sumobot 0:91600a6decef 35 {
Sumobot 0:91600a6decef 36 M2A=0;
Sumobot 0:91600a6decef 37 M2B=1;
Sumobot 0:91600a6decef 38 M1A=1;
Sumobot 0:91600a6decef 39 M1B=0;
Sumobot 0:91600a6decef 40 }
Sumobot 0:91600a6decef 41 void stop2()
Sumobot 0:91600a6decef 42 {
Sumobot 0:91600a6decef 43 M2A = 1;
Sumobot 0:91600a6decef 44 M2B = 1;
Sumobot 0:91600a6decef 45 M1A = 1;
Sumobot 0:91600a6decef 46 M1B = 1;
Sumobot 0:91600a6decef 47 }
Sumobot 0:91600a6decef 48
Sumobot 0:91600a6decef 49 void avanzar()
Sumobot 0:91600a6decef 50 {
Sumobot 0:91600a6decef 51 M2A=0;
Sumobot 0:91600a6decef 52 M2B=1;
Sumobot 0:91600a6decef 53 M1A=0;
Sumobot 0:91600a6decef 54 M1B=1;
Sumobot 0:91600a6decef 55 }
Sumobot 0:91600a6decef 56 void reversa()
Sumobot 0:91600a6decef 57 {
Sumobot 0:91600a6decef 58 M2A=1;
Sumobot 0:91600a6decef 59 M2B=0;
Sumobot 0:91600a6decef 60 M1A=1;
Sumobot 0:91600a6decef 61 M1B=0;
Sumobot 0:91600a6decef 62 }
Sumobot 0:91600a6decef 63
Sumobot 0:91600a6decef 64 void girar_izquierda()
Sumobot 0:91600a6decef 65 {
Sumobot 0:91600a6decef 66 M2A=1;
Sumobot 0:91600a6decef 67 M2B=0;
Sumobot 0:91600a6decef 68 M1A=0;
Sumobot 0:91600a6decef 69 M1B=1;
Sumobot 0:91600a6decef 70 }
Sumobot 0:91600a6decef 71
Sumobot 0:91600a6decef 72 void stop()
Sumobot 0:91600a6decef 73 {
Sumobot 0:91600a6decef 74 M2A=0;
Sumobot 0:91600a6decef 75 M2B=0;
Sumobot 0:91600a6decef 76 M1A=0;
Sumobot 0:91600a6decef 77 M1B=0;
Sumobot 0:91600a6decef 78 }
Sumobot 0:91600a6decef 79
Sumobot 0:91600a6decef 80 /************************INTERRUPT********************/
Sumobot 0:91600a6decef 81 int h = 0;
Sumobot 0:91600a6decef 82 void linea1()
Sumobot 0:91600a6decef 83 {
Sumobot 0:91600a6decef 84 if(h == 0) {
Sumobot 0:91600a6decef 85 stop();
Sumobot 0:91600a6decef 86 reversa();
Sumobot 0:91600a6decef 87 wait(0.3);
Sumobot 0:91600a6decef 88 girar_derecha();
Sumobot 0:91600a6decef 89 wait(0.2);
Sumobot 0:91600a6decef 90 avanzar();
Sumobot 0:91600a6decef 91 wait(0.1);
Sumobot 0:91600a6decef 92 h = h + 1;
Sumobot 0:91600a6decef 93 } else {
Sumobot 0:91600a6decef 94 girar_derecha();
Sumobot 0:91600a6decef 95 wait(0.2);
Sumobot 0:91600a6decef 96 avanzar();
Sumobot 0:91600a6decef 97 wait(0.1);
Sumobot 0:91600a6decef 98 h = 0;
Sumobot 0:91600a6decef 99 }
Sumobot 0:91600a6decef 100 }
Sumobot 0:91600a6decef 101 void linea2()
Sumobot 0:91600a6decef 102 {
Sumobot 0:91600a6decef 103 if(h == 1) {
Sumobot 0:91600a6decef 104 girar_derecha();
Sumobot 0:91600a6decef 105 wait(0.1);
Sumobot 0:91600a6decef 106 avanzar();
Sumobot 0:91600a6decef 107 wait(0.1);
Sumobot 0:91600a6decef 108 h = 0;
Sumobot 0:91600a6decef 109 }
Sumobot 0:91600a6decef 110 }
Sumobot 0:91600a6decef 111
Sumobot 0:91600a6decef 112 void derecha()
Sumobot 0:91600a6decef 113 {
Sumobot 0:91600a6decef 114 led1=1;
Sumobot 0:91600a6decef 115 while(izq == 0) {
Sumobot 0:91600a6decef 116 girar_derecha();
Sumobot 0:91600a6decef 117 wait(0.1);
Sumobot 0:91600a6decef 118 stop();
Sumobot 0:91600a6decef 119 wait(0.01);
Sumobot 0:91600a6decef 120 }
Sumobot 0:91600a6decef 121 led1=0;
Sumobot 0:91600a6decef 122 }
Sumobot 0:91600a6decef 123
Sumobot 0:91600a6decef 124 void izquierda()
Sumobot 0:91600a6decef 125 {
Sumobot 0:91600a6decef 126 led2=1;
Sumobot 0:91600a6decef 127 while(der == 0) {
Sumobot 0:91600a6decef 128 girar_izquierda();
Sumobot 0:91600a6decef 129 wait(0.1);
Sumobot 0:91600a6decef 130 stop();
Sumobot 0:91600a6decef 131 wait(0.01);
Sumobot 0:91600a6decef 132 }
Sumobot 0:91600a6decef 133 led2=0;
Sumobot 0:91600a6decef 134 }
Sumobot 0:91600a6decef 135 void frented()
Sumobot 0:91600a6decef 136 {
Sumobot 0:91600a6decef 137 while(fder == 0 && fizq == 1) {
Sumobot 0:91600a6decef 138 girar_izquierda();
Sumobot 0:91600a6decef 139 wait(0.01);
Sumobot 0:91600a6decef 140 stop();
Sumobot 0:91600a6decef 141 wait(0.01);
Sumobot 0:91600a6decef 142 }
Sumobot 0:91600a6decef 143 while(fder == 0 && fizq == 0) {
Sumobot 0:91600a6decef 144 avanzar();
Sumobot 0:91600a6decef 145 wait(0.01);
Sumobot 0:91600a6decef 146 //stop();
Sumobot 0:91600a6decef 147 //wait(0.01);
Sumobot 0:91600a6decef 148 }
Sumobot 0:91600a6decef 149 }
Sumobot 0:91600a6decef 150 void frenteizq()
Sumobot 0:91600a6decef 151 {
Sumobot 0:91600a6decef 152 while(fizq == 0 && fder == 1) {
Sumobot 0:91600a6decef 153 girar_derecha();
Sumobot 0:91600a6decef 154 wait(0.01);
Sumobot 0:91600a6decef 155 stop();
Sumobot 0:91600a6decef 156 wait(0.01);
Sumobot 0:91600a6decef 157 }
Sumobot 0:91600a6decef 158 while(fizq == 0 && fder == 0) {
Sumobot 0:91600a6decef 159 avanzar();
Sumobot 0:91600a6decef 160 wait(0.01);
Sumobot 0:91600a6decef 161 //stop();
Sumobot 0:91600a6decef 162 //wait(0.01);
Sumobot 0:91600a6decef 163
Sumobot 0:91600a6decef 164 }
Sumobot 0:91600a6decef 165 }
Sumobot 0:91600a6decef 166 //*************************************************/
Sumobot 0:91600a6decef 167
Sumobot 0:91600a6decef 168 int main()
Sumobot 0:91600a6decef 169 {
Sumobot 0:91600a6decef 170 //configuracion en pullup o pulldown
Sumobot 0:91600a6decef 171 der.mode(PullDown);
Sumobot 0:91600a6decef 172 izq.mode(PullDown);
Sumobot 0:91600a6decef 173 fder.mode(PullDown);
Sumobot 0:91600a6decef 174 fizq.mode(PullDown);
Sumobot 0:91600a6decef 175 li.mode(PullUp);
Sumobot 0:91600a6decef 176 ld.mode(PullUp);
Sumobot 0:91600a6decef 177
Sumobot 0:91600a6decef 178 wait(5);
Sumobot 0:91600a6decef 179 ld.rise(&linea1);
Sumobot 0:91600a6decef 180 li.rise(&linea2);
Sumobot 0:91600a6decef 181 fder.fall(&frented);
Sumobot 0:91600a6decef 182 fizq.fall(&frenteizq);
Sumobot 0:91600a6decef 183 der.fall(&izquierda);
Sumobot 0:91600a6decef 184 izq.fall(&derecha);
Sumobot 0:91600a6decef 185 while(1) {
Sumobot 0:91600a6decef 186 avanzar();
Sumobot 0:91600a6decef 187 wait(0.3);
Sumobot 0:91600a6decef 188 stop();
Sumobot 0:91600a6decef 189 wait(0.08);
Sumobot 0:91600a6decef 190
Sumobot 0:91600a6decef 191 }
Sumobot 0:91600a6decef 192 }