Diana Marcela Palomino Martinez / Mbed 2 deprecated RobotColor

Wiki tecnica, wiki usuario

Dependencies:   mbed

main.cpp@0:bf193a68e357, 2019-05-30 (annotated)

Committer:
Diani
Date:
Thu May 30 23:43:57 2019 +0000
Revision:
0:bf193a68e357
Tercer Corte

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Diani 0:bf193a68e357 1 #include "mbed.h"
Diani 0:bf193a68e357 2 #include "scolor_TCS3200.h"
Diani 0:bf193a68e357 3
Diani 0:bf193a68e357 4 Serial command(USBTX, USBRX);
Diani 0:bf193a68e357 5
Diani 0:bf193a68e357 6 PwmOut mybuzzer(D9);
Diani 0:bf193a68e357 7 DigitalOut stepper_step(PB_8);
Diani 0:bf193a68e357 8 DigitalOut steppeer_dir(PB_9);
Diani 0:bf193a68e357 9 DigitalOut stepper_step_2(PA_7);
Diani 0:bf193a68e357 10 DigitalOut steppeer_dir_2(PA_6);
Diani 0:bf193a68e357 11
Diani 0:bf193a68e357 12 scolor_TCS3200 scolor(PA_8, PB_10, PB_4, PB_5, PB_3);
Diani 0:bf193a68e357 13
Diani 0:bf193a68e357 14 #define INITCMD 0xFF
Diani 0:bf193a68e357 15 #define VELOCITY 5000
Diani 0:bf193a68e357 16 #define SENSOR 0x00
Diani 0:bf193a68e357 17 #define TONO_1 0x01
Diani 0:bf193a68e357 18 #define TONO_2 0x02
Diani 0:bf193a68e357 19 #define TONO_3 0x03
Diani 0:bf193a68e357 20 #define TONO_4 0x04
Diani 0:bf193a68e357 21 #define UP 0x05
Diani 0:bf193a68e357 22 #define DOWN 0x06
Diani 0:bf193a68e357 23 #define LEFT 0x07
Diani 0:bf193a68e357 24 #define RIGTH 0x08
Diani 0:bf193a68e357 25 #define VEL 0x09
Diani 0:bf193a68e357 26 #define DO 100
Diani 0:bf193a68e357 27 #define RE 150
Diani 0:bf193a68e357 28 #define MI 200
Diani 0:bf193a68e357 29 #define FA 250
Diani 0:bf193a68e357 30
Diani 0:bf193a68e357 31 // definición de las variables globales
Diani 0:bf193a68e357 32
Diani 0:bf193a68e357 33 uint8_t tip_mov;
Diani 0:bf193a68e357 34 uint8_t n_vueltas;
Diani 0:bf193a68e357 35 uint8_t STEEP;
Diani 0:bf193a68e357 36 float velocidad=500;
Diani 0:bf193a68e357 37 uint8_t tiempo;
Diani 0:bf193a68e357 38
Diani 0:bf193a68e357 39 // definición de las funciones
Diani 0:bf193a68e357 40
Diani 0:bf193a68e357 41 void setup_uart();
Diani 0:bf193a68e357 42 void mover_steper_nema();
Diani 0:bf193a68e357 43 void leer_datos();
Diani 0:bf193a68e357 44 void funcion_pasos();
Diani 0:bf193a68e357 45 void funcion_pasos_2();
Diani 0:bf193a68e357 46 void buzzer_on();
Diani 0:bf193a68e357 47 void leer_color();
Diani 0:bf193a68e357 48
Diani 0:bf193a68e357 49 int main() {
Diani 0:bf193a68e357 50
Diani 0:bf193a68e357 51 setup_uart();
Diani 0:bf193a68e357 52
Diani 0:bf193a68e357 53 while(1){
Diani 0:bf193a68e357 54
Diani 0:bf193a68e357 55 leer_datos();
Diani 0:bf193a68e357 56 mover_steper_nema();
Diani 0:bf193a68e357 57
Diani 0:bf193a68e357 58 }
Diani 0:bf193a68e357 59
Diani 0:bf193a68e357 60 }
Diani 0:bf193a68e357 61
Diani 0:bf193a68e357 62 void setup_uart(){
Diani 0:bf193a68e357 63
Diani 0:bf193a68e357 64 command.baud(115200);
Diani 0:bf193a68e357 65
Diani 0:bf193a68e357 66 }
Diani 0:bf193a68e357 67
Diani 0:bf193a68e357 68 void leer_datos(){
Diani 0:bf193a68e357 69
Diani 0:bf193a68e357 70 while(command.getc()!= INITCMD);
Diani 0:bf193a68e357 71
Diani 0:bf193a68e357 72 tip_mov=command.getc();
Diani 0:bf193a68e357 73 n_vueltas=command.getc();
Diani 0:bf193a68e357 74
Diani 0:bf193a68e357 75
Diani 0:bf193a68e357 76
Diani 0:bf193a68e357 77 }
Diani 0:bf193a68e357 78
Diani 0:bf193a68e357 79 void mover_steper_nema(){
Diani 0:bf193a68e357 80
Diani 0:bf193a68e357 81 switch (tip_mov){
Diani 0:bf193a68e357 82
Diani 0:bf193a68e357 83
Diani 0:bf193a68e357 84 case SENSOR:
Diani 0:bf193a68e357 85
Diani 0:bf193a68e357 86 if (n_vueltas == 0)
Diani 0:bf193a68e357 87 {leer_color();}
Diani 0:bf193a68e357 88 printf("LEER COLOR\n");
Diani 0:bf193a68e357 89
Diani 0:bf193a68e357 90 break;
Diani 0:bf193a68e357 91
Diani 0:bf193a68e357 92 case TONO_1:
Diani 0:bf193a68e357 93
Diani 0:bf193a68e357 94 mybuzzer.write(0);
Diani 0:bf193a68e357 95 mybuzzer.period_ms(DO);
Diani 0:bf193a68e357 96 buzzer_on();
Diani 0:bf193a68e357 97 printf("TONO 1\n");
Diani 0:bf193a68e357 98
Diani 0:bf193a68e357 99 break;
Diani 0:bf193a68e357 100
Diani 0:bf193a68e357 101 case TONO_2:
Diani 0:bf193a68e357 102
Diani 0:bf193a68e357 103 mybuzzer.write(0);
Diani 0:bf193a68e357 104 mybuzzer.period_ms(RE);
Diani 0:bf193a68e357 105 buzzer_on();
Diani 0:bf193a68e357 106 printf("TONO 2\n");
Diani 0:bf193a68e357 107
Diani 0:bf193a68e357 108 break;
Diani 0:bf193a68e357 109
Diani 0:bf193a68e357 110 case TONO_3:
Diani 0:bf193a68e357 111
Diani 0:bf193a68e357 112 mybuzzer.write(0);
Diani 0:bf193a68e357 113 mybuzzer.period_ms(MI);
Diani 0:bf193a68e357 114 buzzer_on();
Diani 0:bf193a68e357 115 printf("TONO 3\n");
Diani 0:bf193a68e357 116
Diani 0:bf193a68e357 117 break;
Diani 0:bf193a68e357 118
Diani 0:bf193a68e357 119 case TONO_4:
Diani 0:bf193a68e357 120
Diani 0:bf193a68e357 121 mybuzzer.write(0);
Diani 0:bf193a68e357 122 mybuzzer.period_ms(FA);
Diani 0:bf193a68e357 123 buzzer_on();
Diani 0:bf193a68e357 124 printf("TONO 4\n");
Diani 0:bf193a68e357 125
Diani 0:bf193a68e357 126 break;
Diani 0:bf193a68e357 127
Diani 0:bf193a68e357 128
Diani 0:bf193a68e357 129
Diani 0:bf193a68e357 130 case UP:
Diani 0:bf193a68e357 131
Diani 0:bf193a68e357 132 steppeer_dir=1;
Diani 0:bf193a68e357 133 steppeer_dir_2=0;
Diani 0:bf193a68e357 134 STEEP=200;
Diani 0:bf193a68e357 135 funcion_pasos();
Diani 0:bf193a68e357 136 printf("MOVER ADELANTE\n");
Diani 0:bf193a68e357 137
Diani 0:bf193a68e357 138 break;
Diani 0:bf193a68e357 139
Diani 0:bf193a68e357 140 case DOWN:
Diani 0:bf193a68e357 141
Diani 0:bf193a68e357 142 steppeer_dir=0;
Diani 0:bf193a68e357 143 steppeer_dir_2=1;
Diani 0:bf193a68e357 144 STEEP=200;
Diani 0:bf193a68e357 145 funcion_pasos();
Diani 0:bf193a68e357 146 printf("MOVER ATRAS\n");
Diani 0:bf193a68e357 147
Diani 0:bf193a68e357 148 break;
Diani 0:bf193a68e357 149
Diani 0:bf193a68e357 150 case LEFT:
Diani 0:bf193a68e357 151
Diani 0:bf193a68e357 152 steppeer_dir=1;
Diani 0:bf193a68e357 153 steppeer_dir_2=1;
Diani 0:bf193a68e357 154 STEEP=50;
Diani 0:bf193a68e357 155 funcion_pasos_2();
Diani 0:bf193a68e357 156 printf("MOVER IZQUIERDA\n");
Diani 0:bf193a68e357 157
Diani 0:bf193a68e357 158 break;
Diani 0:bf193a68e357 159
Diani 0:bf193a68e357 160 case RIGTH:
Diani 0:bf193a68e357 161
Diani 0:bf193a68e357 162 steppeer_dir=0;
Diani 0:bf193a68e357 163 steppeer_dir_2=0;
Diani 0:bf193a68e357 164 STEEP=50;
Diani 0:bf193a68e357 165 funcion_pasos_2();
Diani 0:bf193a68e357 166 printf("MOVER DERECHA\n");
Diani 0:bf193a68e357 167
Diani 0:bf193a68e357 168 break;
Diani 0:bf193a68e357 169
Diani 0:bf193a68e357 170 case VEL:
Diani 0:bf193a68e357 171
Diani 0:bf193a68e357 172 if (n_vueltas==0)
Diani 0:bf193a68e357 173 {velocidad = 250;}
Diani 0:bf193a68e357 174 else if (n_vueltas==1)
Diani 0:bf193a68e357 175 {velocidad = 500;}
Diani 0:bf193a68e357 176 else if (n_vueltas==2)
Diani 0:bf193a68e357 177 {velocidad = 750;}
Diani 0:bf193a68e357 178 printf("%f \n", velocidad);
Diani 0:bf193a68e357 179
Diani 0:bf193a68e357 180 break;
Diani 0:bf193a68e357 181
Diani 0:bf193a68e357 182 }
Diani 0:bf193a68e357 183
Diani 0:bf193a68e357 184 }
Diani 0:bf193a68e357 185
Diani 0:bf193a68e357 186 void funcion_pasos()
Diani 0:bf193a68e357 187 { wait_us(velocidad);
Diani 0:bf193a68e357 188 for (int a=0; a<n_vueltas; a++){
Diani 0:bf193a68e357 189
Diani 0:bf193a68e357 190 for(int i=0; i<STEEP; i++)
Diani 0:bf193a68e357 191
Diani 0:bf193a68e357 192 {
Diani 0:bf193a68e357 193
Diani 0:bf193a68e357 194 stepper_step=1;
Diani 0:bf193a68e357 195 stepper_step_2=1;
Diani 0:bf193a68e357 196 wait_us(velocidad);
Diani 0:bf193a68e357 197 stepper_step=0;
Diani 0:bf193a68e357 198 stepper_step_2=0;
Diani 0:bf193a68e357 199 wait_us(velocidad);
Diani 0:bf193a68e357 200
Diani 0:bf193a68e357 201
Diani 0:bf193a68e357 202 // printf("i");
Diani 0:bf193a68e357 203
Diani 0:bf193a68e357 204 }
Diani 0:bf193a68e357 205
Diani 0:bf193a68e357 206 }
Diani 0:bf193a68e357 207
Diani 0:bf193a68e357 208 }
Diani 0:bf193a68e357 209
Diani 0:bf193a68e357 210 void funcion_pasos_2()
Diani 0:bf193a68e357 211
Diani 0:bf193a68e357 212 { wait_us(1);
Diani 0:bf193a68e357 213 for(int i=0; i<STEEP; i++)
Diani 0:bf193a68e357 214
Diani 0:bf193a68e357 215 {
Diani 0:bf193a68e357 216
Diani 0:bf193a68e357 217 stepper_step=1;
Diani 0:bf193a68e357 218 stepper_step_2=1;
Diani 0:bf193a68e357 219 wait_ms(velocidad);
Diani 0:bf193a68e357 220 stepper_step=0;
Diani 0:bf193a68e357 221 stepper_step_2=0;
Diani 0:bf193a68e357 222 wait_ms(velocidad);
Diani 0:bf193a68e357 223
Diani 0:bf193a68e357 224 printf("i");
Diani 0:bf193a68e357 225
Diani 0:bf193a68e357 226 }
Diani 0:bf193a68e357 227
Diani 0:bf193a68e357 228 }
Diani 0:bf193a68e357 229
Diani 0:bf193a68e357 230 void leer_color(){
Diani 0:bf193a68e357 231
Diani 0:bf193a68e357 232 long red = scolor.ReadRed();
Diani 0:bf193a68e357 233 long green = scolor.ReadGreen();
Diani 0:bf193a68e357 234 long blue = scolor.ReadBlue();
Diani 0:bf193a68e357 235 long clear = scolor.ReadClear();
Diani 0:bf193a68e357 236
Diani 0:bf193a68e357 237 if (red <=800) // lee el rojo------------
Diani 0:bf193a68e357 238
Diani 0:bf193a68e357 239 {
Diani 0:bf193a68e357 240
Diani 0:bf193a68e357 241 if (green>=1200 and green <=1500)
Diani 0:bf193a68e357 242
Diani 0:bf193a68e357 243 {
Diani 0:bf193a68e357 244
Diani 0:bf193a68e357 245 if (blue>=1200 and blue <=1500)
Diani 0:bf193a68e357 246
Diani 0:bf193a68e357 247 {
Diani 0:bf193a68e357 248
Diani 0:bf193a68e357 249
Diani 0:bf193a68e357 250
Diani 0:bf193a68e357 251 printf("|FE 01|\n");
Diani 0:bf193a68e357 252
Diani 0:bf193a68e357 253
Diani 0:bf193a68e357 254
Diani 0:bf193a68e357 255 }
Diani 0:bf193a68e357 256
Diani 0:bf193a68e357 257 }
Diani 0:bf193a68e357 258
Diani 0:bf193a68e357 259 }
Diani 0:bf193a68e357 260
Diani 0:bf193a68e357 261
Diani 0:bf193a68e357 262
Diani 0:bf193a68e357 263 if (red <=2700) // lee el verde --------------------------------------
Diani 0:bf193a68e357 264
Diani 0:bf193a68e357 265 {
Diani 0:bf193a68e357 266
Diani 0:bf193a68e357 267 if (green>=900 and green <=1500)
Diani 0:bf193a68e357 268
Diani 0:bf193a68e357 269 {
Diani 0:bf193a68e357 270
Diani 0:bf193a68e357 271 if (blue>=1400 and blue <=2000)
Diani 0:bf193a68e357 272
Diani 0:bf193a68e357 273 {
Diani 0:bf193a68e357 274
Diani 0:bf193a68e357 275
Diani 0:bf193a68e357 276
Diani 0:bf193a68e357 277 printf("|FE 03|\n");
Diani 0:bf193a68e357 278
Diani 0:bf193a68e357 279
Diani 0:bf193a68e357 280
Diani 0:bf193a68e357 281 }
Diani 0:bf193a68e357 282
Diani 0:bf193a68e357 283 }
Diani 0:bf193a68e357 284
Diani 0:bf193a68e357 285 }
Diani 0:bf193a68e357 286
Diani 0:bf193a68e357 287
Diani 0:bf193a68e357 288
Diani 0:bf193a68e357 289 if (red<=2400) // lee el azul------------------------------------
Diani 0:bf193a68e357 290
Diani 0:bf193a68e357 291 {
Diani 0:bf193a68e357 292
Diani 0:bf193a68e357 293 if (green>=800 and green <=1000)
Diani 0:bf193a68e357 294
Diani 0:bf193a68e357 295 {
Diani 0:bf193a68e357 296
Diani 0:bf193a68e357 297 if (blue>=600 and blue <=800)
Diani 0:bf193a68e357 298
Diani 0:bf193a68e357 299 {
Diani 0:bf193a68e357 300
Diani 0:bf193a68e357 301
Diani 0:bf193a68e357 302
Diani 0:bf193a68e357 303 printf("|FE 02|\n");
Diani 0:bf193a68e357 304
Diani 0:bf193a68e357 305
Diani 0:bf193a68e357 306
Diani 0:bf193a68e357 307 }
Diani 0:bf193a68e357 308
Diani 0:bf193a68e357 309 }
Diani 0:bf193a68e357 310
Diani 0:bf193a68e357 311 }
Diani 0:bf193a68e357 312
Diani 0:bf193a68e357 313 if (red <=700) // lee el amarillo------------------------------------
Diani 0:bf193a68e357 314
Diani 0:bf193a68e357 315 {
Diani 0:bf193a68e357 316
Diani 0:bf193a68e357 317 if (green>=500 and green <=700)
Diani 0:bf193a68e357 318
Diani 0:bf193a68e357 319 {
Diani 0:bf193a68e357 320
Diani 0:bf193a68e357 321 if (blue>=500 and blue <=1500)
Diani 0:bf193a68e357 322
Diani 0:bf193a68e357 323 {
Diani 0:bf193a68e357 324
Diani 0:bf193a68e357 325
Diani 0:bf193a68e357 326
Diani 0:bf193a68e357 327 printf("|FE 04|\n");
Diani 0:bf193a68e357 328
Diani 0:bf193a68e357 329
Diani 0:bf193a68e357 330
Diani 0:bf193a68e357 331 }
Diani 0:bf193a68e357 332
Diani 0:bf193a68e357 333 }
Diani 0:bf193a68e357 334
Diani 0:bf193a68e357 335 }
Diani 0:bf193a68e357 336
Diani 0:bf193a68e357 337
Diani 0:bf193a68e357 338
Diani 0:bf193a68e357 339 if (red <=200) // lectura errada------------------------------------
Diani 0:bf193a68e357 340
Diani 0:bf193a68e357 341 {
Diani 0:bf193a68e357 342
Diani 0:bf193a68e357 343 if (green <=300)
Diani 0:bf193a68e357 344
Diani 0:bf193a68e357 345 {
Diani 0:bf193a68e357 346
Diani 0:bf193a68e357 347 if (blue <=300)
Diani 0:bf193a68e357 348
Diani 0:bf193a68e357 349 {
Diani 0:bf193a68e357 350
Diani 0:bf193a68e357 351
Diani 0:bf193a68e357 352
Diani 0:bf193a68e357 353 printf("|FE 00|\n");
Diani 0:bf193a68e357 354
Diani 0:bf193a68e357 355
Diani 0:bf193a68e357 356
Diani 0:bf193a68e357 357 }
Diani 0:bf193a68e357 358
Diani 0:bf193a68e357 359
Diani 0:bf193a68e357 360
Diani 0:bf193a68e357 361 }
Diani 0:bf193a68e357 362
Diani 0:bf193a68e357 363 }
Diani 0:bf193a68e357 364
Diani 0:bf193a68e357 365
Diani 0:bf193a68e357 366
Diani 0:bf193a68e357 367 // printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", red, green, blue, clear);
Diani 0:bf193a68e357 368
Diani 0:bf193a68e357 369 }
Diani 0:bf193a68e357 370
Diani 0:bf193a68e357 371
Diani 0:bf193a68e357 372
Diani 0:bf193a68e357 373 void buzzer_on()
Diani 0:bf193a68e357 374
Diani 0:bf193a68e357 375
Diani 0:bf193a68e357 376
Diani 0:bf193a68e357 377 { tiempo = n_vueltas;
Diani 0:bf193a68e357 378
Diani 0:bf193a68e357 379 mybuzzer.write(0.5);
Diani 0:bf193a68e357 380
Diani 0:bf193a68e357 381 wait(tiempo);
Diani 0:bf193a68e357 382
Diani 0:bf193a68e357 383 mybuzzer.write(0);
Diani 0:bf193a68e357 384
Diani 0:bf193a68e357 385
Diani 0:bf193a68e357 386
Diani 0:bf193a68e357 387 }