jhonatan angel / Mbed 2 deprecated Arana

abrayan

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }