Einer Fabian Aponte Cubides / Mbed 2 deprecated Final3

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@12:b8ee724b42b1, 2019-05-30 (annotated)

Committer:
Darstack
Date:
Thu May 30 20:41:52 2019 +0000
Revision:
12:b8ee724b42b1
Parent:
11:6b3570e525a1 
Final corte;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fabeltranm 0:fa6bdbdd4239 1 #include "mbed.h"
fabeltranm 0:fa6bdbdd4239 2 #include "scolor_TCS3200.h"
fabeltranm 0:fa6bdbdd4239 3
fabeltranm 0:fa6bdbdd4239 4 /* *****************************************************************************
fabeltranm 0:fa6bdbdd4239 5 Programa que solicita telemetria al sistema embebido, por medio del comando 0xFe
fabeltranm 0:fa6bdbdd4239 6 para preguntar por el color que detecta el sensor TCS3200
fabeltranm 0:fa6bdbdd4239 7
fabeltranm 0:fa6bdbdd4239 8 el sistema embebido recibe el inicio de trama FE y el número de comado: 01
fabeltranm 0:fa6bdbdd4239 9
fabeltranm 0:fa6bdbdd4239 10 | INITELE | CMD |
fabeltranm 0:fa6bdbdd4239 11 | 0xfe | 0x01 |
fabeltranm 0:fa6bdbdd4239 12
fabeltranm 0:fa6bdbdd4239 13
fabeltranm 0:fa6bdbdd4239 14 para enviar los comandos usar el programa Coolterm http://freeware.the-meiers.org/
fabeltranm 0:fa6bdbdd4239 15
fabeltranm 0:fa6bdbdd4239 16
fabeltranm 0:fa6bdbdd4239 17 @fabeltranm 2019
fabeltranm 0:fa6bdbdd4239 18 fbeltranm@ecci.edu.co
fabeltranm 0:fa6bdbdd4239 19
fabeltranm 0:fa6bdbdd4239 20 ********************************************************************************
fabeltranm 0:fa6bdbdd4239 21 datasheet https://www.mouser.com/catalog/specsheets/TCS3200-E11.pdf
fabeltranm 0:fa6bdbdd4239 22
fabeltranm 0:fa6bdbdd4239 23
fabeltranm 0:fa6bdbdd4239 24 S0 Frequency scaling
fabeltranm 0:fa6bdbdd4239 25 S1 Frequency scaling
fabeltranm 0:fa6bdbdd4239 26 S2 Photo diode selection
fabeltranm 0:fa6bdbdd4239 27 S3 Photo diode selection
fabeltranm 0:fa6bdbdd4239 28 OutFreq Frequency
fabeltranm 0:fa6bdbdd4239 29
fabeltranm 0:fa6bdbdd4239 30 -----------------------------------
fabeltranm 0:fa6bdbdd4239 31 | ____________ ____________ |
fabeltranm 0:fa6bdbdd4239 32 ----> | | | | | | ___ ___
fabeltranm 0:fa6bdbdd4239 33 Light | | Photodiode | | Current |--|---OUTPUT_FREQ | |___| |___
fabeltranm 0:fa6bdbdd4239 34 ----> | | Array |---| to | |
fabeltranm 0:fa6bdbdd4239 35 | | | | Frequency | |
fabeltranm 0:fa6bdbdd4239 36 | |____________| |____________| |
fabeltranm 0:fa6bdbdd4239 37 | ^ ^ ^ ^ |
fabeltranm 0:fa6bdbdd4239 38 -------|--|-------------|--|-------
fabeltranm 0:fa6bdbdd4239 39 | | | |
fabeltranm 0:fa6bdbdd4239 40 S2 S3 S0 S1
fabeltranm 0:fa6bdbdd4239 41
fabeltranm 0:fa6bdbdd4239 42 SO | S1 | OUTPUT FREQUENCY SCALING | | S2 | S3 | PHOTODIODE TYPE |
fabeltranm 0:fa6bdbdd4239 43 0 | 0 | power down | | 0 | 0 | Red |
fabeltranm 0:fa6bdbdd4239 44 0 | 1 | 2% | | 0 | 1 | Blue |
fabeltranm 0:fa6bdbdd4239 45 1 | 0 | 20% | | 1 | 0 | Clear (no filter) |
fabeltranm 0:fa6bdbdd4239 46 1 | 1 | 100% | | 1 | 1 | Green |
fabeltranm 0:fa6bdbdd4239 47
fabeltranm 0:fa6bdbdd4239 48 ******************************************************************************/
fabeltranm 0:fa6bdbdd4239 49
Darstack 4:7a06dce3de99 50 #define INITCMD 0xFF
Darstack 4:7a06dce3de99 51 #define DO 2000
Darstack 4:7a06dce3de99 52 #define RE 2500
Darstack 4:7a06dce3de99 53 #define MI 3000
Darstack 4:7a06dce3de99 54 #define FA 3500
fabeltranm 0:fa6bdbdd4239 55
Darstack 4:7a06dce3de99 56 /* ||Definicion de Puertos || */
Darstack 12:b8ee724b42b1 57
Darstack 4:7a06dce3de99 58 // S0, S1, S2, S3, OUT
Darstack 12:b8ee724b42b1 59 scolor_TCS3200 scolor( PA_9, PC_7, PB_6, PA_4, PB_3 ) ;//PA_9, PC_7, PB_6, PA_7, PA_6 //PA_9, PC_7, PB_6, PA_4, PB_3
Darstack 12:b8ee724b42b1 60 Serial command(USBTX, USBRX);
Darstack 12:b8ee724b42b1 61
Darstack 12:b8ee724b42b1 62 DigitalOut stepper_step (PB_4);
Darstack 12:b8ee724b42b1 63 DigitalOut steppeer_dir (PB_5);
Darstack 12:b8ee724b42b1 64 DigitalOut stepper_step2 (PB_10);
Darstack 12:b8ee724b42b1 65 DigitalOut steppeer_dir2 (PA_8);
Darstack 12:b8ee724b42b1 66
Darstack 12:b8ee724b42b1 67 AnalogIn analog_value0 (A0);
Darstack 12:b8ee724b42b1 68 AnalogIn analog_value1 (A1);
Darstack 12:b8ee724b42b1 69 PwmOut mybuzzer(PB_9) ;
Darstack 12:b8ee724b42b1 70
Darstack 11:6b3570e525a1 71 Ticker tk1;
Darstack 12:b8ee724b42b1 72 Ticker tk2;
Darstack 12:b8ee724b42b1 73
Darstack 11:6b3570e525a1 74 volatile int Exit = 0 ;
Darstack 12:b8ee724b42b1 75
Darstack 11:6b3570e525a1 76 /*|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||/ /
Darstack 11:6b3570e525a1 77 / | |/
Darstack 11:6b3570e525a1 78 / | Interrupcion del Joystick- Eliminando ruido |/
Darstack 11:6b3570e525a1 79 / | |/
Darstack 11:6b3570e525a1 80 / | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||*/
fabeltranm 0:fa6bdbdd4239 81
Darstack 11:6b3570e525a1 82 //Definicion de variables
Darstack 11:6b3570e525a1 83 DigitalOut led1(LED1);
Darstack 11:6b3570e525a1 84 InterruptIn Button_joy(USER_BUTTON);
Darstack 11:6b3570e525a1 85 Timeout Button_joy_timeout; // Used for debouncing
Darstack 11:6b3570e525a1 86
Darstack 11:6b3570e525a1 87
Darstack 11:6b3570e525a1 88 //Definicion de las funciones
Darstack 11:6b3570e525a1 89 volatile bool Button_joy_pressed = false; // Used in the main loop
Darstack 11:6b3570e525a1 90 volatile bool Button_joy_enabled = true; // Used for debouncing
Darstack 11:6b3570e525a1 91
Darstack 11:6b3570e525a1 92 // Habilita el botón cuando termina el rebote.
Darstack 11:6b3570e525a1 93 void Button_joy_enabled_cb(void)
Darstack 11:6b3570e525a1 94 {
Darstack 11:6b3570e525a1 95 Button_joy_enabled = true;
Darstack 11:6b3570e525a1 96 }
Darstack 11:6b3570e525a1 97
Darstack 11:6b3570e525a1 98 // ISR Luego de precionar el boton
Darstack 11:6b3570e525a1 99 void Button_joy_onpressed_cb(void)
Darstack 11:6b3570e525a1 100 {
Darstack 11:6b3570e525a1 101 if (Button_joy_enabled) { // Desabilitado mientras e ruido
Darstack 11:6b3570e525a1 102 Button_joy_enabled = false;
Darstack 11:6b3570e525a1 103 Button_joy_pressed = true; // Se cambia el valor para ser leido más adelante
Darstack 11:6b3570e525a1 104 Button_joy_timeout.attach(callback(Button_joy_enabled_cb), 0.3); // Delay de 300 ms
Darstack 11:6b3570e525a1 105 }
Darstack 11:6b3570e525a1 106 if (Button_joy_pressed) { // Etablece cuando se preciona el botn
Darstack 11:6b3570e525a1 107 Button_joy_pressed = false;
Darstack 11:6b3570e525a1 108 //NVIC_SystemReset();
Darstack 11:6b3570e525a1 109 led1 = !led1;
Darstack 11:6b3570e525a1 110 }
Darstack 11:6b3570e525a1 111 }
Darstack 11:6b3570e525a1 112 /*|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||/ /
Darstack 11:6b3570e525a1 113 / |||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||/
Darstack 11:6b3570e525a1 114 / | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||*/
Darstack 11:6b3570e525a1 115
Darstack 11:6b3570e525a1 116 void Rx_interrupt();
Darstack 11:6b3570e525a1 117
Darstack 11:6b3570e525a1 118 /*Definicion de variables codigo principal*/
Darstack 4:7a06dce3de99 119
Darstack 12:b8ee724b42b1 120 uint32_t VELOCITY = 1000; // Tiempo en micro segundos
Darstack 12:b8ee724b42b1 121
Darstack 5:d165935ba818 122 double In [ 1 ] = {} ;
Darstack 4:7a06dce3de99 123
Darstack 11:6b3570e525a1 124 // Definición de las funciones bucle principal
Darstack 5:d165935ba818 125 void leer_datos ();
Darstack 5:d165935ba818 126 void leer_color ();
Darstack 12:b8ee724b42b1 127
Darstack 4:7a06dce3de99 128 void funcionesrobot ( uint8_t _Parametro, uint8_t _Comando );
Darstack 12:b8ee724b42b1 129 int16_t Lectura [ 2 ] = {} ;
Darstack 12:b8ee724b42b1 130 void setup_uart ();
Darstack 12:b8ee724b42b1 131
Darstack 12:b8ee724b42b1 132
Darstack 12:b8ee724b42b1 133 SPI spi(PA_7, PA_6, PA_5); // MOSI, MISO, SCK
fabeltranm 0:fa6bdbdd4239 134
Darstack 12:b8ee724b42b1 135 DigitalOut cs(PB_0); // TFT chipselect pin
Darstack 12:b8ee724b42b1 136 DigitalOut dc(PC_0); // TFT data command select pin
Darstack 12:b8ee724b42b1 137 DigitalOut rst(PC_1); // TFT reset pin
Darstack 12:b8ee724b42b1 138
Darstack 12:b8ee724b42b1 139 #define TFT_WIDTH 320
Darstack 12:b8ee724b42b1 140 #define TFT_HEIGHT 240
Darstack 12:b8ee724b42b1 141
Darstack 12:b8ee724b42b1 142 #define TFT_BLUE 0x1F00
Darstack 12:b8ee724b42b1 143 #define TFT_RED 0x00F8
Darstack 12:b8ee724b42b1 144 #define TFT_GREEN 0xE007
Darstack 12:b8ee724b42b1 145 #define TFT_YELLOW 0xE0FF
Darstack 12:b8ee724b42b1 146 #define TFT_WHITE 0xFFFF
Darstack 12:b8ee724b42b1 147
Darstack 12:b8ee724b42b1 148 /*-------------------------------------------------------------------*/
Darstack 12:b8ee724b42b1 149 /* Write command */
Darstack 12:b8ee724b42b1 150 /*-------------------------------------------------------------------*/
Darstack 12:b8ee724b42b1 151 void write_cmd(uint8_t cmd)
Darstack 12:b8ee724b42b1 152 {
Darstack 12:b8ee724b42b1 153 dc = 0;
Darstack 12:b8ee724b42b1 154 spi.write(cmd);
Darstack 12:b8ee724b42b1 155 }
Darstack 12:b8ee724b42b1 156
Darstack 12:b8ee724b42b1 157 /*-------------------------------------------------------------------*/
Darstack 12:b8ee724b42b1 158 /* Write data */
Darstack 12:b8ee724b42b1 159 /*-------------------------------------------------------------------*/
Darstack 12:b8ee724b42b1 160 void write_data(uint8_t data)
Darstack 12:b8ee724b42b1 161 {
Darstack 12:b8ee724b42b1 162 dc = 1;
Darstack 12:b8ee724b42b1 163 spi.write(data);
Darstack 12:b8ee724b42b1 164 }
Darstack 12:b8ee724b42b1 165
Darstack 12:b8ee724b42b1 166 /*-------------------------------------------------------------------*/
Darstack 12:b8ee724b42b1 167 /* TFT reset */
Darstack 12:b8ee724b42b1 168 /*-------------------------------------------------------------------*/
Darstack 12:b8ee724b42b1 169 void tft_reset()
Darstack 12:b8ee724b42b1 170 {
Darstack 12:b8ee724b42b1 171 wait_ms(200);
Darstack 12:b8ee724b42b1 172 cs = 1;
Darstack 12:b8ee724b42b1 173 dc = 1;
Darstack 12:b8ee724b42b1 174 rst = 1;
Darstack 12:b8ee724b42b1 175 wait_ms(200);
Darstack 12:b8ee724b42b1 176 rst = 0;
Darstack 12:b8ee724b42b1 177 wait_us(10);
Darstack 12:b8ee724b42b1 178 rst = 1;
Darstack 12:b8ee724b42b1 179 wait_ms(120);
Darstack 12:b8ee724b42b1 180 cs = 0;
Darstack 12:b8ee724b42b1 181 wait_ms(10);
Darstack 12:b8ee724b42b1 182
Darstack 12:b8ee724b42b1 183 write_cmd(0x3A); // Pixel Format
Darstack 12:b8ee724b42b1 184 write_data(0x55); // 16bit Color
Darstack 12:b8ee724b42b1 185
Darstack 12:b8ee724b42b1 186 write_cmd(0xB1); // Frame Control
Darstack 12:b8ee724b42b1 187 write_data(0);
Darstack 12:b8ee724b42b1 188 write_data(0x1f);
Darstack 12:b8ee724b42b1 189
Darstack 12:b8ee724b42b1 190 write_cmd(0x36); // Memory Access Control
Darstack 12:b8ee724b42b1 191 write_data(0xE8); // MY MX MV BGR
Darstack 12:b8ee724b42b1 192
Darstack 12:b8ee724b42b1 193 write_cmd(0x11); // Sleep Out
Darstack 12:b8ee724b42b1 194 wait_ms(5);
Darstack 12:b8ee724b42b1 195
Darstack 12:b8ee724b42b1 196 write_cmd(0x29); // Display On
Darstack 12:b8ee724b42b1 197 }
Darstack 12:b8ee724b42b1 198
Darstack 12:b8ee724b42b1 199 /*-------------------------------------------------------------------*/
Darstack 12:b8ee724b42b1 200 /* Set the windows area and Start memory write. */
Darstack 12:b8ee724b42b1 201 /*-------------------------------------------------------------------*/
Darstack 12:b8ee724b42b1 202 void tft_set_window(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1)
Darstack 12:b8ee724b42b1 203 {
Darstack 12:b8ee724b42b1 204 write_cmd(0x2A); // Column Address Set
Darstack 12:b8ee724b42b1 205 write_data(x0 >> 8);
Darstack 12:b8ee724b42b1 206 write_data(x0);
Darstack 12:b8ee724b42b1 207 write_data(x1 >> 8);
Darstack 12:b8ee724b42b1 208 write_data(x1);
Darstack 12:b8ee724b42b1 209
Darstack 12:b8ee724b42b1 210 write_cmd(0x2B); // Page Address Set
Darstack 12:b8ee724b42b1 211 write_data(y0 >> 8);
Darstack 12:b8ee724b42b1 212 write_data(y0);
Darstack 12:b8ee724b42b1 213 write_data(y1 >> 8);
Darstack 12:b8ee724b42b1 214 write_data(y1);
Darstack 12:b8ee724b42b1 215
Darstack 12:b8ee724b42b1 216 write_cmd(0x2C); // Memory Write
Darstack 12:b8ee724b42b1 217
Darstack 12:b8ee724b42b1 218 wait_us(20);
Darstack 12:b8ee724b42b1 219
Darstack 12:b8ee724b42b1 220 dc = 1;
Darstack 12:b8ee724b42b1 221 }
Darstack 12:b8ee724b42b1 222
Darstack 12:b8ee724b42b1 223 /*-------------------------------------------------------------------*/
Darstack 12:b8ee724b42b1 224 /* Clear screen */
Darstack 12:b8ee724b42b1 225 /*-------------------------------------------------------------------*/
Darstack 12:b8ee724b42b1 226 void tft_clear(uint16_t color)
Darstack 12:b8ee724b42b1 227 {
Darstack 12:b8ee724b42b1 228 tft_set_window(0, 0, TFT_WIDTH, TFT_HEIGHT);
Darstack 12:b8ee724b42b1 229
Darstack 12:b8ee724b42b1 230 for (int i = 0; i < TFT_WIDTH * TFT_HEIGHT; ++i)
Darstack 12:b8ee724b42b1 231 {
Darstack 12:b8ee724b42b1 232 spi.write(color & 0xff);
Darstack 12:b8ee724b42b1 233 spi.write(color >> 8);
Darstack 12:b8ee724b42b1 234 }
Darstack 12:b8ee724b42b1 235 }
Darstack 12:b8ee724b42b1 236
Darstack 12:b8ee724b42b1 237
Darstack 12:b8ee724b42b1 238
Darstack 12:b8ee724b42b1 239
Darstack 12:b8ee724b42b1 240
Darstack 12:b8ee724b42b1 241
Darstack 12:b8ee724b42b1 242
Darstack 12:b8ee724b42b1 243
Darstack 12:b8ee724b42b1 244
Darstack 12:b8ee724b42b1 245
Darstack 12:b8ee724b42b1 246
Darstack 12:b8ee724b42b1 247
Darstack 12:b8ee724b42b1 248
Darstack 12:b8ee724b42b1 249
Darstack 12:b8ee724b42b1 250
Darstack 12:b8ee724b42b1 251
Darstack 12:b8ee724b42b1 252
Darstack 12:b8ee724b42b1 253
Darstack 12:b8ee724b42b1 254
Darstack 12:b8ee724b42b1 255
Darstack 12:b8ee724b42b1 256
Darstack 12:b8ee724b42b1 257
fabeltranm 0:fa6bdbdd4239 258 int main() {
fabeltranm 0:fa6bdbdd4239 259
fabeltranm 0:fa6bdbdd4239 260 setup_uart();
Darstack 12:b8ee724b42b1 261 tk1.attach (&leer_color, 10); // Se dirigue a la funcion leer_color cada 0.8 segundos
Darstack 11:6b3570e525a1 262 Button_joy.fall(callback(Button_joy_onpressed_cb)); // Llama a la funcion Button_joy_onpressed_cb al precionar el botón
Darstack 12:b8ee724b42b1 263 spi.frequency(45000000);
Darstack 11:6b3570e525a1 264
Darstack 12:b8ee724b42b1 265 while(1){
Darstack 12:b8ee724b42b1 266
Darstack 12:b8ee724b42b1 267 tft_reset();
Darstack 11:6b3570e525a1 268 //Interrupcion boton
fabeltranm 0:fa6bdbdd4239 269 leer_datos();
Darstack 12:b8ee724b42b1 270 funcionesrobot (Lectura [ 1 ], Lectura [ 0 ]);
Darstack 12:b8ee724b42b1 271 //Lcd();
Darstack 4:7a06dce3de99 272 }
fabeltranm 0:fa6bdbdd4239 273 }
fabeltranm 0:fa6bdbdd4239 274
Darstack 11:6b3570e525a1 275 //Declaracion de las funciones
Darstack 11:6b3570e525a1 276
fabeltranm 0:fa6bdbdd4239 277
fabeltranm 0:fa6bdbdd4239 278 void setup_uart(){
fabeltranm 0:fa6bdbdd4239 279 command.baud(115200);
fabeltranm 0:fa6bdbdd4239 280 }
fabeltranm 0:fa6bdbdd4239 281
fabeltranm 0:fa6bdbdd4239 282
fabeltranm 0:fa6bdbdd4239 283 void leer_datos(){
Darstack 4:7a06dce3de99 284
Darstack 6:35c069133c9a 285 while ( command.getc()!= INITCMD ) ;
Darstack 4:7a06dce3de99 286 uint8_t i ;
Darstack 6:35c069133c9a 287
Darstack 4:7a06dce3de99 288 for ( i = 0 ; i < 2 ; i++){
Darstack 4:7a06dce3de99 289
Darstack 6:35c069133c9a 290 Lectura [ i ] = command.getc ();
Darstack 12:b8ee724b42b1 291 printf ( " %4d ", Lectura [ i ]);
Darstack 6:35c069133c9a 292
Darstack 4:7a06dce3de99 293 }
fabeltranm 0:fa6bdbdd4239 294 }
fabeltranm 0:fa6bdbdd4239 295
Darstack 11:6b3570e525a1 296 void Rx_interrupt() {
Darstack 12:b8ee724b42b1 297
Darstack 12:b8ee724b42b1 298 if ( command.readable () == 1 ){
Darstack 12:b8ee724b42b1 299
Darstack 12:b8ee724b42b1 300 uint8_t l1, l2 ;
Darstack 12:b8ee724b42b1 301
Darstack 12:b8ee724b42b1 302 while ( command.getc()!= INITCMD ) ;
Darstack 12:b8ee724b42b1 303 l1 = command.getc () ;
Darstack 12:b8ee724b42b1 304 l2 = command.getc () ;
Darstack 12:b8ee724b42b1 305
Darstack 12:b8ee724b42b1 306 if ( l1 == 10 and l2 == 2 ){
Darstack 12:b8ee724b42b1 307
Darstack 12:b8ee724b42b1 308 //NVIC_SystemReset();
Darstack 12:b8ee724b42b1 309 Exit = 1 ;
Darstack 12:b8ee724b42b1 310 }
Darstack 12:b8ee724b42b1 311
Darstack 12:b8ee724b42b1 312 }
Darstack 11:6b3570e525a1 313 }
fabeltranm 0:fa6bdbdd4239 314 void leer_color(){
fabeltranm 0:fa6bdbdd4239 315
fabeltranm 0:fa6bdbdd4239 316 long red = scolor.ReadRed();
fabeltranm 0:fa6bdbdd4239 317 long green = scolor.ReadGreen();
fabeltranm 0:fa6bdbdd4239 318 long blue = scolor.ReadBlue();
fabeltranm 0:fa6bdbdd4239 319 long clear = scolor.ReadClear();
Darstack 2:d168f964c331 320
Darstack 2:d168f964c331 321 long frqred;
Darstack 2:d168f964c331 322 long frqgreen;
Darstack 2:d168f964c331 323 long frqblue;
Darstack 2:d168f964c331 324 long frqclear;
Darstack 4:7a06dce3de99 325 int8_t sel_color = 0;
fabeltranm 1:334b40b47233 326 printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", red, green, blue, clear);
fabeltranm 0:fa6bdbdd4239 327
Darstack 2:d168f964c331 328 frqred = ( ( 1.0/red )* 1000.0 );
Darstack 2:d168f964c331 329 frqgreen = ( ( 1.0/green ) * 1000.0);
Darstack 2:d168f964c331 330 frqblue = ( (1.0/blue) *1000.0 );
Darstack 2:d168f964c331 331 frqclear = ( (1.0/clear) *1000.0 );
Darstack 3:3454cb7584e1 332 printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", frqred, frqgreen, frqblue,frqclear);
Darstack 2:d168f964c331 333 ///////////////////////////////////////////////////////////////////////////////
Darstack 2:d168f964c331 334 /*||||||||||||||||Seleccionando los diferentes colores.||||||||||||||||||||||*/
Darstack 2:d168f964c331 335 /*||||||||||||||||||||||||||||||Color rojo|||||||||||||||||||||||||||||||||||*/
Darstack 2:d168f964c331 336 ///////////////////////////////////////////////////////////////////////////////
Darstack 3:3454cb7584e1 337 if ( frqred >= 30.0 and frqred <= 500.0) {
Darstack 3:3454cb7584e1 338
Darstack 3:3454cb7584e1 339 if( frqgreen >= 0.0 and frqgreen <= 20.0 ) {
Darstack 3:3454cb7584e1 340
Darstack 3:3454cb7584e1 341 if ( frqblue >= 5.0 and frqblue <= 29.0 ) {
Darstack 2:d168f964c331 342
Darstack 4:7a06dce3de99 343 sel_color = 1;
Darstack 3:3454cb7584e1 344
Darstack 2:d168f964c331 345 }
Darstack 2:d168f964c331 346 }
Darstack 2:d168f964c331 347 }
Darstack 2:d168f964c331 348 ////////////////////////////////////////////////////////////////////////////////
Darstack 3:3454cb7584e1 349 /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°Color verde°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/
Darstack 3:3454cb7584e1 350 ////////////////////////////////////////////////////////////////////////////////
Darstack 3:3454cb7584e1 351 if ( frqgreen >= 11.0 and frqgreen <= 21.0) {
Darstack 3:3454cb7584e1 352
Darstack 3:3454cb7584e1 353 if( frqred >= 8.0 and frqred <= 20.0 ) {
Darstack 3:3454cb7584e1 354
Darstack 4:7a06dce3de99 355 if ( frqblue >= 10.0 and frqblue <= 26.0 ) {
Darstack 2:d168f964c331 356
Darstack 4:7a06dce3de99 357 sel_color = 2;
Darstack 3:3454cb7584e1 358
Darstack 2:d168f964c331 359 }
Darstack 2:d168f964c331 360 }
Darstack 2:d168f964c331 361 }
Darstack 2:d168f964c331 362 ////////////////////////////////////////////////////////////////////////////////
Darstack 3:3454cb7584e1 363 /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°Color azul°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/
Darstack 2:d168f964c331 364 ///////////////////////////////////////////////////////////////////////////////
Darstack 3:3454cb7584e1 365 if ( frqblue >= 10.0 and frqblue <= 70.0) {
Darstack 3:3454cb7584e1 366
Darstack 3:3454cb7584e1 367 if( frqgreen >= 5.0 and frqgreen <= 26.0 ) {
Darstack 2:d168f964c331 368
Darstack 4:7a06dce3de99 369 if ( frqred >= 5.0 and frqred <= 20.0 ) {
Darstack 3:3454cb7584e1 370
Darstack 4:7a06dce3de99 371 sel_color = 3;
Darstack 3:3454cb7584e1 372
Darstack 2:d168f964c331 373 }
Darstack 2:d168f964c331 374 }
Darstack 2:d168f964c331 375 }
Darstack 2:d168f964c331 376 /////////////////////////////////////////////////////////////////////////////////
Darstack 2:d168f964c331 377 /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°Color amarillo°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/
Darstack 2:d168f964c331 378 ////////////////////////////////////////////////////////////////////////////////
Darstack 4:7a06dce3de99 379 if ( frqblue >= 0.0 and frqblue <= 30.0 ) {
Darstack 3:3454cb7584e1 380
Darstack 3:3454cb7584e1 381 if( frqgreen >= 20.0 and frqgreen <= 40.0) {
Darstack 3:3454cb7584e1 382
Darstack 4:7a06dce3de99 383 if ( frqred >= 20.0 and frqred <= 50.0 ) {
Darstack 11:6b3570e525a1 384
Darstack 4:7a06dce3de99 385 sel_color = 4;
Darstack 3:3454cb7584e1 386
Darstack 2:d168f964c331 387 }
Darstack 2:d168f964c331 388 }
Darstack 2:d168f964c331 389 }
Darstack 2:d168f964c331 390 /////////////////////////////////////////////////////////////////////////////////
Darstack 2:d168f964c331 391 /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°Color no found°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/
Darstack 2:d168f964c331 392 ////////////////////////////////////////////////////////////////////////////////
Darstack 3:3454cb7584e1 393
Darstack 3:3454cb7584e1 394 switch ( sel_color ) {
Darstack 3:3454cb7584e1 395
Darstack 6:35c069133c9a 396 case 0 :
Darstack 4:7a06dce3de99 397
Darstack 4:7a06dce3de99 398 //int32_t enviar5 = RESPUESTA5 ;
Darstack 4:7a06dce3de99 399 //char txt5 [6] ;
Darstack 4:7a06dce3de99 400 //printf ( txt5 , "%02X" , enviar5 );
Darstack 11:6b3570e525a1 401 command.putc( 0xFE );
Darstack 11:6b3570e525a1 402 command.putc( 0x00 );
Darstack 12:b8ee724b42b1 403 // tft_reset();
Darstack 12:b8ee724b42b1 404 tft_clear(TFT_WHITE);
Darstack 12:b8ee724b42b1 405
Darstack 4:7a06dce3de99 406
Darstack 6:35c069133c9a 407 break ;
Darstack 4:7a06dce3de99 408
Darstack 6:35c069133c9a 409 case 1 :
Darstack 3:3454cb7584e1 410
Darstack 4:7a06dce3de99 411 //int32_t enviar1 = RESPUESTA1 ;
Darstack 4:7a06dce3de99 412 //char txt1 [6] ;
Darstack 11:6b3570e525a1 413 command.putc( 0xFE );
Darstack 11:6b3570e525a1 414 command.putc( 0x01 );
Darstack 12:b8ee724b42b1 415 sel_color = 0;
Darstack 12:b8ee724b42b1 416 // tft_reset();
Darstack 12:b8ee724b42b1 417 tft_clear(TFT_RED);
Darstack 3:3454cb7584e1 418
Darstack 6:35c069133c9a 419 break ;
Darstack 3:3454cb7584e1 420
Darstack 6:35c069133c9a 421 case 2 :
Darstack 3:3454cb7584e1 422
Darstack 4:7a06dce3de99 423 //int32_t enviar2 = RESPUESTA2 ;
Darstack 4:7a06dce3de99 424 //char txt2 [6] ;
Darstack 11:6b3570e525a1 425 command.putc ( 0xFE );
Darstack 11:6b3570e525a1 426 command.putc ( 0x02 );
Darstack 4:7a06dce3de99 427 sel_color = 0;
Darstack 12:b8ee724b42b1 428 // tft_reset();
Darstack 12:b8ee724b42b1 429 tft_clear(TFT_GREEN);
Darstack 3:3454cb7584e1 430
Darstack 6:35c069133c9a 431 break ;
Darstack 6:35c069133c9a 432
Darstack 6:35c069133c9a 433 case 3 :
Darstack 2:d168f964c331 434
Darstack 4:7a06dce3de99 435 //int32_t enviar3 = RESPUESTA3;
Darstack 4:7a06dce3de99 436 //char txt3 [6] ;
Darstack 11:6b3570e525a1 437 command.putc ( 0xFE );
Darstack 11:6b3570e525a1 438 command.putc ( 0x03 );
Darstack 4:7a06dce3de99 439 sel_color = 0;
Darstack 3:3454cb7584e1 440
Darstack 12:b8ee724b42b1 441 //tft_reset();
Darstack 12:b8ee724b42b1 442 tft_clear(TFT_BLUE);
Darstack 12:b8ee724b42b1 443
Darstack 12:b8ee724b42b1 444
Darstack 6:35c069133c9a 445 break ;
Darstack 3:3454cb7584e1 446
Darstack 6:35c069133c9a 447 case 4 :
Darstack 3:3454cb7584e1 448
Darstack 4:7a06dce3de99 449 //int32_t enviar4 = RESPUESTA4;
Darstack 4:7a06dce3de99 450 //char txt4 [6] ;
Darstack 11:6b3570e525a1 451 command.putc ( 0xFE );
Darstack 11:6b3570e525a1 452 command.putc ( 0x04 );
Darstack 4:7a06dce3de99 453 sel_color = 0;
Darstack 12:b8ee724b42b1 454 //tft_reset();
Darstack 12:b8ee724b42b1 455 tft_clear(TFT_YELLOW);
Darstack 12:b8ee724b42b1 456
Darstack 6:35c069133c9a 457 break ;
Darstack 2:d168f964c331 458 }
fabeltranm 0:fa6bdbdd4239 459
Darstack 3:3454cb7584e1 460 }
Darstack 12:b8ee724b42b1 461 void funcionesrobot ( uint8_t _Parametro, uint8_t _Comando ){
Darstack 4:7a06dce3de99 462
Darstack 4:7a06dce3de99 463 /* °°Declaración de contadores°° */
Darstack 6:35c069133c9a 464 uint8_t i ;
Darstack 6:35c069133c9a 465 uint8_t j ;
Darstack 4:7a06dce3de99 466
Darstack 4:7a06dce3de99 467 switch ( _Comando ){
Darstack 4:7a06dce3de99 468 // Acciones que ejerce el robot
Darstack 4:7a06dce3de99 469
Darstack 12:b8ee724b42b1 470 case 0 :
Darstack 6:35c069133c9a 471
Darstack 12:b8ee724b42b1 472 tft_reset();
Darstack 12:b8ee724b42b1 473
Darstack 12:b8ee724b42b1 474 tft_clear(TFT_RED);
Darstack 12:b8ee724b42b1 475
Darstack 12:b8ee724b42b1 476 wait(1);
Darstack 12:b8ee724b42b1 477
Darstack 12:b8ee724b42b1 478 tft_clear(TFT_GREEN);
Darstack 12:b8ee724b42b1 479
Darstack 12:b8ee724b42b1 480 wait(1);
Darstack 6:35c069133c9a 481
Darstack 12:b8ee724b42b1 482 tft_clear(TFT_BLUE);
Darstack 12:b8ee724b42b1 483
Darstack 12:b8ee724b42b1 484 wait(1);
Darstack 12:b8ee724b42b1 485
Darstack 12:b8ee724b42b1 486 tft_clear(TFT_YELLOW);
Darstack 12:b8ee724b42b1 487
Darstack 12:b8ee724b42b1 488 wait(1);
Darstack 12:b8ee724b42b1 489
Darstack 12:b8ee724b42b1 490 break ;
Darstack 4:7a06dce3de99 491
Darstack 4:7a06dce3de99 492 case 1 :
Darstack 6:35c069133c9a 493
Darstack 6:35c069133c9a 494 printf ( "Frecuencia: 2000 Tiempo: %d s \n" , _Parametro ) ;
Darstack 6:35c069133c9a 495 mybuzzer.period_us ( DO ) ;
Darstack 6:35c069133c9a 496 mybuzzer.write ( 0.8 ) ;
Darstack 6:35c069133c9a 497 wait( _Parametro ) ;
Darstack 6:35c069133c9a 498 mybuzzer.write ( 0 ) ;
Darstack 6:35c069133c9a 499
Darstack 4:7a06dce3de99 500 break ;
Darstack 4:7a06dce3de99 501
Darstack 4:7a06dce3de99 502 case 2 :
Darstack 6:35c069133c9a 503
Darstack 6:35c069133c9a 504 printf ( "Frecuencia: 2500 Tiempo: %d s \n" , _Parametro ) ;
Darstack 6:35c069133c9a 505 mybuzzer.period_us ( RE ) ;
Darstack 6:35c069133c9a 506 mybuzzer.write ( 0.8 ) ;
Darstack 6:35c069133c9a 507 wait ( _Parametro ) ;
Darstack 6:35c069133c9a 508 mybuzzer.write ( 0 ) ;
Darstack 6:35c069133c9a 509
Darstack 4:7a06dce3de99 510 break ;
Darstack 4:7a06dce3de99 511
Darstack 4:7a06dce3de99 512 case 3 :
Darstack 6:35c069133c9a 513
Darstack 6:35c069133c9a 514 printf ( " Frecuencia: 3000 " ); printf ( " Tiempo: %d s \n " , _Parametro ) ;
Darstack 6:35c069133c9a 515 mybuzzer.period_us ( MI ) ;
Darstack 6:35c069133c9a 516 mybuzzer.write ( 0.8 ) ;
Darstack 6:35c069133c9a 517 wait( _Parametro ) ;
Darstack 6:35c069133c9a 518 mybuzzer.write ( 0 ) ;
Darstack 6:35c069133c9a 519
Darstack 4:7a06dce3de99 520 break ;
Darstack 4:7a06dce3de99 521
Darstack 6:35c069133c9a 522 case 4 :
Darstack 6:35c069133c9a 523
Darstack 6:35c069133c9a 524 printf ( "Frecuencia: 3500 Tiempo: %d s \n" , _Parametro ) ;
Darstack 6:35c069133c9a 525 mybuzzer.period_us ( FA );
Darstack 6:35c069133c9a 526 mybuzzer.write ( 0.8 );
Darstack 6:35c069133c9a 527 wait ( 5 );
Darstack 6:35c069133c9a 528 mybuzzer.write ( 0 );
Darstack 6:35c069133c9a 529
Darstack 4:7a06dce3de99 530 break ;
Darstack 4:7a06dce3de99 531
Darstack 4:7a06dce3de99 532 case 5 :
Darstack 4:7a06dce3de99 533
Darstack 6:35c069133c9a 534 steppeer_dir = 1 ;
Darstack 6:35c069133c9a 535 steppeer_dir2 = 0 ;
Darstack 6:35c069133c9a 536 wait_us ( 1 );
Darstack 6:35c069133c9a 537 for ( j = 1 ; j <= _Parametro ; j++){
Darstack 4:7a06dce3de99 538
Darstack 6:35c069133c9a 539 for ( i= 0 ; i <= 200 ; i++ ){
Darstack 4:7a06dce3de99 540
Darstack 12:b8ee724b42b1 541 stepper_step = 1 ;
Darstack 6:35c069133c9a 542 stepper_step2 = 1;
Darstack 6:35c069133c9a 543 wait_us( VELOCITY );
Darstack 6:35c069133c9a 544 stepper_step = 0;
Darstack 6:35c069133c9a 545 stepper_step2 = 0;
Darstack 6:35c069133c9a 546 wait_us ( VELOCITY );
Darstack 4:7a06dce3de99 547
Darstack 4:7a06dce3de99 548 }
Darstack 4:7a06dce3de99 549 }
Darstack 4:7a06dce3de99 550
Darstack 6:35c069133c9a 551 break ;
Darstack 6:35c069133c9a 552
Darstack 6:35c069133c9a 553 case 6 :
Darstack 6:35c069133c9a 554
Darstack 6:35c069133c9a 555 steppeer_dir = 0;
Darstack 6:35c069133c9a 556 steppeer_dir2 = 1;
Darstack 6:35c069133c9a 557 wait_us ( 1 );
Darstack 6:35c069133c9a 558
Darstack 6:35c069133c9a 559 for ( j = 1 ; j <= _Parametro ; j++){
Darstack 6:35c069133c9a 560
Darstack 6:35c069133c9a 561 for ( i= 0 ; i <= 200 ; i++ ){
Darstack 4:7a06dce3de99 562
Darstack 6:35c069133c9a 563 stepper_step = 1 ;
Darstack 4:7a06dce3de99 564 stepper_step2 = 1 ;
Darstack 4:7a06dce3de99 565 wait_us( VELOCITY ) ;
Darstack 4:7a06dce3de99 566 stepper_step = 0 ;
Darstack 4:7a06dce3de99 567 stepper_step2 = 0 ;
Darstack 5:d165935ba818 568 wait_us ( VELOCITY ) ;
Darstack 4:7a06dce3de99 569
Darstack 4:7a06dce3de99 570 }
Darstack 6:35c069133c9a 571
Darstack 5:d165935ba818 572 }
Darstack 4:7a06dce3de99 573
Darstack 6:35c069133c9a 574 break ;
Darstack 6:35c069133c9a 575
Darstack 6:35c069133c9a 576 case 7 :
Darstack 6:35c069133c9a 577
Darstack 6:35c069133c9a 578 steppeer_dir = 0;
Darstack 6:35c069133c9a 579 wait_us ( 1 );
Darstack 6:35c069133c9a 580
Darstack 6:35c069133c9a 581 for ( j = 1 ; j <= _Parametro ; j++){
Darstack 6:35c069133c9a 582
Darstack 11:6b3570e525a1 583 for ( i= 0 ; i <= 250 ; i++ ){
Darstack 6:35c069133c9a 584
Darstack 6:35c069133c9a 585 stepper_step = 1 ;
Darstack 6:35c069133c9a 586 //stepper_step2 = 1 ;
Darstack 6:35c069133c9a 587 wait_us(VELOCITY) ;
Darstack 6:35c069133c9a 588 stepper_step = 0 ;
Darstack 6:35c069133c9a 589 //stepper_step2 = 0 ;
Darstack 6:35c069133c9a 590 wait_us(VELOCITY) ;
Darstack 6:35c069133c9a 591
Darstack 6:35c069133c9a 592 }
Darstack 6:35c069133c9a 593 }
Darstack 6:35c069133c9a 594
Darstack 6:35c069133c9a 595 break ;
Darstack 6:35c069133c9a 596
Darstack 6:35c069133c9a 597 case 8 :
Darstack 6:35c069133c9a 598
Darstack 6:35c069133c9a 599 steppeer_dir2 = 1;
Darstack 6:35c069133c9a 600 wait_us ( 1 );
Darstack 6:35c069133c9a 601
Darstack 6:35c069133c9a 602 for ( j = 1 ; j <= _Parametro ; j++){
Darstack 6:35c069133c9a 603
Darstack 11:6b3570e525a1 604 for ( i= 0 ; i <= 250 ; i++ ){
Darstack 6:35c069133c9a 605
Darstack 6:35c069133c9a 606 //stepper_step = 1 ;
Darstack 6:35c069133c9a 607 stepper_step2 = 1 ;
Darstack 6:35c069133c9a 608 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 609 //stepper_step = 0 ;
Darstack 6:35c069133c9a 610 stepper_step2 = 0 ;
Darstack 6:35c069133c9a 611 wait_us ( VELOCITY ) ;
Darstack 6:35c069133c9a 612
Darstack 6:35c069133c9a 613 }
Darstack 6:35c069133c9a 614
Darstack 6:35c069133c9a 615 }
Darstack 6:35c069133c9a 616
Darstack 6:35c069133c9a 617 break ;
Darstack 6:35c069133c9a 618
Darstack 6:35c069133c9a 619 case 9 :
Darstack 6:35c069133c9a 620
Darstack 6:35c069133c9a 621 switch ( _Parametro ){
Darstack 6:35c069133c9a 622
Darstack 6:35c069133c9a 623 case 1:
Darstack 6:35c069133c9a 624
Darstack 6:35c069133c9a 625 VELOCITY = 400 ;
Darstack 6:35c069133c9a 626
Darstack 6:35c069133c9a 627 break ;
Darstack 6:35c069133c9a 628
Darstack 6:35c069133c9a 629 case 2:
Darstack 6:35c069133c9a 630
Darstack 6:35c069133c9a 631 VELOCITY = 2500 ;
Darstack 6:35c069133c9a 632
Darstack 6:35c069133c9a 633 break ;
Darstack 6:35c069133c9a 634
Darstack 6:35c069133c9a 635 case 3:
Darstack 6:35c069133c9a 636
Darstack 6:35c069133c9a 637 VELOCITY = 5000 ;
Darstack 6:35c069133c9a 638
Darstack 6:35c069133c9a 639 break ;
Darstack 6:35c069133c9a 640
Darstack 6:35c069133c9a 641 }
Darstack 6:35c069133c9a 642
Darstack 6:35c069133c9a 643 break ;
Darstack 6:35c069133c9a 644
Darstack 6:35c069133c9a 645 case 10:
Darstack 6:35c069133c9a 646
Darstack 11:6b3570e525a1 647
Darstack 6:35c069133c9a 648 if ( _Parametro == 1){
Darstack 6:35c069133c9a 649
Darstack 6:35c069133c9a 650 while ( !Exit ){
Darstack 6:35c069133c9a 651
Darstack 6:35c069133c9a 652 In [ 0 ] = analog_value0.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Darstack 12:b8ee724b42b1 653 // printf(" X = %.04f \n", In[0]);
Darstack 10:92350e68d5ee 654 if ( In [ 0 ] > 0.6 ){
Darstack 6:35c069133c9a 655
Darstack 6:35c069133c9a 656 steppeer_dir = 1;
Darstack 6:35c069133c9a 657 wait_us( 1 );
Darstack 6:35c069133c9a 658
Darstack 6:35c069133c9a 659 for ( i= 0 ; i <= 50 ; i++ ){
Darstack 6:35c069133c9a 660
Darstack 6:35c069133c9a 661 stepper_step = 1 ;
Darstack 6:35c069133c9a 662 //stepper_step2 = 1 ;
Darstack 6:35c069133c9a 663 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 664 stepper_step = 0 ;
Darstack 6:35c069133c9a 665 //stepper_step2 = 0 ;
Darstack 6:35c069133c9a 666 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 667 }
Darstack 6:35c069133c9a 668 }
Darstack 6:35c069133c9a 669
Darstack 10:92350e68d5ee 670 if ( In [ 0 ] < 0.3 ){
Darstack 6:35c069133c9a 671
Darstack 6:35c069133c9a 672 steppeer_dir2 = 1;
Darstack 6:35c069133c9a 673 wait_us( 1 );
Darstack 6:35c069133c9a 674
Darstack 6:35c069133c9a 675 for ( i= 0 ; i <= 50 ; i++ ){
Darstack 6:35c069133c9a 676
Darstack 6:35c069133c9a 677 //stepper_step = 1 ;
Darstack 6:35c069133c9a 678 stepper_step2 = 1 ;
Darstack 6:35c069133c9a 679 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 680 //stepper_step = 0 ;
Darstack 6:35c069133c9a 681 stepper_step2 = 0 ;
Darstack 6:35c069133c9a 682 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 683
Darstack 6:35c069133c9a 684 }
Darstack 6:35c069133c9a 685 }
Darstack 6:35c069133c9a 686
Darstack 6:35c069133c9a 687 In [ 1 ] = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Darstack 9:b7f0cf7e9079 688 //printf(" Y = %.04f \n", In[0]);
Darstack 6:35c069133c9a 689
Darstack 10:92350e68d5ee 690 if (In [ 1 ] > 0.6 ){
Darstack 6:35c069133c9a 691
Darstack 6:35c069133c9a 692 steppeer_dir = 1 ;
Darstack 6:35c069133c9a 693 steppeer_dir2 = 0 ;
Darstack 6:35c069133c9a 694 wait_us( 1 );
Darstack 6:35c069133c9a 695
Darstack 6:35c069133c9a 696 for ( i= 0 ; i <= 50 ; i++ ){
Darstack 6:35c069133c9a 697
Darstack 6:35c069133c9a 698 stepper_step = 1 ;
Darstack 6:35c069133c9a 699 stepper_step2 = 1 ;
Darstack 6:35c069133c9a 700 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 701 stepper_step = 0 ;
Darstack 6:35c069133c9a 702 stepper_step2 = 0 ;
Darstack 6:35c069133c9a 703 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 704
Darstack 6:35c069133c9a 705 }
Darstack 6:35c069133c9a 706 }
Darstack 6:35c069133c9a 707
Darstack 10:92350e68d5ee 708 if ( In [ 1 ] < 0.3 ){
Darstack 6:35c069133c9a 709
Darstack 6:35c069133c9a 710 steppeer_dir = 0 ;
Darstack 6:35c069133c9a 711 steppeer_dir2 = 1 ;
Darstack 6:35c069133c9a 712 wait_us( 1 );
Darstack 6:35c069133c9a 713
Darstack 6:35c069133c9a 714 for ( i= 0 ; i <= 50 ; i++ ){
Darstack 6:35c069133c9a 715
Darstack 6:35c069133c9a 716 stepper_step = 1 ;
Darstack 6:35c069133c9a 717 stepper_step2 = 1 ;
Darstack 6:35c069133c9a 718 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 719 stepper_step = 0 ;
Darstack 6:35c069133c9a 720 stepper_step2 = 0 ;
Darstack 6:35c069133c9a 721 wait_us ( VELOCITY ) ;
Darstack 6:35c069133c9a 722
Darstack 6:35c069133c9a 723 }
Darstack 6:35c069133c9a 724
Darstack 6:35c069133c9a 725 }
Darstack 6:35c069133c9a 726
Darstack 11:6b3570e525a1 727 command.attach( callback(Rx_interrupt), Serial::RxIrq );
Darstack 11:6b3570e525a1 728 /*if ( command.readable () == 1 ){
Darstack 8:a4fb8f46fc00 729
Darstack 8:a4fb8f46fc00 730 uint8_t l1, l2 ;
Darstack 8:a4fb8f46fc00 731
Darstack 8:a4fb8f46fc00 732 while ( command.getc()!= INITCMD ) ;
Darstack 8:a4fb8f46fc00 733 l1 = command.getc () ;
Darstack 8:a4fb8f46fc00 734 l2 = command.getc () ;
Darstack 8:a4fb8f46fc00 735
Darstack 8:a4fb8f46fc00 736 if ( l1 == 10 and l2 == 2 ){
Darstack 6:35c069133c9a 737
Darstack 6:35c069133c9a 738 Exit = 1 ;
Darstack 6:35c069133c9a 739 }
Darstack 11:6b3570e525a1 740 }*/
Darstack 10:92350e68d5ee 741
Darstack 10:92350e68d5ee 742 wait_us ( 1 );
Darstack 10:92350e68d5ee 743
Darstack 5:d165935ba818 744 }
Darstack 6:35c069133c9a 745 }
Darstack 6:35c069133c9a 746
Darstack 6:35c069133c9a 747 break ;
Darstack 6:35c069133c9a 748
Darstack 4:7a06dce3de99 749 }
Darstack 12:b8ee724b42b1 750 }
Darstack 5:d165935ba818 751
Darstack 12:b8ee724b42b1 752