![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sumo
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?
User | Revision | Line number | New 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 | } |