Einer Fabian Aponte Cubides / Mbed 2 deprecated Final3

sensor color con joystick y motores paso a paso mas pantalla LCD

Dependencies:   mbed

main.cpp@7:5f332f47b38f, 2019-04-23 (annotated)

Committer:
Darstack
Date:
Tue Apr 23 14:33:50 2019 +0000
Revision:
7:5f332f47b38f
Parent:
6:35c069133c9a
Child:
8:a4fb8f46fc00
Joystick3;

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
fabeltranm 0:fa6bdbdd4239 56
Darstack 4:7a06dce3de99 57 /* ||Definicion de Puertos || */
Darstack 4:7a06dce3de99 58 Serial command(USBTX, USBRX);
Darstack 4:7a06dce3de99 59 // S0, S1, S2, S3, OUT
Darstack 4:7a06dce3de99 60 scolor_TCS3200 scolor( PA_9, PC_7, PB_6, PA_7, PA_6 ) ;
Darstack 4:7a06dce3de99 61 DigitalOut stepper_step ( PB_4 ) ;
Darstack 4:7a06dce3de99 62 DigitalOut steppeer_dir ( PB_5 ) ;
Darstack 4:7a06dce3de99 63 DigitalOut stepper_step2 ( PB_10 ) ;
Darstack 4:7a06dce3de99 64 DigitalOut steppeer_dir2 ( PA_8 ) ;
Darstack 4:7a06dce3de99 65 AnalogIn analog_value0 ( A0 ) ;
Darstack 4:7a06dce3de99 66 AnalogIn analog_value1 ( A1 ) ;
Darstack 4:7a06dce3de99 67 PwmOut mybuzzer( PB_9 ) ;
fabeltranm 0:fa6bdbdd4239 68
Darstack 4:7a06dce3de99 69 /* ||Definicion de Puertos || */
Darstack 4:7a06dce3de99 70
Darstack 4:7a06dce3de99 71 uint32_t VELOCITY = 400 ; // Tiempo en micro segundos
Darstack 4:7a06dce3de99 72 int16_t Lectura [ 2 ] = {} ;
Darstack 5:d165935ba818 73 double In [ 1 ] = {} ;
Darstack 4:7a06dce3de99 74
fabeltranm 0:fa6bdbdd4239 75 // definición de las funciones
Darstack 5:d165935ba818 76 void setup_uart ();
Darstack 5:d165935ba818 77 void leer_datos ();
Darstack 5:d165935ba818 78 void leer_color ();
Darstack 4:7a06dce3de99 79 void funcionesrobot ( uint8_t _Parametro, uint8_t _Comando );
fabeltranm 0:fa6bdbdd4239 80
fabeltranm 0:fa6bdbdd4239 81 int main() {
fabeltranm 0:fa6bdbdd4239 82
fabeltranm 0:fa6bdbdd4239 83 setup_uart();
Darstack 6:35c069133c9a 84 while(1){
Darstack 6:35c069133c9a 85
fabeltranm 0:fa6bdbdd4239 86 leer_datos();
Darstack 4:7a06dce3de99 87 funcionesrobot ( Lectura [ 1 ], Lectura [ 0 ] );
Darstack 4:7a06dce3de99 88
Darstack 4:7a06dce3de99 89 }
fabeltranm 0:fa6bdbdd4239 90 }
fabeltranm 0:fa6bdbdd4239 91
fabeltranm 0:fa6bdbdd4239 92
fabeltranm 0:fa6bdbdd4239 93 void setup_uart(){
fabeltranm 0:fa6bdbdd4239 94 command.baud(115200);
fabeltranm 0:fa6bdbdd4239 95 }
fabeltranm 0:fa6bdbdd4239 96
fabeltranm 0:fa6bdbdd4239 97
fabeltranm 0:fa6bdbdd4239 98 void leer_datos(){
Darstack 4:7a06dce3de99 99
Darstack 6:35c069133c9a 100 while ( command.getc()!= INITCMD ) ;
Darstack 4:7a06dce3de99 101 uint8_t i ;
Darstack 6:35c069133c9a 102
Darstack 4:7a06dce3de99 103 for ( i = 0 ; i < 2 ; i++){
Darstack 4:7a06dce3de99 104
Darstack 6:35c069133c9a 105 Lectura [ i ] = command.getc ();
Darstack 5:d165935ba818 106 //printf ( " %4d ", Lectura [ i ]);
Darstack 6:35c069133c9a 107
Darstack 4:7a06dce3de99 108 }
fabeltranm 0:fa6bdbdd4239 109 }
fabeltranm 0:fa6bdbdd4239 110
fabeltranm 0:fa6bdbdd4239 111
fabeltranm 0:fa6bdbdd4239 112 void leer_color(){
fabeltranm 0:fa6bdbdd4239 113
Darstack 2:d168f964c331 114 mybuzzer.write(0);
fabeltranm 0:fa6bdbdd4239 115 long red = scolor.ReadRed();
fabeltranm 0:fa6bdbdd4239 116 long green = scolor.ReadGreen();
fabeltranm 0:fa6bdbdd4239 117 long blue = scolor.ReadBlue();
fabeltranm 0:fa6bdbdd4239 118 long clear = scolor.ReadClear();
Darstack 2:d168f964c331 119
Darstack 2:d168f964c331 120 long frqred;
Darstack 2:d168f964c331 121 long frqgreen;
Darstack 2:d168f964c331 122 long frqblue;
Darstack 2:d168f964c331 123 long frqclear;
Darstack 4:7a06dce3de99 124 int8_t sel_color = 0;
fabeltranm 1:334b40b47233 125 printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", red, green, blue, clear);
fabeltranm 0:fa6bdbdd4239 126
Darstack 2:d168f964c331 127 frqred = ( ( 1.0/red )* 1000.0 );
Darstack 2:d168f964c331 128 frqgreen = ( ( 1.0/green ) * 1000.0);
Darstack 2:d168f964c331 129 frqblue = ( (1.0/blue) *1000.0 );
Darstack 2:d168f964c331 130 frqclear = ( (1.0/clear) *1000.0 );
Darstack 3:3454cb7584e1 131 printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", frqred, frqgreen, frqblue,frqclear);
Darstack 2:d168f964c331 132 ///////////////////////////////////////////////////////////////////////////////
Darstack 2:d168f964c331 133 /*||||||||||||||||Seleccionando los diferentes colores.||||||||||||||||||||||*/
Darstack 2:d168f964c331 134 /*||||||||||||||||||||||||||||||Color rojo|||||||||||||||||||||||||||||||||||*/
Darstack 2:d168f964c331 135 ///////////////////////////////////////////////////////////////////////////////
Darstack 3:3454cb7584e1 136 if ( frqred >= 30.0 and frqred <= 500.0) {
Darstack 3:3454cb7584e1 137
Darstack 3:3454cb7584e1 138 if( frqgreen >= 0.0 and frqgreen <= 20.0 ) {
Darstack 3:3454cb7584e1 139
Darstack 3:3454cb7584e1 140 if ( frqblue >= 5.0 and frqblue <= 29.0 ) {
Darstack 2:d168f964c331 141
Darstack 4:7a06dce3de99 142 printf ( "tiende a rojo \n" );
Darstack 2:d168f964c331 143 mybuzzer.period_us(DO);
Darstack 6:35c069133c9a 144 mybuzzer.write (0.8);
Darstack 6:35c069133c9a 145 wait (5);
Darstack 6:35c069133c9a 146 mybuzzer.write (0);
Darstack 4:7a06dce3de99 147 sel_color = 1;
Darstack 3:3454cb7584e1 148
Darstack 2:d168f964c331 149 }
Darstack 2:d168f964c331 150 }
Darstack 2:d168f964c331 151 }
Darstack 2:d168f964c331 152 ////////////////////////////////////////////////////////////////////////////////
Darstack 3:3454cb7584e1 153 /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°Color verde°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/
Darstack 3:3454cb7584e1 154 ////////////////////////////////////////////////////////////////////////////////
Darstack 3:3454cb7584e1 155 if ( frqgreen >= 11.0 and frqgreen <= 21.0) {
Darstack 3:3454cb7584e1 156
Darstack 3:3454cb7584e1 157 if( frqred >= 8.0 and frqred <= 20.0 ) {
Darstack 3:3454cb7584e1 158
Darstack 4:7a06dce3de99 159 if ( frqblue >= 10.0 and frqblue <= 26.0 ) {
Darstack 2:d168f964c331 160
Darstack 3:3454cb7584e1 161 printf ( "tiende a verde \n" );
Darstack 2:d168f964c331 162 mybuzzer.period_us(RE);
Darstack 6:35c069133c9a 163 mybuzzer.write (0.5);
Darstack 6:35c069133c9a 164 wait (5);
Darstack 6:35c069133c9a 165 mybuzzer.write (0);
Darstack 4:7a06dce3de99 166 sel_color = 2;
Darstack 3:3454cb7584e1 167
Darstack 2:d168f964c331 168 }
Darstack 2:d168f964c331 169 }
Darstack 2:d168f964c331 170 }
Darstack 2:d168f964c331 171 ////////////////////////////////////////////////////////////////////////////////
Darstack 3:3454cb7584e1 172 /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°Color azul°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/
Darstack 2:d168f964c331 173 ///////////////////////////////////////////////////////////////////////////////
Darstack 3:3454cb7584e1 174 if ( frqblue >= 10.0 and frqblue <= 70.0) {
Darstack 3:3454cb7584e1 175
Darstack 3:3454cb7584e1 176 if( frqgreen >= 5.0 and frqgreen <= 26.0 ) {
Darstack 2:d168f964c331 177
Darstack 4:7a06dce3de99 178 if ( frqred >= 5.0 and frqred <= 20.0 ) {
Darstack 3:3454cb7584e1 179
Darstack 4:7a06dce3de99 180 printf ( "tiende a azul \n" );
Darstack 6:35c069133c9a 181 mybuzzer.period_us (MI);
Darstack 6:35c069133c9a 182 mybuzzer.write (1);
Darstack 6:35c069133c9a 183 wait (5);
Darstack 6:35c069133c9a 184 mybuzzer.write (0);
Darstack 4:7a06dce3de99 185 sel_color = 3;
Darstack 3:3454cb7584e1 186
Darstack 2:d168f964c331 187 }
Darstack 2:d168f964c331 188 }
Darstack 2:d168f964c331 189 }
Darstack 2:d168f964c331 190 /////////////////////////////////////////////////////////////////////////////////
Darstack 2:d168f964c331 191 /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°Color amarillo°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/
Darstack 2:d168f964c331 192 ////////////////////////////////////////////////////////////////////////////////
Darstack 4:7a06dce3de99 193 if ( frqblue >= 0.0 and frqblue <= 30.0 ) {
Darstack 3:3454cb7584e1 194
Darstack 3:3454cb7584e1 195 if( frqgreen >= 20.0 and frqgreen <= 40.0) {
Darstack 3:3454cb7584e1 196
Darstack 4:7a06dce3de99 197 if ( frqred >= 20.0 and frqred <= 50.0 ) {
Darstack 2:d168f964c331 198
Darstack 3:3454cb7584e1 199 printf ( "tiende a amarillo \n" );
Darstack 6:35c069133c9a 200 mybuzzer.period_us (FA);
Darstack 6:35c069133c9a 201 mybuzzer.write (0.6);
Darstack 6:35c069133c9a 202 wait (5);
Darstack 6:35c069133c9a 203 mybuzzer.write( 0);
Darstack 4:7a06dce3de99 204 sel_color = 4;
Darstack 3:3454cb7584e1 205
Darstack 2:d168f964c331 206 }
Darstack 2:d168f964c331 207 }
Darstack 2:d168f964c331 208 }
Darstack 2:d168f964c331 209 /////////////////////////////////////////////////////////////////////////////////
Darstack 2:d168f964c331 210 /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°Color no found°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/
Darstack 2:d168f964c331 211 ////////////////////////////////////////////////////////////////////////////////
Darstack 3:3454cb7584e1 212
Darstack 3:3454cb7584e1 213 switch ( sel_color ) {
Darstack 3:3454cb7584e1 214
Darstack 6:35c069133c9a 215 case 0 :
Darstack 4:7a06dce3de99 216
Darstack 4:7a06dce3de99 217 //int32_t enviar5 = RESPUESTA5 ;
Darstack 4:7a06dce3de99 218 //char txt5 [6] ;
Darstack 4:7a06dce3de99 219 //printf ( txt5 , "%02X" , enviar5 );
Darstack 4:7a06dce3de99 220 printf ( " FF00 \n " );
Darstack 4:7a06dce3de99 221
Darstack 6:35c069133c9a 222 break ;
Darstack 4:7a06dce3de99 223
Darstack 6:35c069133c9a 224 case 1 :
Darstack 3:3454cb7584e1 225
Darstack 4:7a06dce3de99 226 //int32_t enviar1 = RESPUESTA1 ;
Darstack 4:7a06dce3de99 227 //char txt1 [6] ;
Darstack 4:7a06dce3de99 228 printf ( "FE01 \n" );
Darstack 4:7a06dce3de99 229 sel_color = 0;
Darstack 3:3454cb7584e1 230
Darstack 6:35c069133c9a 231 break ;
Darstack 3:3454cb7584e1 232
Darstack 6:35c069133c9a 233 case 2 :
Darstack 3:3454cb7584e1 234
Darstack 4:7a06dce3de99 235 //int32_t enviar2 = RESPUESTA2 ;
Darstack 4:7a06dce3de99 236 //char txt2 [6] ;
Darstack 4:7a06dce3de99 237 printf ( "FE02 \n" ) ;
Darstack 4:7a06dce3de99 238 sel_color = 0;
Darstack 3:3454cb7584e1 239
Darstack 6:35c069133c9a 240 break ;
Darstack 6:35c069133c9a 241
Darstack 6:35c069133c9a 242 case 3 :
Darstack 2:d168f964c331 243
Darstack 4:7a06dce3de99 244 //int32_t enviar3 = RESPUESTA3;
Darstack 4:7a06dce3de99 245 //char txt3 [6] ;
Darstack 4:7a06dce3de99 246 printf ( "FE03 \n" );
Darstack 4:7a06dce3de99 247 sel_color = 0;
Darstack 3:3454cb7584e1 248
Darstack 6:35c069133c9a 249 break ;
Darstack 3:3454cb7584e1 250
Darstack 6:35c069133c9a 251 case 4 :
Darstack 3:3454cb7584e1 252
Darstack 4:7a06dce3de99 253 //int32_t enviar4 = RESPUESTA4;
Darstack 4:7a06dce3de99 254 //char txt4 [6] ;
Darstack 4:7a06dce3de99 255 printf ( "FE04 \n" );
Darstack 4:7a06dce3de99 256 sel_color = 0;
Darstack 3:3454cb7584e1 257
Darstack 6:35c069133c9a 258 break ;
Darstack 2:d168f964c331 259 }
fabeltranm 0:fa6bdbdd4239 260
Darstack 3:3454cb7584e1 261 }
Darstack 4:7a06dce3de99 262 void funcionesrobot ( uint8_t _Parametro, uint8_t _Comando ){
Darstack 4:7a06dce3de99 263
Darstack 4:7a06dce3de99 264 /* °°Declaración de contadores°° */
Darstack 6:35c069133c9a 265 uint8_t i ;
Darstack 6:35c069133c9a 266 uint8_t j ;
Darstack 4:7a06dce3de99 267
Darstack 4:7a06dce3de99 268 switch ( _Comando ){
Darstack 4:7a06dce3de99 269 // Acciones que ejerce el robot
Darstack 4:7a06dce3de99 270
Darstack 4:7a06dce3de99 271 case 0 :
Darstack 6:35c069133c9a 272
Darstack 6:35c069133c9a 273 leer_color() ;
Darstack 6:35c069133c9a 274
Darstack 4:7a06dce3de99 275 break ;
Darstack 4:7a06dce3de99 276
Darstack 4:7a06dce3de99 277 case 1 :
Darstack 6:35c069133c9a 278
Darstack 6:35c069133c9a 279 printf ( "Frecuencia: 2000 Tiempo: %d s \n" , _Parametro ) ;
Darstack 6:35c069133c9a 280 mybuzzer.period_us ( DO ) ;
Darstack 6:35c069133c9a 281 mybuzzer.write ( 0.8 ) ;
Darstack 6:35c069133c9a 282 wait( _Parametro ) ;
Darstack 6:35c069133c9a 283 mybuzzer.write ( 0 ) ;
Darstack 6:35c069133c9a 284
Darstack 4:7a06dce3de99 285 break ;
Darstack 4:7a06dce3de99 286
Darstack 4:7a06dce3de99 287 case 2 :
Darstack 6:35c069133c9a 288
Darstack 6:35c069133c9a 289 printf ( "Frecuencia: 2500 Tiempo: %d s \n" , _Parametro ) ;
Darstack 6:35c069133c9a 290 mybuzzer.period_us ( RE ) ;
Darstack 6:35c069133c9a 291 mybuzzer.write ( 0.8 ) ;
Darstack 6:35c069133c9a 292 wait ( _Parametro ) ;
Darstack 6:35c069133c9a 293 mybuzzer.write ( 0 ) ;
Darstack 6:35c069133c9a 294
Darstack 4:7a06dce3de99 295 break ;
Darstack 4:7a06dce3de99 296
Darstack 4:7a06dce3de99 297 case 3 :
Darstack 6:35c069133c9a 298
Darstack 6:35c069133c9a 299 printf ( " Frecuencia: 3000 " ); printf ( " Tiempo: %d s \n " , _Parametro ) ;
Darstack 6:35c069133c9a 300 mybuzzer.period_us ( MI ) ;
Darstack 6:35c069133c9a 301 mybuzzer.write ( 0.8 ) ;
Darstack 6:35c069133c9a 302 wait( _Parametro ) ;
Darstack 6:35c069133c9a 303 mybuzzer.write ( 0 ) ;
Darstack 6:35c069133c9a 304
Darstack 4:7a06dce3de99 305 break ;
Darstack 4:7a06dce3de99 306
Darstack 6:35c069133c9a 307 case 4 :
Darstack 6:35c069133c9a 308
Darstack 6:35c069133c9a 309 printf ( "Frecuencia: 3500 Tiempo: %d s \n" , _Parametro ) ;
Darstack 6:35c069133c9a 310 mybuzzer.period_us ( FA );
Darstack 6:35c069133c9a 311 mybuzzer.write ( 0.8 );
Darstack 6:35c069133c9a 312 wait ( 5 );
Darstack 6:35c069133c9a 313 mybuzzer.write ( 0 );
Darstack 6:35c069133c9a 314
Darstack 4:7a06dce3de99 315 break ;
Darstack 4:7a06dce3de99 316
Darstack 4:7a06dce3de99 317 case 5 :
Darstack 4:7a06dce3de99 318
Darstack 6:35c069133c9a 319 steppeer_dir = 1 ;
Darstack 6:35c069133c9a 320 steppeer_dir2 = 0 ;
Darstack 6:35c069133c9a 321 wait_us ( 1 );
Darstack 6:35c069133c9a 322 for ( j = 1 ; j <= _Parametro ; j++){
Darstack 4:7a06dce3de99 323
Darstack 6:35c069133c9a 324 for ( i= 0 ; i <= 200 ; i++ ){
Darstack 4:7a06dce3de99 325
Darstack 4:7a06dce3de99 326 stepper_step = 1 ;
Darstack 6:35c069133c9a 327 stepper_step2 = 1;
Darstack 6:35c069133c9a 328 wait_us( VELOCITY );
Darstack 6:35c069133c9a 329 stepper_step = 0;
Darstack 6:35c069133c9a 330 stepper_step2 = 0;
Darstack 6:35c069133c9a 331 wait_us ( VELOCITY );
Darstack 4:7a06dce3de99 332
Darstack 4:7a06dce3de99 333 }
Darstack 4:7a06dce3de99 334 }
Darstack 4:7a06dce3de99 335
Darstack 6:35c069133c9a 336 break ;
Darstack 6:35c069133c9a 337
Darstack 6:35c069133c9a 338 case 6 :
Darstack 6:35c069133c9a 339
Darstack 6:35c069133c9a 340 steppeer_dir = 0;
Darstack 6:35c069133c9a 341 steppeer_dir2 = 1;
Darstack 6:35c069133c9a 342 wait_us ( 1 );
Darstack 6:35c069133c9a 343
Darstack 6:35c069133c9a 344 for ( j = 1 ; j <= _Parametro ; j++){
Darstack 6:35c069133c9a 345
Darstack 6:35c069133c9a 346 for ( i= 0 ; i <= 200 ; i++ ){
Darstack 4:7a06dce3de99 347
Darstack 6:35c069133c9a 348 stepper_step = 1 ;
Darstack 4:7a06dce3de99 349 stepper_step2 = 1 ;
Darstack 4:7a06dce3de99 350 wait_us( VELOCITY ) ;
Darstack 4:7a06dce3de99 351 stepper_step = 0 ;
Darstack 4:7a06dce3de99 352 stepper_step2 = 0 ;
Darstack 5:d165935ba818 353 wait_us ( VELOCITY ) ;
Darstack 4:7a06dce3de99 354
Darstack 4:7a06dce3de99 355 }
Darstack 6:35c069133c9a 356
Darstack 5:d165935ba818 357 }
Darstack 4:7a06dce3de99 358
Darstack 6:35c069133c9a 359 break ;
Darstack 6:35c069133c9a 360
Darstack 6:35c069133c9a 361 case 7 :
Darstack 6:35c069133c9a 362
Darstack 6:35c069133c9a 363 steppeer_dir = 0;
Darstack 6:35c069133c9a 364 wait_us ( 1 );
Darstack 6:35c069133c9a 365
Darstack 6:35c069133c9a 366 for ( j = 1 ; j <= _Parametro ; j++){
Darstack 6:35c069133c9a 367
Darstack 6:35c069133c9a 368 for ( i= 0 ; i <= 50 ; i++ ){
Darstack 6:35c069133c9a 369
Darstack 6:35c069133c9a 370 stepper_step = 1 ;
Darstack 6:35c069133c9a 371 //stepper_step2 = 1 ;
Darstack 6:35c069133c9a 372 wait_us(VELOCITY) ;
Darstack 6:35c069133c9a 373 stepper_step = 0 ;
Darstack 6:35c069133c9a 374 //stepper_step2 = 0 ;
Darstack 6:35c069133c9a 375 wait_us(VELOCITY) ;
Darstack 6:35c069133c9a 376
Darstack 6:35c069133c9a 377 }
Darstack 6:35c069133c9a 378 }
Darstack 6:35c069133c9a 379
Darstack 6:35c069133c9a 380 break ;
Darstack 6:35c069133c9a 381
Darstack 6:35c069133c9a 382 case 8 :
Darstack 6:35c069133c9a 383
Darstack 6:35c069133c9a 384 steppeer_dir2 = 1;
Darstack 6:35c069133c9a 385 wait_us ( 1 );
Darstack 6:35c069133c9a 386
Darstack 6:35c069133c9a 387 for ( j = 1 ; j <= _Parametro ; j++){
Darstack 6:35c069133c9a 388
Darstack 6:35c069133c9a 389 for ( i= 0 ; i <= 50 ; i++ ){
Darstack 6:35c069133c9a 390
Darstack 6:35c069133c9a 391 //stepper_step = 1 ;
Darstack 6:35c069133c9a 392 stepper_step2 = 1 ;
Darstack 6:35c069133c9a 393 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 394 //stepper_step = 0 ;
Darstack 6:35c069133c9a 395 stepper_step2 = 0 ;
Darstack 6:35c069133c9a 396 wait_us ( VELOCITY ) ;
Darstack 6:35c069133c9a 397
Darstack 6:35c069133c9a 398 }
Darstack 6:35c069133c9a 399
Darstack 6:35c069133c9a 400 }
Darstack 6:35c069133c9a 401
Darstack 6:35c069133c9a 402 break ;
Darstack 6:35c069133c9a 403
Darstack 6:35c069133c9a 404 case 9 :
Darstack 6:35c069133c9a 405
Darstack 6:35c069133c9a 406 switch ( _Parametro ){
Darstack 6:35c069133c9a 407
Darstack 6:35c069133c9a 408 case 1:
Darstack 6:35c069133c9a 409
Darstack 6:35c069133c9a 410 VELOCITY = 400 ;
Darstack 6:35c069133c9a 411
Darstack 6:35c069133c9a 412 break ;
Darstack 6:35c069133c9a 413
Darstack 6:35c069133c9a 414 case 2:
Darstack 6:35c069133c9a 415
Darstack 6:35c069133c9a 416 VELOCITY = 2500 ;
Darstack 6:35c069133c9a 417
Darstack 6:35c069133c9a 418 break ;
Darstack 6:35c069133c9a 419
Darstack 6:35c069133c9a 420 case 3:
Darstack 6:35c069133c9a 421
Darstack 6:35c069133c9a 422 VELOCITY = 5000 ;
Darstack 6:35c069133c9a 423
Darstack 6:35c069133c9a 424 break ;
Darstack 6:35c069133c9a 425
Darstack 6:35c069133c9a 426 }
Darstack 6:35c069133c9a 427
Darstack 6:35c069133c9a 428 break ;
Darstack 6:35c069133c9a 429
Darstack 6:35c069133c9a 430 case 10:
Darstack 6:35c069133c9a 431
Darstack 6:35c069133c9a 432 int8_t Exit = 0 ;
Darstack 6:35c069133c9a 433 if ( _Parametro == 1){
Darstack 6:35c069133c9a 434
Darstack 6:35c069133c9a 435 while ( !Exit ){
Darstack 6:35c069133c9a 436
Darstack 6:35c069133c9a 437 In [ 0 ] = analog_value0.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Darstack 6:35c069133c9a 438 //printf(" X = %.04f \n", In[0]);
Darstack 6:35c069133c9a 439 if ( In [ 0 ] > 0.5 ){
Darstack 6:35c069133c9a 440
Darstack 6:35c069133c9a 441 steppeer_dir = 1;
Darstack 6:35c069133c9a 442 wait_us( 1 );
Darstack 6:35c069133c9a 443
Darstack 6:35c069133c9a 444 for ( i= 0 ; i <= 50 ; i++ ){
Darstack 6:35c069133c9a 445
Darstack 6:35c069133c9a 446 stepper_step = 1 ;
Darstack 6:35c069133c9a 447 //stepper_step2 = 1 ;
Darstack 6:35c069133c9a 448 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 449 stepper_step = 0 ;
Darstack 6:35c069133c9a 450 //stepper_step2 = 0 ;
Darstack 6:35c069133c9a 451 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 452 }
Darstack 6:35c069133c9a 453 }
Darstack 6:35c069133c9a 454
Darstack 6:35c069133c9a 455 if ( In[ 0 ] < 0.5 ){
Darstack 6:35c069133c9a 456
Darstack 6:35c069133c9a 457 steppeer_dir2 = 1;
Darstack 6:35c069133c9a 458 wait_us( 1 );
Darstack 6:35c069133c9a 459
Darstack 6:35c069133c9a 460 for ( i= 0 ; i <= 50 ; i++ ){
Darstack 6:35c069133c9a 461
Darstack 6:35c069133c9a 462 //stepper_step = 1 ;
Darstack 6:35c069133c9a 463 stepper_step2 = 1 ;
Darstack 6:35c069133c9a 464 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 465 //stepper_step = 0 ;
Darstack 6:35c069133c9a 466 stepper_step2 = 0 ;
Darstack 6:35c069133c9a 467 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 468
Darstack 6:35c069133c9a 469 }
Darstack 6:35c069133c9a 470 }
Darstack 6:35c069133c9a 471
Darstack 6:35c069133c9a 472 In [ 1 ] = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Darstack 6:35c069133c9a 473 //printf(" X = %.04f \n", In[0]);
Darstack 6:35c069133c9a 474
Darstack 7:5f332f47b38f 475 if (In [ 1 ] > 0.6 ){
Darstack 6:35c069133c9a 476
Darstack 6:35c069133c9a 477 steppeer_dir = 1 ;
Darstack 6:35c069133c9a 478 steppeer_dir2 = 0 ;
Darstack 6:35c069133c9a 479 wait_us( 1 );
Darstack 6:35c069133c9a 480
Darstack 6:35c069133c9a 481 for ( i= 0 ; i <= 50 ; i++ ){
Darstack 6:35c069133c9a 482
Darstack 6:35c069133c9a 483 stepper_step = 1 ;
Darstack 6:35c069133c9a 484 stepper_step2 = 1 ;
Darstack 6:35c069133c9a 485 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 486 stepper_step = 0 ;
Darstack 6:35c069133c9a 487 stepper_step2 = 0 ;
Darstack 6:35c069133c9a 488 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 489
Darstack 6:35c069133c9a 490 }
Darstack 6:35c069133c9a 491 }
Darstack 6:35c069133c9a 492
Darstack 7:5f332f47b38f 493 if ( In [ 1 ] < 0.6 ){
Darstack 6:35c069133c9a 494
Darstack 6:35c069133c9a 495 steppeer_dir = 0 ;
Darstack 6:35c069133c9a 496 steppeer_dir2 = 1 ;
Darstack 6:35c069133c9a 497 wait_us( 1 );
Darstack 6:35c069133c9a 498
Darstack 6:35c069133c9a 499 for ( i= 0 ; i <= 50 ; i++ ){
Darstack 6:35c069133c9a 500
Darstack 6:35c069133c9a 501 stepper_step = 1 ;
Darstack 6:35c069133c9a 502 stepper_step2 = 1 ;
Darstack 6:35c069133c9a 503 wait_us( VELOCITY ) ;
Darstack 6:35c069133c9a 504 stepper_step = 0 ;
Darstack 6:35c069133c9a 505 stepper_step2 = 0 ;
Darstack 6:35c069133c9a 506 wait_us ( VELOCITY ) ;
Darstack 6:35c069133c9a 507
Darstack 6:35c069133c9a 508 }
Darstack 6:35c069133c9a 509
Darstack 6:35c069133c9a 510 }
Darstack 6:35c069133c9a 511
Darstack 7:5f332f47b38f 512 wait_us ( 1 );
Darstack 6:35c069133c9a 513 if ( command.readable () == 1 ){
Darstack 6:35c069133c9a 514
Darstack 6:35c069133c9a 515 leer_datos();
Darstack 6:35c069133c9a 516
Darstack 6:35c069133c9a 517 if ( Lectura [ 1 ] == 02 ){
Darstack 6:35c069133c9a 518
Darstack 6:35c069133c9a 519 Exit = 1 ;
Darstack 6:35c069133c9a 520 }
Darstack 6:35c069133c9a 521 }
Darstack 6:35c069133c9a 522
Darstack 5:d165935ba818 523 }
Darstack 6:35c069133c9a 524 }
Darstack 6:35c069133c9a 525
Darstack 6:35c069133c9a 526 break ;
Darstack 6:35c069133c9a 527
Darstack 4:7a06dce3de99 528 }
Darstack 5:d165935ba818 529 }
Darstack 5:d165935ba818 530