Einer Fabian Aponte Cubides
sensor color con joystick y motores paso a paso mas pantalla LCD
Diff: main.cpp
- Revision:
- 4:7a06dce3de99
- Parent:
- 3:3454cb7584e1
- Child:
- 5:d165935ba818
--- a/main.cpp Sat Mar 09 17:33:26 2019 +0000
+++ b/main.cpp Fri Apr 19 18:09:38 2019 +0000
@@ -47,51 +47,62 @@
******************************************************************************/
-#define INITELE 0xFE
-#define RESPUESTA1 0xFF01
-#define RESPUESTA2 0xFF02
-#define RESPUESTA3 0xFF03
-#define RESPUESTA4 0xFF04
-#define RESPUESTA5 0xFF05
-#define CMD 0x01
-#define DO 1300
-#define RE 1500
-#define MI 1600
-#define FA 1800
-#define SO 2000
+#define INITCMD 0xFF
+#define DO 2000
+#define RE 2500
+#define MI 3000
+#define FA 3500
-Serial command(USBTX, USBRX);
-// S0, S1, S2, S3, OUT
-scolor_TCS3200 scolor(PA_8, PB_10, PB_4, PB_5, PB_3);
+/* ||Definicion de Puertos || */
+ Serial command(USBTX, USBRX);
+ // S0, S1, S2, S3, OUT
+ scolor_TCS3200 scolor( PA_9, PC_7, PB_6, PA_7, PA_6 ) ;
+ DigitalOut stepper_step ( PB_4 ) ;
+ DigitalOut steppeer_dir ( PB_5 ) ;
+ DigitalOut stepper_step2 ( PB_10 ) ;
+ DigitalOut steppeer_dir2 ( PA_8 ) ;
+ AnalogIn analog_value0 ( A0 ) ;
+ AnalogIn analog_value1 ( A1 ) ;
+ PwmOut mybuzzer( PB_9 ) ;
+/* ||Definicion de Puertos || */
+
+ uint32_t VELOCITY = 400 ; // Tiempo en micro segundos
+ int16_t Lectura [ 2 ] = {} ;
+ double In[ 1 ] = { } ;
+
// definición de las funciones
-void setup_uart();
-void leer_datos();
-void leer_color();
-uint8_t cmd;
-PwmOut mybuzzer(PA_5);
+ void setup_uart();
+ void leer_datos();
+ void leer_color();
+ void funcionesrobot ( uint8_t _Parametro, uint8_t _Comando );
int main() {
setup_uart();
while(1){
leer_datos();
- if (cmd==CMD){}
- leer_color();
- }
+ funcionesrobot ( Lectura [ 1 ], Lectura [ 0 ] );
+
+ }
}
-
void setup_uart(){
command.baud(115200);
}
void leer_datos(){
- while(command.getc()!= INITELE);
- cmd=command.getc();
+
+ while(command.getc()!= INITCMD) ;
+ uint8_t i ;
+ for ( i = 0 ; i < 2 ; i++){
+
+ Lectura [ i ] = command.getc();
+ printf ( " %4d ", Lectura [ i ]);
+ }
}
@@ -107,7 +118,7 @@
long frqgreen;
long frqblue;
long frqclear;
- int8_t sel_color;
+ int8_t sel_color = 0;
printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", red, green, blue, clear);
frqred = ( ( 1.0/red )* 1000.0 );
@@ -125,12 +136,12 @@
if ( frqblue >= 5.0 and frqblue <= 29.0 ) {
- //printf ( "tiende a rojo \n" );
+ printf ( "tiende a rojo \n" );
mybuzzer.period_us(DO);
- mybuzzer.write(0.5);
+ mybuzzer.write(0.8);
wait(5);
mybuzzer.write(0);
- sel_color=1;
+ sel_color = 1;
}
}
@@ -142,14 +153,14 @@
if( frqred >= 8.0 and frqred <= 20.0 ) {
- if ( frqblue >= 10.0 and frqblue <= 22.0 ) {
+ if ( frqblue >= 10.0 and frqblue <= 26.0 ) {
printf ( "tiende a verde \n" );
mybuzzer.period_us(RE);
mybuzzer.write(0.5);
wait(5);
mybuzzer.write(0);
- sel_color=2;
+ sel_color = 2;
}
}
@@ -161,14 +172,14 @@
if( frqgreen >= 5.0 and frqgreen <= 26.0 ) {
- if ( frqred >= 5.0 and frqred <= 25.0 ) {
+ if ( frqred >= 5.0 and frqred <= 20.0 ) {
- //printf ( "tiende a azul \n" );
+ printf ( "tiende a azul \n" );
mybuzzer.period_us(MI);
- mybuzzer.write(0.5);
+ mybuzzer.write(1);
wait(5);
mybuzzer.write(0);
- sel_color=3;
+ sel_color = 3;
}
}
@@ -176,18 +187,18 @@
/////////////////////////////////////////////////////////////////////////////////
/*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°Color amarillo°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/
////////////////////////////////////////////////////////////////////////////////
- if ( frqblue >= 0.0 and frqblue <= 25.0 ) {
+ if ( frqblue >= 0.0 and frqblue <= 30.0 ) {
if( frqgreen >= 20.0 and frqgreen <= 40.0) {
- if ( 20.0 >= frqred <= 46.0 ) {
+ if ( frqred >= 20.0 and frqred <= 50.0 ) {
printf ( "tiende a amarillo \n" );
mybuzzer.period_us(FA);
- mybuzzer.write(0.5);
+ mybuzzer.write(0.6);
wait(5);
mybuzzer.write(0);
- sel_color=4;
+ sel_color = 4;
}
}
@@ -198,47 +209,320 @@
switch ( sel_color ) {
+ case 0:
+
+ //int32_t enviar5 = RESPUESTA5 ;
+ //char txt5 [6] ;
+ //printf ( txt5 , "%02X" , enviar5 );
+ printf ( " FF00 \n " );
+
+ break;
+
case 1:
- int32_t enviar = RESPUESTA1 ;
- char txt [6] ;
- printf ( txt, "%04X" , RESPUESTA1 );
+ //int32_t enviar1 = RESPUESTA1 ;
+ //char txt1 [6] ;
+ printf ( "FE01 \n" );
+ sel_color = 0;
break;
case 2:
- int32_t enviar = RESPUESTA2 ;
- char txt [6] ;
- printf ( txt, "%04X" , RESPUESTA2 ) ;
-
+ //int32_t enviar2 = RESPUESTA2 ;
+ //char txt2 [6] ;
+ printf ( "FE02 \n" ) ;
+ sel_color = 0;
break;
case 3:
- int32_t enviar = RESPUESTA3;
- char txt [6] ;
- printf ( txt, "%04X" , RESPUESTA3 );
+ //int32_t enviar3 = RESPUESTA3;
+ //char txt3 [6] ;
+ printf ( "FE03 \n" );
+ sel_color = 0;
break;
case 4:
- int32_t enviar = RESPUESTA4;
- char txt [6] ;
- printf ( txt, "%04X" , RESPUESTA4 );
- break;
+ //int32_t enviar4 = RESPUESTA4;
+ //char txt4 [6] ;
+ printf ( "FE04 \n" );
+ sel_color = 0;
- default:{
-
- int32_t enviar = RESPUESTA4;
- char txt [6] ;
- printf ( txt, "%04X" , RESPUESTA4 );
-
- }
-
-
+ break;
}
}
+ void funcionesrobot ( uint8_t _Parametro, uint8_t _Comando ){
+
+ /* °°Declaración de contadores°° */
+ uint8_t i ;
+ uint8_t j ;
+
+ switch ( _Comando ){
+ // Acciones que ejerce el robot
+
+ case 0 :
+ leer_color() ;
+ break ;
+
+ case 1 :
+ printf ( "Frecuencia: 2000 Tiempo: %d s \n" , _Parametro ) ;
+ mybuzzer.period_us( DO ) ;
+ mybuzzer.write( 0.8 ) ;
+ wait( _Parametro ) ;
+ mybuzzer.write( 0 ) ;
+ break ;
+
+ case 2 :
+ printf ( "Frecuencia: 2500 Tiempo: %d s \n" , _Parametro ) ;
+ mybuzzer.period_us( RE ) ;
+ mybuzzer.write( 0.8 ) ;
+ wait( _Parametro ) ;
+ mybuzzer.write( 0 ) ;
+ break ;
+
+ case 3 :
+ printf ( " Frecuencia: 3000 " ); printf ( " Tiempo: %d s \n " , _Parametro ) ;
+ mybuzzer.period_us( MI ) ;
+ mybuzzer.write( 0.8 ) ;
+ wait( _Parametro ) ;
+ mybuzzer.write( 0 ) ;
+ break ;
+
+ case 4:
+ printf ( "Frecuencia: 3500 Tiempo: %d s \n" , _Parametro ) ;
+ mybuzzer.period_us( FA );
+ mybuzzer.write( 0.8 );
+ wait( 5 );
+ mybuzzer.write(0);
+ break ;
+
+ case 5 :
+
+ steppeer_dir = 1 ;
+ steppeer_dir2 = 0 ;
+ wait_us( 1 );
+ for ( j = 1 ; j <= _Parametro ; j++){
+
+ for ( i= 0 ; i <= 200 ; i++ ){
+
+ stepper_step = 1 ;
+ stepper_step2 = 1;
+ wait_us(VELOCITY);
+ stepper_step = 0;
+ stepper_step2 = 0;
+ wait_us(VELOCITY);
+
+ }
+
+ }
+
+ break;
+
+ case 6 :
+ steppeer_dir = 0;
+ steppeer_dir2 = 1;
+ wait_us( 1 );
+
+ for ( j = 1 ; j <= _Parametro ; j++){
+
+ for ( i= 0 ; i <= 200 ; i++ ){
+
+ stepper_step = 1 ;
+ stepper_step2 = 1 ;
+ wait_us(VELOCITY) ;
+ stepper_step = 0 ;
+ stepper_step2 = 0 ;
+ wait_us(VELOCITY) ;
+
+ }
+
+ }
+
+ break;
+ case 7:
+
+ steppeer_dir = 1;
+ wait_us( 1 );
+
+ for ( j = 1 ; j <= _Parametro ; j++){
+
+ for ( i= 0 ; i <= 50 ; i++ ){
+
+ stepper_step = 1 ;
+ //stepper_step2 = 1 ;
+ wait_us(VELOCITY) ;
+ stepper_step = 0 ;
+ //stepper_step2 = 0 ;
+ wait_us(VELOCITY) ;
+
+ }
+
+ }
+
+ break;
+
+ case 8 :
+
+ steppeer_dir = 1;
+ wait_us( 1 );
+
+ for ( j = 1 ; j <= _Parametro ; j++){
+
+ for ( i= 0 ; i <= 50 ; i++ ){
+
+ //stepper_step = 1 ;
+ stepper_step2 = 1 ;
+ wait_us(VELOCITY) ;
+ //stepper_step = 0 ;
+ stepper_step2 = 0 ;
+ wait_us(VELOCITY) ;
+
+ }
+
+ }
+
+ break;
+
+ case 9 :
+
+ switch ( _Parametro ){
+
+ case 1:
+
+ VELOCITY = 400 ;
+
+ break;
+
+ case 2:
+
+ VELOCITY = 2500 ;
+
+ break;
+
+ case 3:
+
+ VELOCITY = 5000 ;
+
+ break;
+
+ }
+
+ break;
+
+ case 10:
+
+ switch ( _Parametro ){
+
+ case 1:
+ In[0] = analog_value0.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+ printf(" X = %.04f \n", In[0]);
+
+ while ( In[0] >= 0.9 ){
+
+ steppeer_dir = 1;
+ wait_us( 1 );
+
+ for ( i= 0 ; i <= 50 ; i++ ){
+
+ stepper_step = 1 ;
+ //stepper_step2 = 1 ;
+ wait_us( VELOCITY ) ;
+ stepper_step = 0 ;
+ //stepper_step2 = 0 ;
+ wait_us( VELOCITY ) ;
+ In[0] = analog_value0.read();
+ }
+ }
+
+ while ( In[0] <= 0.4 ){
+
+ steppeer_dir2 = 1;
+ wait_us( 1 );
+
+ for ( i= 0 ; i <= 50 ; i++ ){
+
+ //stepper_step = 1 ;
+ stepper_step2 = 1 ;
+ wait_us( VELOCITY ) ;
+ //stepper_step = 0 ;
+ stepper_step2 = 0 ;
+ wait_us( VELOCITY ) ;
+ In[0] = analog_value0.read();
+ }
+ }
+ In[1] = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+ printf(" Y = %.04f \n", In[1]);
+ while (In[1] >= 0.9){
+
+ steppeer_dir = 1 ;
+ steppeer_dir2 = 0 ;
+ wait_us( 1 );
+ for ( i= 0 ; i <= 50 ; i++ ){
+
+ stepper_step = 1 ;
+ stepper_step2 = 1 ;
+ wait_us( VELOCITY ) ;
+ stepper_step = 0 ;
+ stepper_step2 = 0 ;
+ wait_us( VELOCITY ) ;
+
+ }
+ In[1] = analog_value1.read();
+
+ }
+
+ while (In[1] <= 0.4){
+
+ steppeer_dir = 0 ;
+ steppeer_dir2 = 1 ;
+ wait_us( 1 );
+ for ( i= 0 ; i <= 50 ; i++ ){
+
+ stepper_step = 1 ;
+ stepper_step2 = 1 ;
+ wait_us( VELOCITY ) ;
+ stepper_step = 0 ;
+ stepper_step2 = 0 ;
+ wait_us( VELOCITY ) ;
+
+ }
+ In[1] = analog_value1.read();
+
+ }
+ wait ( 1 );
+ break;
+
+ case 2:
+
+ In[0] = 0 ;
+ In[1] = 0 ;
+ printf(" X = %.04f, Y = %.04f \n", In[0], In[1] );
+
+ break;
+
+ }
+ break;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ }