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@1:59be7d7e1b54, 2018-11-20 (annotated)

Committer:
sebasmartinez
Date:
Tue Nov 20 01:48:09 2018 +0000
Revision:
1:59be7d7e1b54
Parent:
0:73e805ecf765 
Programa_final;

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 1:59be7d7e1b54 6 #define COLOR_SIZE 1 // cantidad de lecturas iguales
sebasmartinez 1:59be7d7e1b54 7 #define TIEMPO_SIZE 0.1 //tiempo de captura color
sebasmartinez 0:73e805ecf765 8 MEM_TYPE buffer[MEM_SIZE];
sebasmartinez 0:73e805ecf765 9 Serial command(USBTX, USBRX); // Usar Serial PC USB
sebasmartinez 1:59be7d7e1b54 10 Serial Bluetooth(PC_10,PC_11); //Tx, RX. Usar Bluetooth HC-06.
sebasmartinez 1:59be7d7e1b54 11 DigitalOut Led(LED1); // digital output
sebasmartinez 1:59be7d7e1b54 12 DigitalIn button(USER_BUTTON);
sebasmartinez 1:59be7d7e1b54 13 void Rx_interrupt();
sebasmartinez 1:59be7d7e1b54 14 void Rx_interruptBT();
sebasmartinez 0:73e805ecf765 15
sebasmartinez 0:73e805ecf765 16 /* //Puertos Sensor color
sebasmartinez 0:73e805ecf765 17 //433
sebasmartinez 0:73e805ecf765 18 DigitalIn OUT(PC_0);//A5 BC1
sebasmartinez 0:73e805ecf765 19 DigitalOut S2(PC_1);//A4 VD1
sebasmartinez 0:73e805ecf765 20 DigitalOut S3(PC_2);//A3 NG
sebasmartinez 0:73e805ecf765 21 DigitalOut S1(PC_3);//A2 BC2
sebasmartinez 0:73e805ecf765 22 DigitalOut S0(PA_1);//A1 VD2
sebasmartinez 0:73e805ecf765 23 */
sebasmartinez 0:73e805ecf765 24 //446
sebasmartinez 0:73e805ecf765 25 DigitalIn OUT(PC_0);//A5 BC1
sebasmartinez 0:73e805ecf765 26 DigitalOut S2(PC_1);//A4 VD1
sebasmartinez 0:73e805ecf765 27 DigitalOut S3(PB_0);//A3 NG
sebasmartinez 0:73e805ecf765 28 DigitalOut S1(PA_4);//A2 BC2
sebasmartinez 0:73e805ecf765 29 DigitalOut S0(PA_1);//A1 VD2
sebasmartinez 0:73e805ecf765 30 //
sebasmartinez 0:73e805ecf765 31 Timer tiempo;
sebasmartinez 0:73e805ecf765 32 //Variables de Trabajo Colores identificados
sebasmartinez 0:73e805ecf765 33 int Rojo = 0;
sebasmartinez 0:73e805ecf765 34 int Azul = 0;
sebasmartinez 0:73e805ecf765 35 int Verde = 0;
sebasmartinez 0:73e805ecf765 36 int Amarillo = 0;
sebasmartinez 0:73e805ecf765 37 int Negro = 0;
sebasmartinez 0:73e805ecf765 38 int Blanco = 0;
sebasmartinez 0:73e805ecf765 39 int color = 0;
sebasmartinez 0:73e805ecf765 40 // color
sebasmartinez 1:59be7d7e1b54 41
sebasmartinez 0:73e805ecf765 42 int main() {
sebasmartinez 1:59be7d7e1b54 43 //command.baud(9600);
sebasmartinez 1:59be7d7e1b54 44 //Bluetooth.baud(9600);
sebasmartinez 0:73e805ecf765 45 init_serial();
sebasmartinez 1:59be7d7e1b54 46 // Setup a serial interrupt function to receive data
sebasmartinez 1:59be7d7e1b54 47 command.attach(&Rx_interrupt, Serial::RxIrq);
sebasmartinez 1:59be7d7e1b54 48 Bluetooth.attach(&Rx_interruptBT, Serial::RxIrq);
sebasmartinez 1:59be7d7e1b54 49 init_servo();
sebasmartinez 1:59be7d7e1b54 50
sebasmartinez 0:73e805ecf765 51 debug_m("inicio \n");
sebasmartinez 1:59be7d7e1b54 52
sebasmartinez 0:73e805ecf765 53 while(1)
sebasmartinez 0:73e805ecf765 54 {
sebasmartinez 1:59be7d7e1b54 55 //caminar(7);
sebasmartinez 1:59be7d7e1b54 56
sebasmartinez 1:59be7d7e1b54 57 //activar en produccion
sebasmartinez 1:59be7d7e1b54 58 int action = leer_color();
sebasmartinez 1:59be7d7e1b54 59 switch ( action ) {
sebasmartinez 1:59be7d7e1b54 60
sebasmartinez 1:59be7d7e1b54 61 case 1: //Gira Derecha
sebasmartinez 1:59be7d7e1b54 62 caminar(action);
sebasmartinez 1:59be7d7e1b54 63 break;
sebasmartinez 1:59be7d7e1b54 64 case 2: //Gira Izq
sebasmartinez 1:59be7d7e1b54 65 caminar(action);
sebasmartinez 1:59be7d7e1b54 66 break;
sebasmartinez 1:59be7d7e1b54 67 case 3: //Adelante
sebasmartinez 1:59be7d7e1b54 68 caminar(action);
sebasmartinez 1:59be7d7e1b54 69 break;
sebasmartinez 1:59be7d7e1b54 70 case 4: //atras
sebasmartinez 1:59be7d7e1b54 71 caminar(action);
sebasmartinez 1:59be7d7e1b54 72 break;
sebasmartinez 1:59be7d7e1b54 73 case 5: //Saludar 1
sebasmartinez 1:59be7d7e1b54 74 caminar(action);
sebasmartinez 1:59be7d7e1b54 75 break;
sebasmartinez 1:59be7d7e1b54 76 case 6: //Saludar 2
sebasmartinez 1:59be7d7e1b54 77 caminar(action);
sebasmartinez 1:59be7d7e1b54 78 break;
sebasmartinez 1:59be7d7e1b54 79 default: break;
sebasmartinez 1:59be7d7e1b54 80 }
sebasmartinez 0:73e805ecf765 81 }
sebasmartinez 0:73e805ecf765 82 }
sebasmartinez 0:73e805ecf765 83
sebasmartinez 1:59be7d7e1b54 84 /*
sebasmartinez 0:73e805ecf765 85
sebasmartinez 0:73e805ecf765 86 uint32_t read_command()
sebasmartinez 0:73e805ecf765 87 {
sebasmartinez 0:73e805ecf765 88 // retorna los byte recibidos concatenados en un entero,
sebasmartinez 0:73e805ecf765 89 // Formato | FF | TC | NM | GR | FD
sebasmartinez 0:73e805ecf765 90 // Datos | Inicio | Tele-comando | Numero Motor | Grados | Cierre
sebasmartinez 0:73e805ecf765 91 //char intc=command.getc();
sebasmartinez 1:59be7d7e1b54 92 debug_m("inicio matriz\n");
sebasmartinez 0:73e805ecf765 93 buffer[0] = command.getc();
sebasmartinez 1:59be7d7e1b54 94 debug_m("comando matriz\n");
sebasmartinez 0:73e805ecf765 95 inicio:
sebasmartinez 0:73e805ecf765 96
sebasmartinez 0:73e805ecf765 97 while(buffer[0] != 0xFF ){ //7B y 7D son { y }
sebasmartinez 0:73e805ecf765 98
sebasmartinez 0:73e805ecf765 99 buffer[0] = command.getc();
sebasmartinez 0:73e805ecf765 100
sebasmartinez 0:73e805ecf765 101 debug_m("comando inicio invalido \n");
sebasmartinez 0:73e805ecf765 102 }
sebasmartinez 0:73e805ecf765 103 debug_m("comando inicio Valido \n");
sebasmartinez 0:73e805ecf765 104 char ini = buffer[0];
sebasmartinez 0:73e805ecf765 105 printf("inicio encontrado: %x \n", ini);
sebasmartinez 0:73e805ecf765 106
sebasmartinez 0:73e805ecf765 107 buffer[1] = command.getc();
sebasmartinez 0:73e805ecf765 108 if (buffer[1] != 0xFF && buffer[1] != 0xFD){
sebasmartinez 0:73e805ecf765 109 buffer[2] = command.getc();
sebasmartinez 0:73e805ecf765 110 }else{
sebasmartinez 0:73e805ecf765 111 buffer[0] = buffer[1];
sebasmartinez 0:73e805ecf765 112 goto inicio;
sebasmartinez 0:73e805ecf765 113 }
sebasmartinez 0:73e805ecf765 114 if (buffer[2] != 0xFF && buffer[2] != 0xFD){
sebasmartinez 0:73e805ecf765 115 buffer[3] = command.getc();
sebasmartinez 0:73e805ecf765 116 }else{
sebasmartinez 0:73e805ecf765 117 buffer[0] = buffer[2];
sebasmartinez 0:73e805ecf765 118 goto inicio;
sebasmartinez 0:73e805ecf765 119 }
sebasmartinez 0:73e805ecf765 120 if (buffer[3] != 0xFF && buffer[3] != 0xFD){
sebasmartinez 0:73e805ecf765 121 buffer[4] = command.getc();
sebasmartinez 0:73e805ecf765 122 }else{
sebasmartinez 0:73e805ecf765 123 buffer[0] = buffer[3];
sebasmartinez 0:73e805ecf765 124 goto inicio;
sebasmartinez 0:73e805ecf765 125 }
sebasmartinez 0:73e805ecf765 126 //para que evalue el cierre
sebasmartinez 0:73e805ecf765 127 if (buffer[4] != 0xFF && buffer[4] == 0xFD){
sebasmartinez 0:73e805ecf765 128 debug_m("comando Cierre valido \n");
sebasmartinez 0:73e805ecf765 129 }else{
sebasmartinez 0:73e805ecf765 130 buffer[0] = buffer[4];
sebasmartinez 0:73e805ecf765 131 goto inicio;
sebasmartinez 0:73e805ecf765 132 }
sebasmartinez 0:73e805ecf765 133
sebasmartinez 0:73e805ecf765 134
sebasmartinez 0:73e805ecf765 135
sebasmartinez 0:73e805ecf765 136 //intc=command.getc();
sebasmartinez 0:73e805ecf765 137 int tele = buffer[1];
sebasmartinez 0:73e805ecf765 138 char telec = buffer[1];
sebasmartinez 0:73e805ecf765 139 printf("Tele comando: %x \n", telec );
sebasmartinez 0:73e805ecf765 140
sebasmartinez 0:73e805ecf765 141
sebasmartinez 0:73e805ecf765 142 return tele;
sebasmartinez 0:73e805ecf765 143
sebasmartinez 0:73e805ecf765 144 }
sebasmartinez 1:59be7d7e1b54 145 */
sebasmartinez 1:59be7d7e1b54 146 uint32_t read_command2()
sebasmartinez 1:59be7d7e1b54 147 {
sebasmartinez 1:59be7d7e1b54 148 // retorna los byte recibidos concatenados en un entero,
sebasmartinez 1:59be7d7e1b54 149 // Formato | FF | TC | NM | GR | FD
sebasmartinez 1:59be7d7e1b54 150 // Datos | Inicio | Tele-comando | Numero Motor | Grados | Cierre
sebasmartinez 1:59be7d7e1b54 151 inicio2:
sebasmartinez 1:59be7d7e1b54 152 debug_m("inicio matriz2\n");
sebasmartinez 1:59be7d7e1b54 153 buffer[0] = Bluetooth.getc();
sebasmartinez 1:59be7d7e1b54 154 debug_m("comando matriz2\n");
sebasmartinez 1:59be7d7e1b54 155
sebasmartinez 1:59be7d7e1b54 156
sebasmartinez 1:59be7d7e1b54 157 while(buffer[0] != 0xFF ){ //7B y 7D son { y }
sebasmartinez 1:59be7d7e1b54 158
sebasmartinez 1:59be7d7e1b54 159 buffer[0] = Bluetooth.getc();
sebasmartinez 1:59be7d7e1b54 160 debug_m("P1: %x \n", buffer[0]);
sebasmartinez 1:59be7d7e1b54 161 debug_m("comando inicio invalido \n");
sebasmartinez 1:59be7d7e1b54 162 }
sebasmartinez 1:59be7d7e1b54 163 debug_m("comando inicio Valido \n");
sebasmartinez 1:59be7d7e1b54 164 char ini = buffer[0];
sebasmartinez 1:59be7d7e1b54 165 debug_m("inicio encontrado: %x \n", ini);
sebasmartinez 1:59be7d7e1b54 166
sebasmartinez 1:59be7d7e1b54 167 buffer[1] = Bluetooth.getc();
sebasmartinez 1:59be7d7e1b54 168 debug_m("P2: %x \n", buffer[1]);
sebasmartinez 1:59be7d7e1b54 169 if (buffer[1] != 0xFF && buffer[1] != 0xFD){
sebasmartinez 1:59be7d7e1b54 170 buffer[2] = Bluetooth.getc();
sebasmartinez 1:59be7d7e1b54 171 }else{
sebasmartinez 1:59be7d7e1b54 172 buffer[0] = buffer[1];
sebasmartinez 1:59be7d7e1b54 173 goto inicio2;
sebasmartinez 1:59be7d7e1b54 174 }
sebasmartinez 1:59be7d7e1b54 175 debug_m("P3: %x \n", buffer[2]);
sebasmartinez 1:59be7d7e1b54 176 if (buffer[2] != 0xFF && buffer[2] != 0xFD){
sebasmartinez 1:59be7d7e1b54 177 buffer[3] = Bluetooth.getc();
sebasmartinez 1:59be7d7e1b54 178 }else{
sebasmartinez 1:59be7d7e1b54 179 buffer[0] = buffer[2];
sebasmartinez 1:59be7d7e1b54 180 goto inicio2;
sebasmartinez 1:59be7d7e1b54 181 }
sebasmartinez 1:59be7d7e1b54 182 debug_m("P4: %x \n", buffer[3]);
sebasmartinez 1:59be7d7e1b54 183 if (buffer[3] != 0xFF && buffer[3] != 0xFD){
sebasmartinez 1:59be7d7e1b54 184 buffer[4] = Bluetooth.getc();
sebasmartinez 1:59be7d7e1b54 185 }else{
sebasmartinez 1:59be7d7e1b54 186 buffer[0] = buffer[3];
sebasmartinez 1:59be7d7e1b54 187 goto inicio2;
sebasmartinez 1:59be7d7e1b54 188 }
sebasmartinez 1:59be7d7e1b54 189 //para que evalue el cierre
sebasmartinez 1:59be7d7e1b54 190 debug_m("P5: %x \n", buffer[4]);
sebasmartinez 1:59be7d7e1b54 191 if (buffer[4] != 0xFF && buffer[4] == 0xFD){
sebasmartinez 1:59be7d7e1b54 192 debug_m("comando Cierre valido \n");
sebasmartinez 1:59be7d7e1b54 193 }else{
sebasmartinez 1:59be7d7e1b54 194 buffer[0] = buffer[4];
sebasmartinez 1:59be7d7e1b54 195 goto inicio2;
sebasmartinez 1:59be7d7e1b54 196 }
sebasmartinez 1:59be7d7e1b54 197
sebasmartinez 1:59be7d7e1b54 198
sebasmartinez 1:59be7d7e1b54 199 int tele = buffer[1];
sebasmartinez 1:59be7d7e1b54 200 char telec = buffer[1];
sebasmartinez 1:59be7d7e1b54 201 printf("Tele comando: %x \n", telec );
sebasmartinez 0:73e805ecf765 202
sebasmartinez 0:73e805ecf765 203
sebasmartinez 1:59be7d7e1b54 204 return tele;
sebasmartinez 1:59be7d7e1b54 205
sebasmartinez 1:59be7d7e1b54 206 }
sebasmartinez 0:73e805ecf765 207 void init_serial()
sebasmartinez 0:73e805ecf765 208 {
sebasmartinez 0:73e805ecf765 209 command.baud(9600);
sebasmartinez 1:59be7d7e1b54 210 Bluetooth.baud(9600);
sebasmartinez 0:73e805ecf765 211 debug_m("Serial 9600 \n");
sebasmartinez 0:73e805ecf765 212 }
sebasmartinez 0:73e805ecf765 213
sebasmartinez 0:73e805ecf765 214
sebasmartinez 0:73e805ecf765 215 void moving(){
sebasmartinez 0:73e805ecf765 216 debug_m("se inicia el comado mover..\n");
sebasmartinez 0:73e805ecf765 217
sebasmartinez 0:73e805ecf765 218 char nmotor = buffer[2];
sebasmartinez 0:73e805ecf765 219 printf("Motor: %x \n", nmotor );
sebasmartinez 0:73e805ecf765 220 char grados = buffer[3];
sebasmartinez 0:73e805ecf765 221 printf("Grados: %x \n", grados );
sebasmartinez 0:73e805ecf765 222 //char endc = buffer[4];
sebasmartinez 0:73e805ecf765 223 //printf("Cierre: %c \n", endc );
sebasmartinez 0:73e805ecf765 224 mover_ser(nmotor,grados);
sebasmartinez 0:73e805ecf765 225
sebasmartinez 0:73e805ecf765 226 debug_m("fin del comado mover..\n");
sebasmartinez 0:73e805ecf765 227
sebasmartinez 0:73e805ecf765 228 }
sebasmartinez 0:73e805ecf765 229 void pares(){
sebasmartinez 0:73e805ecf765 230 debug_m("se inicia el comado pares..\n");
sebasmartinez 0:73e805ecf765 231
sebasmartinez 0:73e805ecf765 232 char par = buffer[2];
sebasmartinez 0:73e805ecf765 233 printf("Par: %x \n", par );
sebasmartinez 0:73e805ecf765 234 char dire = buffer[3];
sebasmartinez 0:73e805ecf765 235 printf("Dir: %x \n", dire );
sebasmartinez 0:73e805ecf765 236 //char endcc = buffer[4];
sebasmartinez 0:73e805ecf765 237 //printf("Cierre: %x \n", endcc );
sebasmartinez 1:59be7d7e1b54 238 //while(button == 1) { // 433 0 // 446 1
sebasmartinez 0:73e805ecf765 239 mover_par(par,dire);
sebasmartinez 1:59be7d7e1b54 240 //}
sebasmartinez 1:59be7d7e1b54 241 //centro(par);
sebasmartinez 0:73e805ecf765 242 debug_m("fin del comado pares..\n");
sebasmartinez 0:73e805ecf765 243 }
sebasmartinez 0:73e805ecf765 244
sebasmartinez 1:59be7d7e1b54 245 void caminar(int w){
sebasmartinez 1:59be7d7e1b54 246 debug_m("Caminando..\n");
sebasmartinez 1:59be7d7e1b54 247 switch ( w ) {
sebasmartinez 0:73e805ecf765 248
sebasmartinez 1:59be7d7e1b54 249 case 1: //Gira Derecha
sebasmartinez 1:59be7d7e1b54 250 direccion(0x01);
sebasmartinez 1:59be7d7e1b54 251 break;
sebasmartinez 1:59be7d7e1b54 252 case 2: //Gira Izq
sebasmartinez 1:59be7d7e1b54 253 direccion(0x02);
sebasmartinez 1:59be7d7e1b54 254 break;
sebasmartinez 1:59be7d7e1b54 255 case 3: //Adelante
sebasmartinez 1:59be7d7e1b54 256 direccion(0x03);
sebasmartinez 1:59be7d7e1b54 257 break;
sebasmartinez 1:59be7d7e1b54 258 case 4: //atras
sebasmartinez 1:59be7d7e1b54 259 direccion(0x06);
sebasmartinez 1:59be7d7e1b54 260 break;
sebasmartinez 1:59be7d7e1b54 261 case 5: //Saludar 1
sebasmartinez 1:59be7d7e1b54 262 direccion(0x05);
sebasmartinez 1:59be7d7e1b54 263 break;
sebasmartinez 1:59be7d7e1b54 264 case 6: //Saludar 2
sebasmartinez 1:59be7d7e1b54 265 direccion(0x06);
sebasmartinez 1:59be7d7e1b54 266 break;
sebasmartinez 1:59be7d7e1b54 267 case 7: //pruebas
sebasmartinez 1:59be7d7e1b54 268 direccion(0x07);
sebasmartinez 1:59be7d7e1b54 269 break;
sebasmartinez 1:59be7d7e1b54 270 default: break;
sebasmartinez 1:59be7d7e1b54 271 }
sebasmartinez 1:59be7d7e1b54 272
sebasmartinez 1:59be7d7e1b54 273
sebasmartinez 1:59be7d7e1b54 274
sebasmartinez 1:59be7d7e1b54 275
sebasmartinez 1:59be7d7e1b54 276
sebasmartinez 0:73e805ecf765 277 }
sebasmartinez 0:73e805ecf765 278
sebasmartinez 0:73e805ecf765 279 unsigned int leer_color(void){
sebasmartinez 0:73e805ecf765 280 //Variables de Lectura
sebasmartinez 0:73e805ecf765 281 unsigned int Red = 0;
sebasmartinez 0:73e805ecf765 282 unsigned int Blue = 0;
sebasmartinez 0:73e805ecf765 283 unsigned int Green = 0;
sebasmartinez 0:73e805ecf765 284 int salida = 0;
sebasmartinez 0:73e805ecf765 285 while(salida == 0){
sebasmartinez 0:73e805ecf765 286 S0 = 1;
sebasmartinez 0:73e805ecf765 287 S1 = 1;
sebasmartinez 0:73e805ecf765 288 //Leer Rojo
sebasmartinez 0:73e805ecf765 289 S2 = 0;
sebasmartinez 0:73e805ecf765 290 S3 = 0;
sebasmartinez 0:73e805ecf765 291 Red = lectura();
sebasmartinez 0:73e805ecf765 292 //Leer Azul
sebasmartinez 0:73e805ecf765 293 S2 = 0;
sebasmartinez 0:73e805ecf765 294 S3 = 1;
sebasmartinez 0:73e805ecf765 295 Blue = lectura();
sebasmartinez 0:73e805ecf765 296 //Leer Verde
sebasmartinez 0:73e805ecf765 297 S2 = 1;
sebasmartinez 0:73e805ecf765 298 S3 = 1;
sebasmartinez 0:73e805ecf765 299 Green = lectura();
sebasmartinez 0:73e805ecf765 300
sebasmartinez 0:73e805ecf765 301 command.printf("Red: %d, Green: %d, Blue: %d ",Red,Green,Blue);
sebasmartinez 0:73e805ecf765 302 wait(TIEMPO_SIZE);
sebasmartinez 0:73e805ecf765 303 /*
sebasmartinez 0:73e805ecf765 304 if( Red <= 9 && Red >= 5 && Green <= 7 && Green >= 3 && Blue <= 9 && Blue >= 6 ){ // Limite Inferior && Limite Superior por R G B
sebasmartinez 0:73e805ecf765 305 //ROJO
sebasmartinez 0:73e805ecf765 306 Rojo++;
sebasmartinez 0:73e805ecf765 307 command.printf("R++ %i ",Rojo);
sebasmartinez 0:73e805ecf765 308
sebasmartinez 0:73e805ecf765 309 }else if( Red <= 51 && Red >= 45 && Green <= 6 && Green >= 3 && Blue <= 51 && Blue >= 48 ){
sebasmartinez 0:73e805ecf765 310 //AZUL
sebasmartinez 0:73e805ecf765 311 Azul++;
sebasmartinez 0:73e805ecf765 312 command.printf("Az++ %i ",Azul);
sebasmartinez 0:73e805ecf765 313
sebasmartinez 0:73e805ecf765 314 }else if( Red <= 29 && Red >= 23 && Green <= 6 && Green >= 3 && Blue <= 30 && Blue >= 23 ){
sebasmartinez 0:73e805ecf765 315 //VERDE
sebasmartinez 0:73e805ecf765 316 Verde++;
sebasmartinez 0:73e805ecf765 317 command.printf("V++ %i ",Verde);
sebasmartinez 0:73e805ecf765 318
sebasmartinez 0:73e805ecf765 319 }else if( Red <= 6 && Red >= 4 && Green <= 4 && Green >= 2 && Blue <= 7 && Blue >= 4 ){
sebasmartinez 0:73e805ecf765 320 //AMARILLO
sebasmartinez 0:73e805ecf765 321 Amarillo++;
sebasmartinez 0:73e805ecf765 322 command.printf("Am++ %i ",Amarillo);
sebasmartinez 0:73e805ecf765 323
sebasmartinez 0:73e805ecf765 324 }else if( Red <= 67 && Red >= 63 && Green <= 18 && Green >= 15 && Blue <= 68 && Blue >= 64 ){
sebasmartinez 0:73e805ecf765 325 //NEGRO
sebasmartinez 0:73e805ecf765 326 Negro++;
sebasmartinez 0:73e805ecf765 327 command.printf("N++ %i ",Negro);
sebasmartinez 0:73e805ecf765 328
sebasmartinez 0:73e805ecf765 329 }else if( Red <= 1 && Red >= 1 && Green <= 18 && Green >= 15 && Blue <= 68 && Blue >= 64 ){
sebasmartinez 0:73e805ecf765 330 //BLANCO
sebasmartinez 0:73e805ecf765 331 Blanco++;
sebasmartinez 0:73e805ecf765 332 command.printf("B++ %i ",Blanco);
sebasmartinez 0:73e805ecf765 333
sebasmartinez 0:73e805ecf765 334 }else{
sebasmartinez 0:73e805ecf765 335 //INDEFINIDO
sebasmartinez 0:73e805ecf765 336 command.printf("Nada ");
sebasmartinez 0:73e805ecf765 337
sebasmartinez 0:73e805ecf765 338 }
sebasmartinez 0:73e805ecf765 339 */
sebasmartinez 0:73e805ecf765 340 if( Red <= 9 && Red >= 5 && Green <= 38 && Green >= 25 && Blue <= 25 && Blue >= 18 ){ // Limite Inferior && Limite Superior por R G B
sebasmartinez 0:73e805ecf765 341 //ROJO
sebasmartinez 0:73e805ecf765 342 Rojo++;
sebasmartinez 0:73e805ecf765 343 command.printf("R++ %i ",Rojo);
sebasmartinez 0:73e805ecf765 344
sebasmartinez 0:73e805ecf765 345 }else if( Red <= 46 && Red >= 35 && Green <= 25 && Green >= 18 && Blue <= 11 && Blue >= 5 ){
sebasmartinez 0:73e805ecf765 346 //AZUL
sebasmartinez 0:73e805ecf765 347 Azul++;
sebasmartinez 0:73e805ecf765 348 command.printf("Az++ %i ",Azul);
sebasmartinez 0:73e805ecf765 349
sebasmartinez 0:73e805ecf765 350 }else if( Red <= 27 && Red >= 20 && Green <= 12 && Green >= 8 && Blue <= 15 && Blue >= 10 ){
sebasmartinez 0:73e805ecf765 351 //VERDE
sebasmartinez 0:73e805ecf765 352 Verde++;
sebasmartinez 0:73e805ecf765 353 command.printf("V++ %i ",Verde);
sebasmartinez 0:73e805ecf765 354
sebasmartinez 0:73e805ecf765 355 }else if( Red <= 10 && Red >= 3 && Green <= 10 && Green >= 4 && Blue <= 13 && Blue >= 10 ){
sebasmartinez 0:73e805ecf765 356 //AMARILLO
sebasmartinez 0:73e805ecf765 357 Amarillo++;
sebasmartinez 0:73e805ecf765 358 command.printf("Am++ %i ",Amarillo);
sebasmartinez 0:73e805ecf765 359
sebasmartinez 0:73e805ecf765 360 }else if( Red <= 66 && Red >= 50 && Green <= 65 && Green >= 60 && Blue <= 45 && Blue >= 40 ){
sebasmartinez 0:73e805ecf765 361 //NEGRO
sebasmartinez 0:73e805ecf765 362 Negro++;
sebasmartinez 0:73e805ecf765 363 command.printf("N++ %i ",Negro);
sebasmartinez 0:73e805ecf765 364
sebasmartinez 0:73e805ecf765 365 }else if( Red <= 6 && Red >= 3 && Green <= 6 && Green >= 3 && Blue <= 6 && Blue >= 3 ){
sebasmartinez 0:73e805ecf765 366 //BLANCO
sebasmartinez 0:73e805ecf765 367 Blanco++;
sebasmartinez 0:73e805ecf765 368 command.printf("B++ %i ",Blanco);
sebasmartinez 0:73e805ecf765 369
sebasmartinez 0:73e805ecf765 370 }else{
sebasmartinez 0:73e805ecf765 371 //INDEFINIDO
sebasmartinez 0:73e805ecf765 372 command.printf("Nada ");
sebasmartinez 1:59be7d7e1b54 373
sebasmartinez 0:73e805ecf765 374
sebasmartinez 0:73e805ecf765 375 }
sebasmartinez 0:73e805ecf765 376 if( Rojo > COLOR_SIZE ){ //
sebasmartinez 0:73e805ecf765 377 //ROJO
sebasmartinez 0:73e805ecf765 378 command.printf("Rojo\n");
sebasmartinez 0:73e805ecf765 379 borrar();
sebasmartinez 0:73e805ecf765 380 salida = 1;
sebasmartinez 0:73e805ecf765 381 }else if( Azul > COLOR_SIZE ){
sebasmartinez 0:73e805ecf765 382 //AZUL
sebasmartinez 0:73e805ecf765 383 command.printf("Azul\n");
sebasmartinez 0:73e805ecf765 384 borrar();
sebasmartinez 0:73e805ecf765 385 salida = 2;
sebasmartinez 0:73e805ecf765 386 }else if( Verde > COLOR_SIZE ){
sebasmartinez 0:73e805ecf765 387 //VERDE
sebasmartinez 0:73e805ecf765 388 command.printf("Verde\n");
sebasmartinez 0:73e805ecf765 389 borrar();
sebasmartinez 0:73e805ecf765 390 salida = 3;
sebasmartinez 0:73e805ecf765 391 }else if( Amarillo > COLOR_SIZE ){
sebasmartinez 0:73e805ecf765 392 //AMARILLO
sebasmartinez 0:73e805ecf765 393 command.printf("Amarillo\n");
sebasmartinez 0:73e805ecf765 394 borrar();
sebasmartinez 0:73e805ecf765 395 salida = 4;
sebasmartinez 0:73e805ecf765 396 }else if( Negro > COLOR_SIZE ){
sebasmartinez 0:73e805ecf765 397 //NEGRO
sebasmartinez 0:73e805ecf765 398 command.printf("Negro\n");
sebasmartinez 0:73e805ecf765 399 borrar();
sebasmartinez 0:73e805ecf765 400 salida = 5;
sebasmartinez 0:73e805ecf765 401 }else if( Blanco > COLOR_SIZE ){
sebasmartinez 0:73e805ecf765 402 //BLANCO
sebasmartinez 0:73e805ecf765 403 command.printf("Blanco\n");
sebasmartinez 0:73e805ecf765 404 borrar();
sebasmartinez 0:73e805ecf765 405 salida = 6;
sebasmartinez 0:73e805ecf765 406 }else{
sebasmartinez 0:73e805ecf765 407 //INDEFINIDO
sebasmartinez 0:73e805ecf765 408 command.printf("Contando \n");
sebasmartinez 1:59be7d7e1b54 409
sebasmartinez 0:73e805ecf765 410 }
sebasmartinez 0:73e805ecf765 411
sebasmartinez 0:73e805ecf765 412 }
sebasmartinez 0:73e805ecf765 413 //command.printf("Color encontrado %i \n",salida);
sebasmartinez 0:73e805ecf765 414 return salida;
sebasmartinez 0:73e805ecf765 415 }
sebasmartinez 0:73e805ecf765 416
sebasmartinez 0:73e805ecf765 417 unsigned int lectura(void){
sebasmartinez 0:73e805ecf765 418 unsigned int Resultado = 0;
sebasmartinez 0:73e805ecf765 419 while(OUT){}
sebasmartinez 0:73e805ecf765 420 while(!OUT){}
sebasmartinez 0:73e805ecf765 421 while(OUT){}
sebasmartinez 0:73e805ecf765 422 tiempo.start();
sebasmartinez 0:73e805ecf765 423 while(!OUT){}
sebasmartinez 0:73e805ecf765 424 tiempo.stop();
sebasmartinez 0:73e805ecf765 425 Resultado = tiempo.read_us();
sebasmartinez 0:73e805ecf765 426 tiempo.reset();
sebasmartinez 0:73e805ecf765 427 return(Resultado);
sebasmartinez 0:73e805ecf765 428 }
sebasmartinez 0:73e805ecf765 429 void borrar(void){
sebasmartinez 0:73e805ecf765 430
sebasmartinez 0:73e805ecf765 431 Rojo = 0;
sebasmartinez 0:73e805ecf765 432 Azul = 0;
sebasmartinez 0:73e805ecf765 433 Verde = 0;
sebasmartinez 0:73e805ecf765 434 Amarillo = 0;
sebasmartinez 0:73e805ecf765 435 Negro = 0;
sebasmartinez 0:73e805ecf765 436 Blanco = 0;
sebasmartinez 0:73e805ecf765 437
sebasmartinez 0:73e805ecf765 438 return;
sebasmartinez 0:73e805ecf765 439 }
sebasmartinez 0:73e805ecf765 440
sebasmartinez 0:73e805ecf765 441 void debug_m(char *s , ... ){
sebasmartinez 0:73e805ecf765 442 #if DEBUG
sebasmartinez 0:73e805ecf765 443 command.printf(s);
sebasmartinez 1:59be7d7e1b54 444 Bluetooth.printf(s);
sebasmartinez 0:73e805ecf765 445 #endif
sebasmartinez 1:59be7d7e1b54 446 }
sebasmartinez 1:59be7d7e1b54 447 void Rx_interruptBT() {
sebasmartinez 1:59be7d7e1b54 448 Led=!Led;
sebasmartinez 1:59be7d7e1b54 449 //char d = Bluetooth.getc();
sebasmartinez 1:59be7d7e1b54 450 //Bluetooth.printf("blue %x\n",d);
sebasmartinez 1:59be7d7e1b54 451 //command.printf("bluetooth %x\n",d);
sebasmartinez 1:59be7d7e1b54 452 debug_m("Entrando TC...\n");
sebasmartinez 1:59be7d7e1b54 453 uint32_t read_cc;
sebasmartinez 1:59be7d7e1b54 454 read_cc=read_command2();
sebasmartinez 1:59be7d7e1b54 455
sebasmartinez 1:59be7d7e1b54 456 switch (read_cc) {
sebasmartinez 1:59be7d7e1b54 457
sebasmartinez 1:59be7d7e1b54 458 case 0x01:
sebasmartinez 1:59be7d7e1b54 459 debug_m("Tc 1 Iniciado..\n");
sebasmartinez 1:59be7d7e1b54 460 moving();
sebasmartinez 1:59be7d7e1b54 461 debug_m("Tc 1 Finalizado\n");
sebasmartinez 1:59be7d7e1b54 462 break;
sebasmartinez 1:59be7d7e1b54 463 case 0x02:
sebasmartinez 1:59be7d7e1b54 464 debug_m("Tc 2 Iniciado.. \n");
sebasmartinez 1:59be7d7e1b54 465 pares();
sebasmartinez 1:59be7d7e1b54 466 debug_m("Tc 2 Finalizado\n");
sebasmartinez 1:59be7d7e1b54 467 break;
sebasmartinez 1:59be7d7e1b54 468 case 0x03:
sebasmartinez 1:59be7d7e1b54 469 debug_m("Tc 3 Iniciado.. \n");
sebasmartinez 1:59be7d7e1b54 470 caminar(3);
sebasmartinez 1:59be7d7e1b54 471 debug_m("Tc 3 Finalizado\n");
sebasmartinez 1:59be7d7e1b54 472 break;
sebasmartinez 1:59be7d7e1b54 473 default: debug_m("Error de comando. \nSe espera entre 0x01 - 0x08 \n");break ;
sebasmartinez 1:59be7d7e1b54 474 }
sebasmartinez 1:59be7d7e1b54 475 wait(3);
sebasmartinez 1:59be7d7e1b54 476 return;
sebasmartinez 1:59be7d7e1b54 477 }
sebasmartinez 1:59be7d7e1b54 478 void Rx_interrupt() {
sebasmartinez 1:59be7d7e1b54 479
sebasmartinez 1:59be7d7e1b54 480 char d = command.getc();
sebasmartinez 1:59be7d7e1b54 481 command.printf("Serial %c\n",d);
sebasmartinez 1:59be7d7e1b54 482 if (d=='i')
sebasmartinez 1:59be7d7e1b54 483 {
sebasmartinez 1:59be7d7e1b54 484 command.printf("Arri1\n");
sebasmartinez 1:59be7d7e1b54 485 direccion(0x03);
sebasmartinez 1:59be7d7e1b54 486 }else if(d=='d')
sebasmartinez 1:59be7d7e1b54 487 {
sebasmartinez 1:59be7d7e1b54 488 command.printf("Abaj1\n");
sebasmartinez 1:59be7d7e1b54 489 direccion(0x04);
sebasmartinez 1:59be7d7e1b54 490 }else if(d=='o')
sebasmartinez 1:59be7d7e1b54 491 {
sebasmartinez 1:59be7d7e1b54 492 command.printf("Izq1\n");
sebasmartinez 1:59be7d7e1b54 493 direccion(0x02);
sebasmartinez 1:59be7d7e1b54 494 }else if(d=='p')
sebasmartinez 1:59be7d7e1b54 495 {
sebasmartinez 1:59be7d7e1b54 496 command.printf("Der1\n");
sebasmartinez 1:59be7d7e1b54 497 direccion(0x01);
sebasmartinez 1:59be7d7e1b54 498 }else if(d=='1')
sebasmartinez 1:59be7d7e1b54 499 {
sebasmartinez 1:59be7d7e1b54 500 command.printf("Izq2\n");
sebasmartinez 1:59be7d7e1b54 501 direccion(0x05);
sebasmartinez 1:59be7d7e1b54 502 }else if(d=='2')
sebasmartinez 1:59be7d7e1b54 503 {
sebasmartinez 1:59be7d7e1b54 504 command.printf("Der2\n");
sebasmartinez 1:59be7d7e1b54 505 direccion(0x06);
sebasmartinez 1:59be7d7e1b54 506 }else if(d=='3')
sebasmartinez 1:59be7d7e1b54 507 {
sebasmartinez 1:59be7d7e1b54 508 command.printf("Arri2\n");
sebasmartinez 1:59be7d7e1b54 509 arrancar();
sebasmartinez 1:59be7d7e1b54 510 }else if(d=='4')
sebasmartinez 1:59be7d7e1b54 511 {
sebasmartinez 1:59be7d7e1b54 512 command.printf("Abaj2\n");
sebasmartinez 1:59be7d7e1b54 513 }else if(d=='Y')
sebasmartinez 1:59be7d7e1b54 514 {
sebasmartinez 1:59be7d7e1b54 515 command.printf("Y\n");
sebasmartinez 1:59be7d7e1b54 516 centro(0x01);
sebasmartinez 1:59be7d7e1b54 517
sebasmartinez 1:59be7d7e1b54 518 }else if(d=='X')
sebasmartinez 1:59be7d7e1b54 519 {
sebasmartinez 1:59be7d7e1b54 520 command.printf("X\n");
sebasmartinez 1:59be7d7e1b54 521 centro(0x02);
sebasmartinez 1:59be7d7e1b54 522 }else if(d=='B')
sebasmartinez 1:59be7d7e1b54 523 {
sebasmartinez 1:59be7d7e1b54 524 command.printf("B\n");
sebasmartinez 1:59be7d7e1b54 525 centro(0x03);
sebasmartinez 1:59be7d7e1b54 526 }else if(d=='A')
sebasmartinez 1:59be7d7e1b54 527 {
sebasmartinez 1:59be7d7e1b54 528 command.printf("A\n");
sebasmartinez 1:59be7d7e1b54 529 centro(0x04);
sebasmartinez 1:59be7d7e1b54 530 }else{
sebasmartinez 1:59be7d7e1b54 531 command.printf("Sin especificar\n");
sebasmartinez 1:59be7d7e1b54 532 Led=!Led;
sebasmartinez 1:59be7d7e1b54 533 }
sebasmartinez 1:59be7d7e1b54 534
sebasmartinez 1:59be7d7e1b54 535
sebasmartinez 1:59be7d7e1b54 536 /*//telecomandos desde serial
sebasmartinez 1:59be7d7e1b54 537 uint32_t read_cc;
sebasmartinez 1:59be7d7e1b54 538 read_cc=read_command();
sebasmartinez 1:59be7d7e1b54 539 debug_m("Entrando TC...\n");
sebasmartinez 1:59be7d7e1b54 540
sebasmartinez 1:59be7d7e1b54 541 switch (read_cc) {
sebasmartinez 1:59be7d7e1b54 542
sebasmartinez 1:59be7d7e1b54 543 case 0x01:
sebasmartinez 1:59be7d7e1b54 544 debug_m("Tc 1 Iniciado..\n");
sebasmartinez 1:59be7d7e1b54 545 moving();
sebasmartinez 1:59be7d7e1b54 546 debug_m("Tc 1 Finalizado\n");
sebasmartinez 1:59be7d7e1b54 547 break;
sebasmartinez 1:59be7d7e1b54 548 case 0x02:
sebasmartinez 1:59be7d7e1b54 549 debug_m("Tc 2 Iniciado.. \n");
sebasmartinez 1:59be7d7e1b54 550 pares();
sebasmartinez 1:59be7d7e1b54 551 debug_m("Tc 2 Finalizado\n");
sebasmartinez 1:59be7d7e1b54 552 break;
sebasmartinez 1:59be7d7e1b54 553 case 0x03:
sebasmartinez 1:59be7d7e1b54 554 debug_m("Tc 3 Iniciado.. \n");
sebasmartinez 1:59be7d7e1b54 555 caminar();
sebasmartinez 1:59be7d7e1b54 556 debug_m("Tc 3 Finalizado\n");
sebasmartinez 1:59be7d7e1b54 557 break;
sebasmartinez 1:59be7d7e1b54 558 case 0x05:
sebasmartinez 1:59be7d7e1b54 559 debug_m("Tc 5 Centrar Iniciado.. \n");
sebasmartinez 1:59be7d7e1b54 560 centro(01);
sebasmartinez 1:59be7d7e1b54 561 centro(02);
sebasmartinez 1:59be7d7e1b54 562 centro(03);
sebasmartinez 1:59be7d7e1b54 563 centro(04);
sebasmartinez 1:59be7d7e1b54 564 debug_m("Tc 5 Centrar Finalizado\n");
sebasmartinez 1:59be7d7e1b54 565 break;
sebasmartinez 1:59be7d7e1b54 566 default: debug_m("Error de comando. \nSe espera entre 0x01 - 0x08 \n");break ;
sebasmartinez 1:59be7d7e1b54 567 }
sebasmartinez 1:59be7d7e1b54 568 */
sebasmartinez 1:59be7d7e1b54 569 return;
sebasmartinez 1:59be7d7e1b54 570
sebasmartinez 0:73e805ecf765 571 }