jhonatan angel
/
Arana
abrayan
main.cpp@6:8d7f6fe73ed1, 2018-11-10 (annotated)
- Committer:
- angel123
- Date:
- Sat Nov 10 22:41:02 2018 +0000
- Revision:
- 6:8d7f6fe73ed1
- Parent:
- 2:3007b3c06d2c
ara?a completa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fabeltranm | 0:89b318e49395 | 1 | #include "mbed.h" |
fabeltranm | 0:89b318e49395 | 2 | #include "main.h" |
fabeltranm | 0:89b318e49395 | 3 | |
angel123 | 6:8d7f6fe73ed1 | 4 | DigitalOut s0(PB_13); |
angel123 | 6:8d7f6fe73ed1 | 5 | DigitalOut s1(PB_14); |
angel123 | 6:8d7f6fe73ed1 | 6 | DigitalOut s2(PB_15); |
angel123 | 6:8d7f6fe73ed1 | 7 | DigitalOut s3(PB_1); |
angel123 | 6:8d7f6fe73ed1 | 8 | DigitalIn out (PB_2); |
angel123 | 6:8d7f6fe73ed1 | 9 | |
angel123 | 6:8d7f6fe73ed1 | 10 | DigitalOut LED(LED1); |
angel123 | 6:8d7f6fe73ed1 | 11 | InterruptIn pulse(PC_13); |
angel123 | 6:8d7f6fe73ed1 | 12 | float retardo = 0; |
angel123 | 6:8d7f6fe73ed1 | 13 | |
angel123 | 6:8d7f6fe73ed1 | 14 | |
angel123 | 6:8d7f6fe73ed1 | 15 | void ISR_Poff() |
angel123 | 6:8d7f6fe73ed1 | 16 | |
angel123 | 6:8d7f6fe73ed1 | 17 | { |
angel123 | 6:8d7f6fe73ed1 | 18 | |
angel123 | 6:8d7f6fe73ed1 | 19 | retardo = (0.05); |
angel123 | 6:8d7f6fe73ed1 | 20 | |
angel123 | 6:8d7f6fe73ed1 | 21 | } |
angel123 | 6:8d7f6fe73ed1 | 22 | |
angel123 | 6:8d7f6fe73ed1 | 23 | |
angel123 | 6:8d7f6fe73ed1 | 24 | |
angel123 | 6:8d7f6fe73ed1 | 25 | Timer tiempo; |
angel123 | 6:8d7f6fe73ed1 | 26 | |
angel123 | 6:8d7f6fe73ed1 | 27 | unsigned int lectura (void); |
angel123 | 6:8d7f6fe73ed1 | 28 | |
fabeltranm | 0:89b318e49395 | 29 | Serial command(USBTX, USBRX); |
fabeltranm | 0:89b318e49395 | 30 | |
fabeltranm | 0:89b318e49395 | 31 | int main() { |
fabeltranm | 0:89b318e49395 | 32 | init_servo(); |
fabeltranm | 0:89b318e49395 | 33 | init_serial(); |
angel123 | 6:8d7f6fe73ed1 | 34 | pulse.fall (&ISR_Poff); |
fabeltranm | 1:526bdd5faa37 | 35 | |
fabeltranm | 0:89b318e49395 | 36 | debug_m("inicio \n"); |
fabeltranm | 0:89b318e49395 | 37 | uint32_t read_cc; |
fabeltranm | 0:89b318e49395 | 38 | while(1) |
fabeltranm | 0:89b318e49395 | 39 | { |
angel123 | 6:8d7f6fe73ed1 | 40 | LED = !LED; |
angel123 | 6:8d7f6fe73ed1 | 41 | wait (retardo); |
angel123 | 6:8d7f6fe73ed1 | 42 | |
fabeltranm | 0:89b318e49395 | 43 | read_cc=read_command(); |
angel123 | 2:3007b3c06d2c | 44 | |
angel123 | 2:3007b3c06d2c | 45 | switch (read_cc) |
angel123 | 2:3007b3c06d2c | 46 | { |
angel123 | 2:3007b3c06d2c | 47 | case 0x01: moving(); |
angel123 | 2:3007b3c06d2c | 48 | break; |
angel123 | 2:3007b3c06d2c | 49 | |
angel123 | 6:8d7f6fe73ed1 | 50 | case 0x02: movimiento(); |
angel123 | 6:8d7f6fe73ed1 | 51 | break; |
angel123 | 6:8d7f6fe73ed1 | 52 | |
angel123 | 6:8d7f6fe73ed1 | 53 | case 0x03: sensor(); |
angel123 | 6:8d7f6fe73ed1 | 54 | break; |
angel123 | 6:8d7f6fe73ed1 | 55 | |
angel123 | 6:8d7f6fe73ed1 | 56 | case 0x04: saludo(); |
angel123 | 6:8d7f6fe73ed1 | 57 | break; |
angel123 | 6:8d7f6fe73ed1 | 58 | |
angel123 | 2:3007b3c06d2c | 59 | default: debug_m("error de comando.\n"); |
angel123 | 2:3007b3c06d2c | 60 | break ; |
fabeltranm | 0:89b318e49395 | 61 | } |
angel123 | 6:8d7f6fe73ed1 | 62 | |
fabeltranm | 0:89b318e49395 | 63 | } |
angel123 | 6:8d7f6fe73ed1 | 64 | |
fabeltranm | 0:89b318e49395 | 65 | } |
fabeltranm | 0:89b318e49395 | 66 | |
fabeltranm | 0:89b318e49395 | 67 | |
fabeltranm | 0:89b318e49395 | 68 | |
fabeltranm | 0:89b318e49395 | 69 | uint32_t read_command() |
fabeltranm | 0:89b318e49395 | 70 | { |
fabeltranm | 1:526bdd5faa37 | 71 | char intc=command.getc(); |
fabeltranm | 0:89b318e49395 | 72 | |
angel123 | 2:3007b3c06d2c | 73 | while(intc != 0xff) |
fabeltranm | 1:526bdd5faa37 | 74 | intc=command.getc(); |
angel123 | 2:3007b3c06d2c | 75 | return command.getc(); |
fabeltranm | 0:89b318e49395 | 76 | } |
fabeltranm | 1:526bdd5faa37 | 77 | |
fabeltranm | 1:526bdd5faa37 | 78 | |
angel123 | 6:8d7f6fe73ed1 | 79 | |
fabeltranm | 0:89b318e49395 | 80 | void init_serial() |
fabeltranm | 0:89b318e49395 | 81 | { |
fabeltranm | 0:89b318e49395 | 82 | command.baud(9600); |
fabeltranm | 0:89b318e49395 | 83 | } |
fabeltranm | 0:89b318e49395 | 84 | |
fabeltranm | 0:89b318e49395 | 85 | |
angel123 | 2:3007b3c06d2c | 86 | void moving() |
angel123 | 2:3007b3c06d2c | 87 | { |
angel123 | 6:8d7f6fe73ed1 | 88 | command.printf("se inicia el 1er comado mover servos..\n"); |
fabeltranm | 1:526bdd5faa37 | 89 | char nmotor=command.getc(); |
fabeltranm | 1:526bdd5faa37 | 90 | char grados=command.getc(); |
fabeltranm | 1:526bdd5faa37 | 91 | char endc=command.getc(); |
fabeltranm | 1:526bdd5faa37 | 92 | mover_ser(nmotor,grados); |
angel123 | 6:8d7f6fe73ed1 | 93 | debug_m("Moviendo servos...\n"); |
angel123 | 6:8d7f6fe73ed1 | 94 | } |
angel123 | 6:8d7f6fe73ed1 | 95 | |
angel123 | 6:8d7f6fe73ed1 | 96 | |
angel123 | 6:8d7f6fe73ed1 | 97 | void movimiento() |
angel123 | 6:8d7f6fe73ed1 | 98 | { |
angel123 | 6:8d7f6fe73ed1 | 99 | command.printf("se inicia el 2do comado mover patas..\n"); |
angel123 | 6:8d7f6fe73ed1 | 100 | char lado=command.getc(); |
angel123 | 6:8d7f6fe73ed1 | 101 | char movi=command.getc(); |
angel123 | 6:8d7f6fe73ed1 | 102 | char endc=command.getc(); |
angel123 | 6:8d7f6fe73ed1 | 103 | |
angel123 | 6:8d7f6fe73ed1 | 104 | switch (lado) |
angel123 | 6:8d7f6fe73ed1 | 105 | { |
angel123 | 6:8d7f6fe73ed1 | 106 | case 0x01: switch (movi) //Pata Derecha_delantera |
angel123 | 6:8d7f6fe73ed1 | 107 | { |
angel123 | 6:8d7f6fe73ed1 | 108 | case 0x01: mover_ser(0x01,0x01); |
angel123 | 6:8d7f6fe73ed1 | 109 | command.printf("moviendo pata derecha delantera arriba..\n"); |
angel123 | 6:8d7f6fe73ed1 | 110 | break; |
angel123 | 6:8d7f6fe73ed1 | 111 | |
angel123 | 6:8d7f6fe73ed1 | 112 | case 0x02: mover_ser(0x01,0x2d); |
angel123 | 6:8d7f6fe73ed1 | 113 | command.printf("moviendo pata derecha delantera abajo..\n"); |
angel123 | 6:8d7f6fe73ed1 | 114 | break; |
angel123 | 6:8d7f6fe73ed1 | 115 | |
angel123 | 6:8d7f6fe73ed1 | 116 | case 0x03: mover_ser(0x02,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 117 | command.printf("moviendo pata izquierda delantera adelante..\n"); |
angel123 | 6:8d7f6fe73ed1 | 118 | break; |
angel123 | 6:8d7f6fe73ed1 | 119 | |
angel123 | 6:8d7f6fe73ed1 | 120 | case 0x04: mover_ser(0x02,0x0a); |
angel123 | 6:8d7f6fe73ed1 | 121 | command.printf("moviendo pata izquierda delantera atras..\n"); |
angel123 | 6:8d7f6fe73ed1 | 122 | break; |
angel123 | 6:8d7f6fe73ed1 | 123 | } |
angel123 | 6:8d7f6fe73ed1 | 124 | break; |
angel123 | 6:8d7f6fe73ed1 | 125 | |
angel123 | 6:8d7f6fe73ed1 | 126 | case 0x02: switch (movi) //Pata Derecha_trasera |
angel123 | 6:8d7f6fe73ed1 | 127 | { |
angel123 | 6:8d7f6fe73ed1 | 128 | case 0x01: mover_ser(0x03,0x01); |
angel123 | 6:8d7f6fe73ed1 | 129 | command.printf("moviendo pata derecha trasera arriba..\n"); |
angel123 | 6:8d7f6fe73ed1 | 130 | break; |
angel123 | 6:8d7f6fe73ed1 | 131 | |
angel123 | 6:8d7f6fe73ed1 | 132 | case 0x02: mover_ser(0x03,0x2d); |
angel123 | 6:8d7f6fe73ed1 | 133 | command.printf("moviendo pata derecha trasera abajo..\n"); |
angel123 | 6:8d7f6fe73ed1 | 134 | break; |
angel123 | 6:8d7f6fe73ed1 | 135 | |
angel123 | 6:8d7f6fe73ed1 | 136 | case 0x03: mover_ser(0x04,0xaa); |
angel123 | 6:8d7f6fe73ed1 | 137 | command.printf("moviendo pata derecha trasera adelante..\n"); |
angel123 | 6:8d7f6fe73ed1 | 138 | break; |
angel123 | 6:8d7f6fe73ed1 | 139 | |
angel123 | 6:8d7f6fe73ed1 | 140 | case 0x04: mover_ser(0x04,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 141 | command.printf("moviendo pata derecha trasera atras..\n"); |
angel123 | 6:8d7f6fe73ed1 | 142 | break; |
angel123 | 6:8d7f6fe73ed1 | 143 | } |
angel123 | 6:8d7f6fe73ed1 | 144 | break; |
angel123 | 6:8d7f6fe73ed1 | 145 | |
angel123 | 6:8d7f6fe73ed1 | 146 | case 0x03: switch (movi) //Pata Izquierda_delantera |
angel123 | 6:8d7f6fe73ed1 | 147 | { |
angel123 | 6:8d7f6fe73ed1 | 148 | case 0x01: mover_ser(0x05,0x01); |
angel123 | 6:8d7f6fe73ed1 | 149 | command.printf("moviendo pata izquierda delantera arriba..\n"); |
angel123 | 6:8d7f6fe73ed1 | 150 | break; |
angel123 | 6:8d7f6fe73ed1 | 151 | |
angel123 | 6:8d7f6fe73ed1 | 152 | case 0x02: mover_ser(0x05,0x2d); |
angel123 | 6:8d7f6fe73ed1 | 153 | command.printf("moviendo pata izquierda delantera abajo..\n"); |
angel123 | 6:8d7f6fe73ed1 | 154 | break; |
angel123 | 6:8d7f6fe73ed1 | 155 | |
angel123 | 6:8d7f6fe73ed1 | 156 | case 0x03: mover_ser(0x06,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 157 | command.printf("moviendo pata izquierda delantera adelante..\n"); |
angel123 | 6:8d7f6fe73ed1 | 158 | break; |
angel123 | 6:8d7f6fe73ed1 | 159 | |
angel123 | 6:8d7f6fe73ed1 | 160 | case 0x04: mover_ser(0x06,0xaa); |
angel123 | 6:8d7f6fe73ed1 | 161 | command.printf("moviendo pata izquierda delantera atras..\n"); |
angel123 | 6:8d7f6fe73ed1 | 162 | break; |
angel123 | 6:8d7f6fe73ed1 | 163 | } |
angel123 | 6:8d7f6fe73ed1 | 164 | break; |
angel123 | 6:8d7f6fe73ed1 | 165 | |
angel123 | 6:8d7f6fe73ed1 | 166 | case 0x04: switch (movi) //Pata Izquierda_trasera |
angel123 | 6:8d7f6fe73ed1 | 167 | { |
angel123 | 6:8d7f6fe73ed1 | 168 | case 0x01: mover_ser(0x07,0x01); |
angel123 | 6:8d7f6fe73ed1 | 169 | command.printf("moviendo pata izquierda trasera arriba..\n"); |
angel123 | 6:8d7f6fe73ed1 | 170 | break; |
angel123 | 6:8d7f6fe73ed1 | 171 | |
angel123 | 6:8d7f6fe73ed1 | 172 | case 0x02: mover_ser(0x07,0x2d); |
angel123 | 6:8d7f6fe73ed1 | 173 | command.printf("moviendo pata izquierda trasera abajo..\n"); |
angel123 | 6:8d7f6fe73ed1 | 174 | break; |
angel123 | 6:8d7f6fe73ed1 | 175 | |
angel123 | 6:8d7f6fe73ed1 | 176 | case 0x03: mover_ser(0x08,0x0a); |
angel123 | 6:8d7f6fe73ed1 | 177 | command.printf("moviendo pata izquierda trasera adelante..\n"); |
angel123 | 6:8d7f6fe73ed1 | 178 | break; |
angel123 | 6:8d7f6fe73ed1 | 179 | |
angel123 | 6:8d7f6fe73ed1 | 180 | case 0x04: mover_ser(0x08,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 181 | command.printf("moviendo pata izquierda trasera atras..\n"); |
angel123 | 6:8d7f6fe73ed1 | 182 | break; |
angel123 | 6:8d7f6fe73ed1 | 183 | } |
angel123 | 6:8d7f6fe73ed1 | 184 | break; |
angel123 | 6:8d7f6fe73ed1 | 185 | |
angel123 | 6:8d7f6fe73ed1 | 186 | default: debug_m("error de comando.\n"); |
angel123 | 6:8d7f6fe73ed1 | 187 | break ; |
angel123 | 6:8d7f6fe73ed1 | 188 | } |
angel123 | 6:8d7f6fe73ed1 | 189 | //debug_m("fin del comado guardar..\n"); |
fabeltranm | 0:89b318e49395 | 190 | } |
fabeltranm | 0:89b318e49395 | 191 | |
angel123 | 6:8d7f6fe73ed1 | 192 | void sensor() |
angel123 | 6:8d7f6fe73ed1 | 193 | { |
angel123 | 6:8d7f6fe73ed1 | 194 | command.printf ("se inicia el 3er comando sensor de color..\n"); |
angel123 | 6:8d7f6fe73ed1 | 195 | char leer=command.getc(); |
angel123 | 6:8d7f6fe73ed1 | 196 | char endc=command.getc(); |
angel123 | 6:8d7f6fe73ed1 | 197 | |
angel123 | 6:8d7f6fe73ed1 | 198 | unsigned int ROJO=0; |
angel123 | 6:8d7f6fe73ed1 | 199 | unsigned int VERDE=0; |
angel123 | 6:8d7f6fe73ed1 | 200 | unsigned int AZUL=0; |
angel123 | 6:8d7f6fe73ed1 | 201 | |
angel123 | 6:8d7f6fe73ed1 | 202 | if(leer==0x01) |
angel123 | 6:8d7f6fe73ed1 | 203 | { |
angel123 | 6:8d7f6fe73ed1 | 204 | s0=1; |
angel123 | 6:8d7f6fe73ed1 | 205 | s1=0; |
angel123 | 6:8d7f6fe73ed1 | 206 | |
angel123 | 6:8d7f6fe73ed1 | 207 | s2=0; |
angel123 | 6:8d7f6fe73ed1 | 208 | s3=0; |
angel123 | 6:8d7f6fe73ed1 | 209 | ROJO= lectura(); |
angel123 | 6:8d7f6fe73ed1 | 210 | |
angel123 | 6:8d7f6fe73ed1 | 211 | s2=0; |
angel123 | 6:8d7f6fe73ed1 | 212 | s3=1; |
angel123 | 6:8d7f6fe73ed1 | 213 | AZUL= lectura(); |
angel123 | 6:8d7f6fe73ed1 | 214 | |
angel123 | 6:8d7f6fe73ed1 | 215 | s2=1; |
angel123 | 6:8d7f6fe73ed1 | 216 | s3=1; |
angel123 | 6:8d7f6fe73ed1 | 217 | VERDE= lectura(); |
angel123 | 6:8d7f6fe73ed1 | 218 | |
angel123 | 6:8d7f6fe73ed1 | 219 | if (ROJO<VERDE && VERDE>AZUL && AZUL>ROJO) |
angel123 | 6:8d7f6fe73ed1 | 220 | { |
angel123 | 6:8d7f6fe73ed1 | 221 | command.printf("Color detectado: ROJO \n"); |
angel123 | 6:8d7f6fe73ed1 | 222 | command.printf("ADELANTE....\n"); |
angel123 | 6:8d7f6fe73ed1 | 223 | |
angel123 | 6:8d7f6fe73ed1 | 224 | for(int x=0;x<5;x++) |
angel123 | 6:8d7f6fe73ed1 | 225 | { |
angel123 | 6:8d7f6fe73ed1 | 226 | mover_ser(0x01,0x00); |
angel123 | 6:8d7f6fe73ed1 | 227 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 228 | mover_ser(0x02,0x32); |
angel123 | 6:8d7f6fe73ed1 | 229 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 230 | mover_ser(0x01,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 231 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 232 | mover_ser(0x03,0x00); |
angel123 | 6:8d7f6fe73ed1 | 233 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 234 | mover_ser(0x04,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 235 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 236 | mover_ser(0x03,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 237 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 238 | mover_ser(0x02,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 239 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 240 | mover_ser(0x04,0x82); |
angel123 | 6:8d7f6fe73ed1 | 241 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 242 | mover_ser(0x05,0x00); |
angel123 | 6:8d7f6fe73ed1 | 243 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 244 | mover_ser(0x06,0x82); |
angel123 | 6:8d7f6fe73ed1 | 245 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 246 | mover_ser(0x05,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 247 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 248 | mover_ser(0x07,0x00); |
angel123 | 6:8d7f6fe73ed1 | 249 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 250 | mover_ser(0x08,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 251 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 252 | mover_ser(0x07,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 253 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 254 | mover_ser(0x06,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 255 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 256 | mover_ser(0x08,0x32); |
angel123 | 6:8d7f6fe73ed1 | 257 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 258 | |
angel123 | 6:8d7f6fe73ed1 | 259 | } |
angel123 | 6:8d7f6fe73ed1 | 260 | } |
angel123 | 6:8d7f6fe73ed1 | 261 | else |
angel123 | 6:8d7f6fe73ed1 | 262 | { |
angel123 | 6:8d7f6fe73ed1 | 263 | if (VERDE<AZUL && AZUL>ROJO && ROJO>VERDE) |
angel123 | 6:8d7f6fe73ed1 | 264 | { |
angel123 | 6:8d7f6fe73ed1 | 265 | command.printf("Color detectado: VERDE \n"); |
angel123 | 6:8d7f6fe73ed1 | 266 | command.printf("ROTAR DERECHA\n"); |
angel123 | 6:8d7f6fe73ed1 | 267 | |
angel123 | 6:8d7f6fe73ed1 | 268 | for(int x=0;x<5;x++) |
angel123 | 6:8d7f6fe73ed1 | 269 | { |
angel123 | 6:8d7f6fe73ed1 | 270 | mover_ser(0x01,0x00); |
angel123 | 6:8d7f6fe73ed1 | 271 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 272 | mover_ser(0x02,0x32); |
angel123 | 6:8d7f6fe73ed1 | 273 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 274 | mover_ser(0x01,0x3C); |
angel123 | 6:8d7f6fe73ed1 | 275 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 276 | mover_ser(0x03,0x00); |
angel123 | 6:8d7f6fe73ed1 | 277 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 278 | mover_ser(0x04,0x32); |
angel123 | 6:8d7f6fe73ed1 | 279 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 280 | mover_ser(0x03,0x3C); |
angel123 | 6:8d7f6fe73ed1 | 281 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 282 | mover_ser(0x07,0x00); |
angel123 | 6:8d7f6fe73ed1 | 283 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 284 | mover_ser(0x08,0x32); |
angel123 | 6:8d7f6fe73ed1 | 285 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 286 | mover_ser(0x07,0x3C); |
angel123 | 6:8d7f6fe73ed1 | 287 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 288 | mover_ser(0x05,0x00); |
angel123 | 6:8d7f6fe73ed1 | 289 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 290 | mover_ser(0x06,0x32); |
angel123 | 6:8d7f6fe73ed1 | 291 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 292 | mover_ser(0x05,0x3C); |
angel123 | 6:8d7f6fe73ed1 | 293 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 294 | mover_ser(0x02,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 295 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 296 | mover_ser(0x04,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 297 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 298 | mover_ser(0x06,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 299 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 300 | mover_ser(0x08,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 301 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 302 | } |
angel123 | 6:8d7f6fe73ed1 | 303 | } |
angel123 | 6:8d7f6fe73ed1 | 304 | else |
angel123 | 6:8d7f6fe73ed1 | 305 | { |
angel123 | 6:8d7f6fe73ed1 | 306 | if (AZUL<ROJO && ROJO>VERDE && VERDE>AZUL) |
angel123 | 6:8d7f6fe73ed1 | 307 | { |
angel123 | 6:8d7f6fe73ed1 | 308 | command.printf("Color detectado: AZUL \n"); |
angel123 | 6:8d7f6fe73ed1 | 309 | command.printf("ATRAS....\n"); |
angel123 | 6:8d7f6fe73ed1 | 310 | |
angel123 | 6:8d7f6fe73ed1 | 311 | |
angel123 | 6:8d7f6fe73ed1 | 312 | for(int x=0;x<5;x++) |
angel123 | 6:8d7f6fe73ed1 | 313 | { |
angel123 | 6:8d7f6fe73ed1 | 314 | mover_ser(0x01,0x00); |
angel123 | 6:8d7f6fe73ed1 | 315 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 316 | mover_ser(0x02,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 317 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 318 | mover_ser(0x01,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 319 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 320 | mover_ser(0x03,0x00); |
angel123 | 6:8d7f6fe73ed1 | 321 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 322 | mover_ser(0x04,0x82); |
angel123 | 6:8d7f6fe73ed1 | 323 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 324 | mover_ser(0x03,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 325 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 326 | mover_ser(0x02,0x32); |
angel123 | 6:8d7f6fe73ed1 | 327 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 328 | mover_ser(0x04,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 329 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 330 | mover_ser(0x05,0x00); |
angel123 | 6:8d7f6fe73ed1 | 331 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 332 | mover_ser(0x06,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 333 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 334 | mover_ser(0x05,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 335 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 336 | mover_ser(0x07,0x00); |
angel123 | 6:8d7f6fe73ed1 | 337 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 338 | mover_ser(0x08,0x32); |
angel123 | 6:8d7f6fe73ed1 | 339 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 340 | mover_ser(0x07,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 341 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 342 | mover_ser(0x06,0x82); |
angel123 | 6:8d7f6fe73ed1 | 343 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 344 | mover_ser(0x08,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 345 | wait(0.2); |
angel123 | 6:8d7f6fe73ed1 | 346 | |
angel123 | 6:8d7f6fe73ed1 | 347 | } |
angel123 | 6:8d7f6fe73ed1 | 348 | } |
angel123 | 6:8d7f6fe73ed1 | 349 | else |
angel123 | 6:8d7f6fe73ed1 | 350 | { |
angel123 | 6:8d7f6fe73ed1 | 351 | command.printf("otro color \n"); |
angel123 | 6:8d7f6fe73ed1 | 352 | } |
angel123 | 6:8d7f6fe73ed1 | 353 | } |
angel123 | 6:8d7f6fe73ed1 | 354 | } |
angel123 | 6:8d7f6fe73ed1 | 355 | |
angel123 | 6:8d7f6fe73ed1 | 356 | wait(0.5); |
angel123 | 6:8d7f6fe73ed1 | 357 | } |
angel123 | 6:8d7f6fe73ed1 | 358 | } |
angel123 | 6:8d7f6fe73ed1 | 359 | |
angel123 | 6:8d7f6fe73ed1 | 360 | |
angel123 | 6:8d7f6fe73ed1 | 361 | unsigned int lectura (void) |
angel123 | 6:8d7f6fe73ed1 | 362 | { |
angel123 | 6:8d7f6fe73ed1 | 363 | |
angel123 | 6:8d7f6fe73ed1 | 364 | unsigned int inicio=0, final=0, resultado=0; |
angel123 | 6:8d7f6fe73ed1 | 365 | tiempo.start (); |
angel123 | 6:8d7f6fe73ed1 | 366 | while (out) {} |
angel123 | 6:8d7f6fe73ed1 | 367 | while (!out) {} |
angel123 | 6:8d7f6fe73ed1 | 368 | while (out) {} |
angel123 | 6:8d7f6fe73ed1 | 369 | |
angel123 | 6:8d7f6fe73ed1 | 370 | inicio= tiempo.read_us(); |
angel123 | 6:8d7f6fe73ed1 | 371 | while (!out) {} |
angel123 | 6:8d7f6fe73ed1 | 372 | final=tiempo.read_us(); |
angel123 | 6:8d7f6fe73ed1 | 373 | resultado=(final-inicio); |
angel123 | 6:8d7f6fe73ed1 | 374 | tiempo.reset (); |
angel123 | 6:8d7f6fe73ed1 | 375 | return (resultado); |
angel123 | 6:8d7f6fe73ed1 | 376 | |
angel123 | 6:8d7f6fe73ed1 | 377 | |
angel123 | 6:8d7f6fe73ed1 | 378 | } |
angel123 | 6:8d7f6fe73ed1 | 379 | |
angel123 | 6:8d7f6fe73ed1 | 380 | |
angel123 | 6:8d7f6fe73ed1 | 381 | void saludo (void) |
angel123 | 6:8d7f6fe73ed1 | 382 | { |
angel123 | 6:8d7f6fe73ed1 | 383 | char lado=command.getc(); |
angel123 | 6:8d7f6fe73ed1 | 384 | switch (lado) |
angel123 | 6:8d7f6fe73ed1 | 385 | { |
angel123 | 6:8d7f6fe73ed1 | 386 | case 0x01: command.printf("HOLA!!!!....\n"); |
angel123 | 6:8d7f6fe73ed1 | 387 | mover_ser(0x03,0x00); |
angel123 | 6:8d7f6fe73ed1 | 388 | mover_ser(0x07,0x00); |
angel123 | 6:8d7f6fe73ed1 | 389 | mover_ser(0x05,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 390 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 391 | mover_ser(0x01,0x00); |
angel123 | 6:8d7f6fe73ed1 | 392 | wait(0.4); |
angel123 | 6:8d7f6fe73ed1 | 393 | mover_ser(0x02,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 394 | wait(0.4); |
angel123 | 6:8d7f6fe73ed1 | 395 | mover_ser(0x02,0x0a); |
angel123 | 6:8d7f6fe73ed1 | 396 | wait(0.4); |
angel123 | 6:8d7f6fe73ed1 | 397 | mover_ser(0x02,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 398 | wait(0.4); |
angel123 | 6:8d7f6fe73ed1 | 399 | mover_ser(0x02,0x0a); |
angel123 | 6:8d7f6fe73ed1 | 400 | wait(0.4); |
angel123 | 6:8d7f6fe73ed1 | 401 | mover_ser(0x02,0x5a); |
angel123 | 6:8d7f6fe73ed1 | 402 | wait(0.4); |
angel123 | 6:8d7f6fe73ed1 | 403 | mover_ser(0x01,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 404 | mover_ser(0x03,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 405 | mover_ser(0x07,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 406 | mover_ser(0x05,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 407 | wait(0.1); |
angel123 | 6:8d7f6fe73ed1 | 408 | break; |
angel123 | 6:8d7f6fe73ed1 | 409 | |
angel123 | 6:8d7f6fe73ed1 | 410 | case 0x02: command.printf("EMOCIONADO!!!!....\n"); |
angel123 | 6:8d7f6fe73ed1 | 411 | mover_ser(0x01,0x00); |
angel123 | 6:8d7f6fe73ed1 | 412 | mover_ser(0x03,0x00); |
angel123 | 6:8d7f6fe73ed1 | 413 | mover_ser(0x05,0x00); |
angel123 | 6:8d7f6fe73ed1 | 414 | mover_ser(0x07,0x00); |
angel123 | 6:8d7f6fe73ed1 | 415 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 416 | mover_ser(0x01,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 417 | mover_ser(0x03,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 418 | mover_ser(0x05,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 419 | mover_ser(0x07,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 420 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 421 | mover_ser(0x01,0x00); |
angel123 | 6:8d7f6fe73ed1 | 422 | mover_ser(0x03,0x00); |
angel123 | 6:8d7f6fe73ed1 | 423 | mover_ser(0x05,0x00); |
angel123 | 6:8d7f6fe73ed1 | 424 | mover_ser(0x07,0x00); |
angel123 | 6:8d7f6fe73ed1 | 425 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 426 | mover_ser(0x01,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 427 | mover_ser(0x03,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 428 | mover_ser(0x05,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 429 | mover_ser(0x07,0x3c); |
angel123 | 6:8d7f6fe73ed1 | 430 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 431 | break; |
angel123 | 6:8d7f6fe73ed1 | 432 | |
angel123 | 6:8d7f6fe73ed1 | 433 | case 0x03: command.printf("''GIRANDO''!!!!....\n"); |
angel123 | 6:8d7f6fe73ed1 | 434 | mover_ser(0x01,0x00); |
angel123 | 6:8d7f6fe73ed1 | 435 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 436 | mover_ser(0x03,0x00); |
angel123 | 6:8d7f6fe73ed1 | 437 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 438 | mover_ser(0x07,0x00); |
angel123 | 6:8d7f6fe73ed1 | 439 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 440 | mover_ser(0x01,0x46); |
angel123 | 6:8d7f6fe73ed1 | 441 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 442 | mover_ser(0x05,0x00); |
angel123 | 6:8d7f6fe73ed1 | 443 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 444 | mover_ser(0x03,0x46); |
angel123 | 6:8d7f6fe73ed1 | 445 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 446 | mover_ser(0x01,0x00); |
angel123 | 6:8d7f6fe73ed1 | 447 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 448 | mover_ser(0x07,0x46); |
angel123 | 6:8d7f6fe73ed1 | 449 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 450 | mover_ser(0x03,0x00); |
angel123 | 6:8d7f6fe73ed1 | 451 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 452 | mover_ser(0x05,0x46); |
angel123 | 6:8d7f6fe73ed1 | 453 | mover_ser(0x01,0x46); |
angel123 | 6:8d7f6fe73ed1 | 454 | mover_ser(0x03,0x46); |
angel123 | 6:8d7f6fe73ed1 | 455 | mover_ser(0x07,0x46); |
angel123 | 6:8d7f6fe73ed1 | 456 | wait(0.3); |
angel123 | 6:8d7f6fe73ed1 | 457 | break; |
angel123 | 6:8d7f6fe73ed1 | 458 | |
angel123 | 6:8d7f6fe73ed1 | 459 | |
angel123 | 6:8d7f6fe73ed1 | 460 | default: command.printf("error de comando.\n"); |
angel123 | 6:8d7f6fe73ed1 | 461 | break ; |
angel123 | 6:8d7f6fe73ed1 | 462 | } |
angel123 | 6:8d7f6fe73ed1 | 463 | |
angel123 | 6:8d7f6fe73ed1 | 464 | |
angel123 | 6:8d7f6fe73ed1 | 465 | } |
angel123 | 6:8d7f6fe73ed1 | 466 | |
angel123 | 6:8d7f6fe73ed1 | 467 | |
fabeltranm | 0:89b318e49395 | 468 | void debug_m(char *s , ... ){ |
fabeltranm | 0:89b318e49395 | 469 | #if DEBUG |
fabeltranm | 0:89b318e49395 | 470 | command.printf(s); |
fabeltranm | 0:89b318e49395 | 471 | #endif |
fabeltranm | 0:89b318e49395 | 472 | } |