sebastian martinez / Mbed 2 deprecated Avance_Color

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:73e805ecf765, 2018-10-10 (annotated)

Committer:
sebasmartinez
Date:
Wed Oct 10 22:49:18 2018 +0000
Revision:
0:73e805ecf765
Child:
1:59be7d7e1b54
ENTREGA

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sebasmartinez 0:73e805ecf765 1 #include "mbed.h"
sebasmartinez 0:73e805ecf765 2 #include <Timer.h>
sebasmartinez 0:73e805ecf765 3 #include "main.h"
sebasmartinez 0:73e805ecf765 4 #define MEM_SIZE 5
sebasmartinez 0:73e805ecf765 5 #define MEM_TYPE char
sebasmartinez 0:73e805ecf765 6 #define COLOR_SIZE 3
sebasmartinez 0:73e805ecf765 7 #define TIEMPO_SIZE 0.2
sebasmartinez 0:73e805ecf765 8 MEM_TYPE buffer[MEM_SIZE];
sebasmartinez 0:73e805ecf765 9 Serial command(USBTX, USBRX); // Usar Serial PC USB
sebasmartinez 0:73e805ecf765 10 //Serial command(PC_10,PC_11); //Tx, RX. Usar Bluetooth HC-06.
sebasmartinez 0:73e805ecf765 11
sebasmartinez 0:73e805ecf765 12 DigitalIn button(USER_BUTTON);
sebasmartinez 0:73e805ecf765 13 /* //Puertos Sensor color
sebasmartinez 0:73e805ecf765 14 //433
sebasmartinez 0:73e805ecf765 15 DigitalIn OUT(PC_0);//A5 BC1
sebasmartinez 0:73e805ecf765 16 DigitalOut S2(PC_1);//A4 VD1
sebasmartinez 0:73e805ecf765 17 DigitalOut S3(PC_2);//A3 NG
sebasmartinez 0:73e805ecf765 18 DigitalOut S1(PC_3);//A2 BC2
sebasmartinez 0:73e805ecf765 19 DigitalOut S0(PA_1);//A1 VD2
sebasmartinez 0:73e805ecf765 20 */
sebasmartinez 0:73e805ecf765 21 //446
sebasmartinez 0:73e805ecf765 22 DigitalIn OUT(PC_0);//A5 BC1
sebasmartinez 0:73e805ecf765 23 DigitalOut S2(PC_1);//A4 VD1
sebasmartinez 0:73e805ecf765 24 DigitalOut S3(PB_0);//A3 NG
sebasmartinez 0:73e805ecf765 25 DigitalOut S1(PA_4);//A2 BC2
sebasmartinez 0:73e805ecf765 26 DigitalOut S0(PA_1);//A1 VD2
sebasmartinez 0:73e805ecf765 27 //
sebasmartinez 0:73e805ecf765 28 Timer tiempo;
sebasmartinez 0:73e805ecf765 29 //Variables de Trabajo Colores identificados
sebasmartinez 0:73e805ecf765 30 int Rojo = 0;
sebasmartinez 0:73e805ecf765 31 int Azul = 0;
sebasmartinez 0:73e805ecf765 32 int Verde = 0;
sebasmartinez 0:73e805ecf765 33 int Amarillo = 0;
sebasmartinez 0:73e805ecf765 34 int Negro = 0;
sebasmartinez 0:73e805ecf765 35 int Blanco = 0;
sebasmartinez 0:73e805ecf765 36 int color = 0;
sebasmartinez 0:73e805ecf765 37 // color
sebasmartinez 0:73e805ecf765 38 int main() {
sebasmartinez 0:73e805ecf765 39 init_servo();
sebasmartinez 0:73e805ecf765 40 init_serial();
sebasmartinez 0:73e805ecf765 41 debug_m("inicio \n");
sebasmartinez 0:73e805ecf765 42 uint32_t read_cc;
sebasmartinez 0:73e805ecf765 43 while(1)
sebasmartinez 0:73e805ecf765 44 {
sebasmartinez 0:73e805ecf765 45 read_cc=read_command();
sebasmartinez 0:73e805ecf765 46 debug_m("Entrando TC...\n");
sebasmartinez 0:73e805ecf765 47
sebasmartinez 0:73e805ecf765 48 switch (read_cc) {
sebasmartinez 0:73e805ecf765 49
sebasmartinez 0:73e805ecf765 50 case 0x01:
sebasmartinez 0:73e805ecf765 51 debug_m("Tc 1 Iniciado..\n");
sebasmartinez 0:73e805ecf765 52 moving();
sebasmartinez 0:73e805ecf765 53 debug_m("Tc 1 Finalizado\n");
sebasmartinez 0:73e805ecf765 54 break;
sebasmartinez 0:73e805ecf765 55 case 0x02:
sebasmartinez 0:73e805ecf765 56 debug_m("Tc 2 Iniciado.. \n");
sebasmartinez 0:73e805ecf765 57 pares();
sebasmartinez 0:73e805ecf765 58 debug_m("Tc 2 Finalizado\n");
sebasmartinez 0:73e805ecf765 59 break;
sebasmartinez 0:73e805ecf765 60 case 0x03:
sebasmartinez 0:73e805ecf765 61 debug_m("Tc 3 Iniciado.. \n");
sebasmartinez 0:73e805ecf765 62 caminar();
sebasmartinez 0:73e805ecf765 63 debug_m("Tc 3 Finalizado\n");
sebasmartinez 0:73e805ecf765 64 break;
sebasmartinez 0:73e805ecf765 65 case 0x05:
sebasmartinez 0:73e805ecf765 66 debug_m("Tc 5 Centrar Iniciado.. \n");
sebasmartinez 0:73e805ecf765 67 centro(01);
sebasmartinez 0:73e805ecf765 68 centro(02);
sebasmartinez 0:73e805ecf765 69 centro(03);
sebasmartinez 0:73e805ecf765 70 centro(04);
sebasmartinez 0:73e805ecf765 71 debug_m("Tc 5 Centrar Finalizado\n");
sebasmartinez 0:73e805ecf765 72 break;
sebasmartinez 0:73e805ecf765 73 default: debug_m("Error de comando. \nSe espera entre 0x01 - 0x08 \n");break ;
sebasmartinez 0:73e805ecf765 74 }
sebasmartinez 0:73e805ecf765 75 }
sebasmartinez 0:73e805ecf765 76 }
sebasmartinez 0:73e805ecf765 77
sebasmartinez 0:73e805ecf765 78
sebasmartinez 0:73e805ecf765 79
sebasmartinez 0:73e805ecf765 80 uint32_t read_command()
sebasmartinez 0:73e805ecf765 81 {
sebasmartinez 0:73e805ecf765 82 // retorna los byte recibidos concatenados en un entero,
sebasmartinez 0:73e805ecf765 83 // Formato | FF | TC | NM | GR | FD
sebasmartinez 0:73e805ecf765 84 // Datos | Inicio | Tele-comando | Numero Motor | Grados | Cierre
sebasmartinez 0:73e805ecf765 85 //char intc=command.getc();
sebasmartinez 0:73e805ecf765 86 buffer[0] = command.getc();
sebasmartinez 0:73e805ecf765 87
sebasmartinez 0:73e805ecf765 88 inicio:
sebasmartinez 0:73e805ecf765 89
sebasmartinez 0:73e805ecf765 90 while(buffer[0] != 0xFF ){ //7B y 7D son { y }
sebasmartinez 0:73e805ecf765 91
sebasmartinez 0:73e805ecf765 92 buffer[0] = command.getc();
sebasmartinez 0:73e805ecf765 93
sebasmartinez 0:73e805ecf765 94 debug_m("comando inicio invalido \n");
sebasmartinez 0:73e805ecf765 95 }
sebasmartinez 0:73e805ecf765 96 debug_m("comando inicio Valido \n");
sebasmartinez 0:73e805ecf765 97 char ini = buffer[0];
sebasmartinez 0:73e805ecf765 98 printf("inicio encontrado: %x \n", ini);
sebasmartinez 0:73e805ecf765 99
sebasmartinez 0:73e805ecf765 100 buffer[1] = command.getc();
sebasmartinez 0:73e805ecf765 101 if (buffer[1] != 0xFF && buffer[1] != 0xFD){
sebasmartinez 0:73e805ecf765 102 buffer[2] = command.getc();
sebasmartinez 0:73e805ecf765 103 }else{
sebasmartinez 0:73e805ecf765 104 buffer[0] = buffer[1];
sebasmartinez 0:73e805ecf765 105 goto inicio;
sebasmartinez 0:73e805ecf765 106 }
sebasmartinez 0:73e805ecf765 107 if (buffer[2] != 0xFF && buffer[2] != 0xFD){
sebasmartinez 0:73e805ecf765 108 buffer[3] = command.getc();
sebasmartinez 0:73e805ecf765 109 }else{
sebasmartinez 0:73e805ecf765 110 buffer[0] = buffer[2];
sebasmartinez 0:73e805ecf765 111 goto inicio;
sebasmartinez 0:73e805ecf765 112 }
sebasmartinez 0:73e805ecf765 113 if (buffer[3] != 0xFF && buffer[3] != 0xFD){
sebasmartinez 0:73e805ecf765 114 buffer[4] = command.getc();
sebasmartinez 0:73e805ecf765 115 }else{
sebasmartinez 0:73e805ecf765 116 buffer[0] = buffer[3];
sebasmartinez 0:73e805ecf765 117 goto inicio;
sebasmartinez 0:73e805ecf765 118 }
sebasmartinez 0:73e805ecf765 119 //para que evalue el cierre
sebasmartinez 0:73e805ecf765 120 if (buffer[4] != 0xFF && buffer[4] == 0xFD){
sebasmartinez 0:73e805ecf765 121 debug_m("comando Cierre valido \n");
sebasmartinez 0:73e805ecf765 122 }else{
sebasmartinez 0:73e805ecf765 123 buffer[0] = buffer[4];
sebasmartinez 0:73e805ecf765 124 goto inicio;
sebasmartinez 0:73e805ecf765 125 }
sebasmartinez 0:73e805ecf765 126
sebasmartinez 0:73e805ecf765 127
sebasmartinez 0:73e805ecf765 128
sebasmartinez 0:73e805ecf765 129 //intc=command.getc();
sebasmartinez 0:73e805ecf765 130 int tele = buffer[1];
sebasmartinez 0:73e805ecf765 131 char telec = buffer[1];
sebasmartinez 0:73e805ecf765 132 printf("Tele comando: %x \n", telec );
sebasmartinez 0:73e805ecf765 133
sebasmartinez 0:73e805ecf765 134
sebasmartinez 0:73e805ecf765 135 return tele;
sebasmartinez 0:73e805ecf765 136
sebasmartinez 0:73e805ecf765 137 }
sebasmartinez 0:73e805ecf765 138
sebasmartinez 0:73e805ecf765 139
sebasmartinez 0:73e805ecf765 140 void init_serial()
sebasmartinez 0:73e805ecf765 141 {
sebasmartinez 0:73e805ecf765 142 command.baud(9600);
sebasmartinez 0:73e805ecf765 143 debug_m("Serial 9600 \n");
sebasmartinez 0:73e805ecf765 144 }
sebasmartinez 0:73e805ecf765 145
sebasmartinez 0:73e805ecf765 146
sebasmartinez 0:73e805ecf765 147 void moving(){
sebasmartinez 0:73e805ecf765 148 debug_m("se inicia el comado mover..\n");
sebasmartinez 0:73e805ecf765 149
sebasmartinez 0:73e805ecf765 150 char nmotor = buffer[2];
sebasmartinez 0:73e805ecf765 151 printf("Motor: %x \n", nmotor );
sebasmartinez 0:73e805ecf765 152 char grados = buffer[3];
sebasmartinez 0:73e805ecf765 153 printf("Grados: %x \n", grados );
sebasmartinez 0:73e805ecf765 154 //char endc = buffer[4];
sebasmartinez 0:73e805ecf765 155 //printf("Cierre: %c \n", endc );
sebasmartinez 0:73e805ecf765 156 mover_ser(nmotor,grados);
sebasmartinez 0:73e805ecf765 157
sebasmartinez 0:73e805ecf765 158 debug_m("fin del comado mover..\n");
sebasmartinez 0:73e805ecf765 159
sebasmartinez 0:73e805ecf765 160 }
sebasmartinez 0:73e805ecf765 161 void pares(){
sebasmartinez 0:73e805ecf765 162 debug_m("se inicia el comado pares..\n");
sebasmartinez 0:73e805ecf765 163
sebasmartinez 0:73e805ecf765 164 char par = buffer[2];
sebasmartinez 0:73e805ecf765 165 printf("Par: %x \n", par );
sebasmartinez 0:73e805ecf765 166 char dire = buffer[3];
sebasmartinez 0:73e805ecf765 167 printf("Dir: %x \n", dire );
sebasmartinez 0:73e805ecf765 168 //char endcc = buffer[4];
sebasmartinez 0:73e805ecf765 169 //printf("Cierre: %x \n", endcc );
sebasmartinez 0:73e805ecf765 170 while(button == 1) { // 433 0 // 446 1
sebasmartinez 0:73e805ecf765 171 mover_par(par,dire);
sebasmartinez 0:73e805ecf765 172 }
sebasmartinez 0:73e805ecf765 173 centro(par);
sebasmartinez 0:73e805ecf765 174 debug_m("fin del comado pares..\n");
sebasmartinez 0:73e805ecf765 175 }
sebasmartinez 0:73e805ecf765 176
sebasmartinez 0:73e805ecf765 177 void caminar(){
sebasmartinez 0:73e805ecf765 178 debug_m("Caminando..\n");
sebasmartinez 0:73e805ecf765 179
sebasmartinez 0:73e805ecf765 180 char col = buffer[2];
sebasmartinez 0:73e805ecf765 181 printf("Color: %x \n", col );
sebasmartinez 0:73e805ecf765 182 char acc = buffer[3];
sebasmartinez 0:73e805ecf765 183 printf("Accion: %x \n", acc );
sebasmartinez 0:73e805ecf765 184 if (buffer[2] > 0x00 && buffer[2] < 0x06){
sebasmartinez 0:73e805ecf765 185 int salir = 0;
sebasmartinez 0:73e805ecf765 186 while(salir == 0){
sebasmartinez 0:73e805ecf765 187 if (color != buffer[2]){
sebasmartinez 0:73e805ecf765 188 command.printf("Programa Color \n");
sebasmartinez 0:73e805ecf765 189 leer:
sebasmartinez 0:73e805ecf765 190 color = leer_color();
sebasmartinez 0:73e805ecf765 191 command.printf("Color encontrado %i \n",color);
sebasmartinez 0:73e805ecf765 192 if(color == buffer[2]){
sebasmartinez 0:73e805ecf765 193 if(color > 0){
sebasmartinez 0:73e805ecf765 194 command.printf("Color Aceptado %i \n",color);
sebasmartinez 0:73e805ecf765 195 command.printf("Ejecutando Accion %x \n", acc );
sebasmartinez 0:73e805ecf765 196 switch ( buffer[3] ) {
sebasmartinez 0:73e805ecf765 197
sebasmartinez 0:73e805ecf765 198 case 0x01: mover_ser(0x01,0x00); break;
sebasmartinez 0:73e805ecf765 199 case 0x02: mover_ser(0x02,0x00);break;
sebasmartinez 0:73e805ecf765 200 case 0x03: mover_ser(0x03,0x00);break;
sebasmartinez 0:73e805ecf765 201 case 0x04: mover_ser(0x04,0x00);break;
sebasmartinez 0:73e805ecf765 202 case 0x05: mover_ser(0x01,0xB4);break;
sebasmartinez 0:73e805ecf765 203 case 0x06: mover_ser(0x02,0xB4);break;
sebasmartinez 0:73e805ecf765 204 case 0x07: mover_ser(0x03,0xB4);break;
sebasmartinez 0:73e805ecf765 205 case 0x08: mover_ser(0x04,0x4B);break;
sebasmartinez 0:73e805ecf765 206 case 0x09: mover_ser(0x01,0x00);break;
sebasmartinez 0:73e805ecf765 207 case 0x10: mover_ser(0x02,0x00);break;
sebasmartinez 0:73e805ecf765 208 case 0x11: mover_ser(0x03,0x00);break;
sebasmartinez 0:73e805ecf765 209 case 0x12: mover_ser(0x04,0x00);break;
sebasmartinez 0:73e805ecf765 210 case 0x13: mover_ser(0x01,0xB4);break;
sebasmartinez 0:73e805ecf765 211 case 0x14: mover_ser(0x02,0xB4);break;
sebasmartinez 0:73e805ecf765 212 case 0x15: mover_ser(0x03,0xB4);break;
sebasmartinez 0:73e805ecf765 213 case 0x16: mover_ser(0x04,0xB4);break;
sebasmartinez 0:73e805ecf765 214 default: break;
sebasmartinez 0:73e805ecf765 215 }
sebasmartinez 0:73e805ecf765 216
sebasmartinez 0:73e805ecf765 217
sebasmartinez 0:73e805ecf765 218
sebasmartinez 0:73e805ecf765 219
sebasmartinez 0:73e805ecf765 220 salir = 1;
sebasmartinez 0:73e805ecf765 221 }
sebasmartinez 0:73e805ecf765 222 }else{
sebasmartinez 0:73e805ecf765 223 goto leer;
sebasmartinez 0:73e805ecf765 224 }
sebasmartinez 0:73e805ecf765 225 }
sebasmartinez 0:73e805ecf765 226 }
sebasmartinez 0:73e805ecf765 227 /*
sebasmartinez 0:73e805ecf765 228 while(button == 0) {
sebasmartinez 0:73e805ecf765 229 mover_par(par,dire);
sebasmartinez 0:73e805ecf765 230 }
sebasmartinez 0:73e805ecf765 231 centro(par);
sebasmartinez 0:73e805ecf765 232 */
sebasmartinez 0:73e805ecf765 233 debug_m("Stop..\n"); }else{
sebasmartinez 0:73e805ecf765 234 debug_m("Fuera de rango color..\n");
sebasmartinez 0:73e805ecf765 235 }
sebasmartinez 0:73e805ecf765 236 }
sebasmartinez 0:73e805ecf765 237
sebasmartinez 0:73e805ecf765 238 unsigned int leer_color(void){
sebasmartinez 0:73e805ecf765 239 //Variables de Lectura
sebasmartinez 0:73e805ecf765 240 unsigned int Red = 0;
sebasmartinez 0:73e805ecf765 241 unsigned int Blue = 0;
sebasmartinez 0:73e805ecf765 242 unsigned int Green = 0;
sebasmartinez 0:73e805ecf765 243 int salida = 0;
sebasmartinez 0:73e805ecf765 244 while(salida == 0){
sebasmartinez 0:73e805ecf765 245 S0 = 1;
sebasmartinez 0:73e805ecf765 246 S1 = 1;
sebasmartinez 0:73e805ecf765 247 //Leer Rojo
sebasmartinez 0:73e805ecf765 248 S2 = 0;
sebasmartinez 0:73e805ecf765 249 S3 = 0;
sebasmartinez 0:73e805ecf765 250 Red = lectura();
sebasmartinez 0:73e805ecf765 251 //Leer Azul
sebasmartinez 0:73e805ecf765 252 S2 = 0;
sebasmartinez 0:73e805ecf765 253 S3 = 1;
sebasmartinez 0:73e805ecf765 254 Blue = lectura();
sebasmartinez 0:73e805ecf765 255 //Leer Verde
sebasmartinez 0:73e805ecf765 256 S2 = 1;
sebasmartinez 0:73e805ecf765 257 S3 = 1;
sebasmartinez 0:73e805ecf765 258 Green = lectura();
sebasmartinez 0:73e805ecf765 259
sebasmartinez 0:73e805ecf765 260 command.printf("Red: %d, Green: %d, Blue: %d ",Red,Green,Blue);
sebasmartinez 0:73e805ecf765 261 wait(TIEMPO_SIZE);
sebasmartinez 0:73e805ecf765 262 /*
sebasmartinez 0:73e805ecf765 263 if( Red <= 9 && Red >= 5 && Green <= 7 && Green >= 3 && Blue <= 9 && Blue >= 6 ){ // Limite Inferior && Limite Superior por R G B
sebasmartinez 0:73e805ecf765 264 //ROJO
sebasmartinez 0:73e805ecf765 265 Rojo++;
sebasmartinez 0:73e805ecf765 266 command.printf("R++ %i ",Rojo);
sebasmartinez 0:73e805ecf765 267
sebasmartinez 0:73e805ecf765 268 }else if( Red <= 51 && Red >= 45 && Green <= 6 && Green >= 3 && Blue <= 51 && Blue >= 48 ){
sebasmartinez 0:73e805ecf765 269 //AZUL
sebasmartinez 0:73e805ecf765 270 Azul++;
sebasmartinez 0:73e805ecf765 271 command.printf("Az++ %i ",Azul);
sebasmartinez 0:73e805ecf765 272
sebasmartinez 0:73e805ecf765 273 }else if( Red <= 29 && Red >= 23 && Green <= 6 && Green >= 3 && Blue <= 30 && Blue >= 23 ){
sebasmartinez 0:73e805ecf765 274 //VERDE
sebasmartinez 0:73e805ecf765 275 Verde++;
sebasmartinez 0:73e805ecf765 276 command.printf("V++ %i ",Verde);
sebasmartinez 0:73e805ecf765 277
sebasmartinez 0:73e805ecf765 278 }else if( Red <= 6 && Red >= 4 && Green <= 4 && Green >= 2 && Blue <= 7 && Blue >= 4 ){
sebasmartinez 0:73e805ecf765 279 //AMARILLO
sebasmartinez 0:73e805ecf765 280 Amarillo++;
sebasmartinez 0:73e805ecf765 281 command.printf("Am++ %i ",Amarillo);
sebasmartinez 0:73e805ecf765 282
sebasmartinez 0:73e805ecf765 283 }else if( Red <= 67 && Red >= 63 && Green <= 18 && Green >= 15 && Blue <= 68 && Blue >= 64 ){
sebasmartinez 0:73e805ecf765 284 //NEGRO
sebasmartinez 0:73e805ecf765 285 Negro++;
sebasmartinez 0:73e805ecf765 286 command.printf("N++ %i ",Negro);
sebasmartinez 0:73e805ecf765 287
sebasmartinez 0:73e805ecf765 288 }else if( Red <= 1 && Red >= 1 && Green <= 18 && Green >= 15 && Blue <= 68 && Blue >= 64 ){
sebasmartinez 0:73e805ecf765 289 //BLANCO
sebasmartinez 0:73e805ecf765 290 Blanco++;
sebasmartinez 0:73e805ecf765 291 command.printf("B++ %i ",Blanco);
sebasmartinez 0:73e805ecf765 292
sebasmartinez 0:73e805ecf765 293 }else{
sebasmartinez 0:73e805ecf765 294 //INDEFINIDO
sebasmartinez 0:73e805ecf765 295 command.printf("Nada ");
sebasmartinez 0:73e805ecf765 296
sebasmartinez 0:73e805ecf765 297 }
sebasmartinez 0:73e805ecf765 298 */
sebasmartinez 0:73e805ecf765 299 if( Red <= 9 && Red >= 5 && Green <= 38 && Green >= 25 && Blue <= 25 && Blue >= 18 ){ // Limite Inferior && Limite Superior por R G B
sebasmartinez 0:73e805ecf765 300 //ROJO
sebasmartinez 0:73e805ecf765 301 Rojo++;
sebasmartinez 0:73e805ecf765 302 command.printf("R++ %i ",Rojo);
sebasmartinez 0:73e805ecf765 303
sebasmartinez 0:73e805ecf765 304 }else if( Red <= 46 && Red >= 35 && Green <= 25 && Green >= 18 && Blue <= 11 && Blue >= 5 ){
sebasmartinez 0:73e805ecf765 305 //AZUL
sebasmartinez 0:73e805ecf765 306 Azul++;
sebasmartinez 0:73e805ecf765 307 command.printf("Az++ %i ",Azul);
sebasmartinez 0:73e805ecf765 308
sebasmartinez 0:73e805ecf765 309 }else if( Red <= 27 && Red >= 20 && Green <= 12 && Green >= 8 && Blue <= 15 && Blue >= 10 ){
sebasmartinez 0:73e805ecf765 310 //VERDE
sebasmartinez 0:73e805ecf765 311 Verde++;
sebasmartinez 0:73e805ecf765 312 command.printf("V++ %i ",Verde);
sebasmartinez 0:73e805ecf765 313
sebasmartinez 0:73e805ecf765 314 }else if( Red <= 10 && Red >= 3 && Green <= 10 && Green >= 4 && Blue <= 13 && Blue >= 10 ){
sebasmartinez 0:73e805ecf765 315 //AMARILLO
sebasmartinez 0:73e805ecf765 316 Amarillo++;
sebasmartinez 0:73e805ecf765 317 command.printf("Am++ %i ",Amarillo);
sebasmartinez 0:73e805ecf765 318
sebasmartinez 0:73e805ecf765 319 }else if( Red <= 66 && Red >= 50 && Green <= 65 && Green >= 60 && Blue <= 45 && Blue >= 40 ){
sebasmartinez 0:73e805ecf765 320 //NEGRO
sebasmartinez 0:73e805ecf765 321 Negro++;
sebasmartinez 0:73e805ecf765 322 command.printf("N++ %i ",Negro);
sebasmartinez 0:73e805ecf765 323
sebasmartinez 0:73e805ecf765 324 }else if( Red <= 6 && Red >= 3 && Green <= 6 && Green >= 3 && Blue <= 6 && Blue >= 3 ){
sebasmartinez 0:73e805ecf765 325 //BLANCO
sebasmartinez 0:73e805ecf765 326 Blanco++;
sebasmartinez 0:73e805ecf765 327 command.printf("B++ %i ",Blanco);
sebasmartinez 0:73e805ecf765 328
sebasmartinez 0:73e805ecf765 329 }else{
sebasmartinez 0:73e805ecf765 330 //INDEFINIDO
sebasmartinez 0:73e805ecf765 331 command.printf("Nada ");
sebasmartinez 0:73e805ecf765 332
sebasmartinez 0:73e805ecf765 333 }
sebasmartinez 0:73e805ecf765 334 if( Rojo > COLOR_SIZE ){ //
sebasmartinez 0:73e805ecf765 335 //ROJO
sebasmartinez 0:73e805ecf765 336 command.printf("Rojo\n");
sebasmartinez 0:73e805ecf765 337 borrar();
sebasmartinez 0:73e805ecf765 338 salida = 1;
sebasmartinez 0:73e805ecf765 339 }else if( Azul > COLOR_SIZE ){
sebasmartinez 0:73e805ecf765 340 //AZUL
sebasmartinez 0:73e805ecf765 341 command.printf("Azul\n");
sebasmartinez 0:73e805ecf765 342 borrar();
sebasmartinez 0:73e805ecf765 343 salida = 2;
sebasmartinez 0:73e805ecf765 344 }else if( Verde > COLOR_SIZE ){
sebasmartinez 0:73e805ecf765 345 //VERDE
sebasmartinez 0:73e805ecf765 346 command.printf("Verde\n");
sebasmartinez 0:73e805ecf765 347 borrar();
sebasmartinez 0:73e805ecf765 348 salida = 3;
sebasmartinez 0:73e805ecf765 349 }else if( Amarillo > COLOR_SIZE ){
sebasmartinez 0:73e805ecf765 350 //AMARILLO
sebasmartinez 0:73e805ecf765 351 command.printf("Amarillo\n");
sebasmartinez 0:73e805ecf765 352 borrar();
sebasmartinez 0:73e805ecf765 353 salida = 4;
sebasmartinez 0:73e805ecf765 354 }else if( Negro > COLOR_SIZE ){
sebasmartinez 0:73e805ecf765 355 //NEGRO
sebasmartinez 0:73e805ecf765 356 command.printf("Negro\n");
sebasmartinez 0:73e805ecf765 357 borrar();
sebasmartinez 0:73e805ecf765 358 salida = 5;
sebasmartinez 0:73e805ecf765 359 }else if( Blanco > COLOR_SIZE ){
sebasmartinez 0:73e805ecf765 360 //BLANCO
sebasmartinez 0:73e805ecf765 361 command.printf("Blanco\n");
sebasmartinez 0:73e805ecf765 362 borrar();
sebasmartinez 0:73e805ecf765 363 salida = 6;
sebasmartinez 0:73e805ecf765 364 }else{
sebasmartinez 0:73e805ecf765 365 //INDEFINIDO
sebasmartinez 0:73e805ecf765 366 command.printf("Contando \n");
sebasmartinez 0:73e805ecf765 367 }
sebasmartinez 0:73e805ecf765 368
sebasmartinez 0:73e805ecf765 369 }
sebasmartinez 0:73e805ecf765 370 //command.printf("Color encontrado %i \n",salida);
sebasmartinez 0:73e805ecf765 371 return salida;
sebasmartinez 0:73e805ecf765 372 }
sebasmartinez 0:73e805ecf765 373
sebasmartinez 0:73e805ecf765 374 unsigned int lectura(void){
sebasmartinez 0:73e805ecf765 375 unsigned int Resultado = 0;
sebasmartinez 0:73e805ecf765 376 while(OUT){}
sebasmartinez 0:73e805ecf765 377 while(!OUT){}
sebasmartinez 0:73e805ecf765 378 while(OUT){}
sebasmartinez 0:73e805ecf765 379 tiempo.start();
sebasmartinez 0:73e805ecf765 380 while(!OUT){}
sebasmartinez 0:73e805ecf765 381 tiempo.stop();
sebasmartinez 0:73e805ecf765 382 Resultado = tiempo.read_us();
sebasmartinez 0:73e805ecf765 383 tiempo.reset();
sebasmartinez 0:73e805ecf765 384 return(Resultado);
sebasmartinez 0:73e805ecf765 385 }
sebasmartinez 0:73e805ecf765 386 void borrar(void){
sebasmartinez 0:73e805ecf765 387
sebasmartinez 0:73e805ecf765 388 Rojo = 0;
sebasmartinez 0:73e805ecf765 389 Azul = 0;
sebasmartinez 0:73e805ecf765 390 Verde = 0;
sebasmartinez 0:73e805ecf765 391 Amarillo = 0;
sebasmartinez 0:73e805ecf765 392 Negro = 0;
sebasmartinez 0:73e805ecf765 393 Blanco = 0;
sebasmartinez 0:73e805ecf765 394
sebasmartinez 0:73e805ecf765 395 return;
sebasmartinez 0:73e805ecf765 396 }
sebasmartinez 0:73e805ecf765 397
sebasmartinez 0:73e805ecf765 398 void debug_m(char *s , ... ){
sebasmartinez 0:73e805ecf765 399 #if DEBUG
sebasmartinez 0:73e805ecf765 400 command.printf(s);
sebasmartinez 0:73e805ecf765 401 #endif
sebasmartinez 0:73e805ecf765 402 }