Diff: setup.cpp

Revision:

23:2126e38bb48c

Parent:

22:d5431fff164b

Child:

24:a1d16835201c

--- a/setup.cpp	Mon Jul 02 04:35:28 2012 +0000
+++ b/setup.cpp	Fri Jul 06 17:34:16 2012 +0000
@@ -18,6 +18,7 @@
 extern Serial       pc;
 extern Serial       RS_232;
 extern I2C          encoder;
+extern Timer        crono;
 extern DigitalIn    isPC;
 extern DigitalOut   pin_son;   // SON
 extern DigitalOut   pin_dir;   // SIGN+
@@ -30,16 +31,55 @@
 int fq_posicion = 10000;  // Variable global donde se almacenara 
                          // la velocidad de posicionamiento en Hz
 
+float t_alto;          // para el posicionamiento del motor
+
 const int addres = 0x36;  // Direccion del enconder en modo esclavo
 
 int read_encoder()
 {
-    int count;
+    int  count = 0;
+    int  temp = 0;
+    
+    char w_1 [2];
+    char w_2 [2];
+    char w_3 [2];
+    char w_4 [2];
+    
+    char r_1 [2];
+    char r_2 [2];
+    char r_3 [2];
+    char r_4 [2];
+    
+    w_1[0] = 0x10;
+    w_2[0] = 0x11;
+    w_3[0] = 0x12;
+    w_4[0] = 0x13;
+    
+    //pc.printf("\nEnviando dato\n");
     
-    char buffer[4];
-    encoder.read( addres, buffer, 1 ); // 8bit por ahora
-    count = (int) buffer[0];
+    encoder.write( addres, w_1,1 );
+    encoder.read( addres, r_1, 1 ); 
+    
+    encoder.write( addres, w_2,1 );
+    encoder.read ( addres, r_2, 1 ); 
+    
+    encoder.write( addres, w_3,1 );
+    encoder.read ( addres, r_3, 1 ); 
     
+    encoder.write( addres, w_4,1 );
+    encoder.read( addres, r_4, 1 ); 
+        
+    count  = r_1[0];
+    
+    temp = r_2[0];
+    count |= temp << 8;
+    
+    temp = r_3[0];
+    count |= temp << 16;
+    
+    temp = r_4[0];
+    count |= temp << 24;
+  
     return count;
 }
 
@@ -51,6 +91,7 @@
     buffer[1] = 0x02; // Comando para cambiar estado de led
     
     encoder.write( addres, buffer, 1 );
+    encoder.stop();
 }
 
 void setTimer2()
@@ -88,69 +129,90 @@
 {
     int value;        // Nuevo Valor
     char command;     // Comando al que aplicar el nuevo valor
+    
 
     if( isPC )
-        pc.scanf( "%d-%c", &value, &command ) ;
+        pc.scanf( "%d-%c", &value, &command );
     else
-        RS_232.scanf( "%d-%c", &value, &command ) ;
+        RS_232.scanf( "%d-%c", &value, &command );
+
+    switch( command )
+    {
     
-    // Establecer nueva frecuencia
-    if( command == 'H')
-        setPTO( value );
-    
-    else if( command == 'K' )
-        setPTO( value * 1000 );
+        case 'R':
+            int ang = 0;
+            float rpm;
+            
+            clear_encoder();                  // Encerar encoder
+            crono.reset();                   // Encerar el cronometro;
+            
+            while( ang == 0 )              // Esperar hasta q exista lectura del encoder
+                ang = read_encoder();     // Leer nueva posicion
+                
+            crono.stop();               // detener cronometro
+            rpm = ( ang * 500 ) / (crono.read_ms() * 3 );
+            
+            //  grados      1000 ms    60 s     1 Rev       500  Rev
+            //  ------   = -------- * ------ * ---------- = -------- 
+            //    ms          1s       1min     360 grad     3   Min
+            
+            if( isPC )
+                pc.printf("%f", rpm);
+            else
+                RS_232.printf("%f", rpm);
+            break;
         
-    // Nuevo voltaje de salida
-    // Alguna formula para calcular el Vout necesario
-    // -100% a +100%
-    else if( command == 'A')
-        aout = (float)( value + 10000.0 ) / 20000.0;
-        
-        
-    // Cambiar la direccion
-    else if( command == 'D')
-    {
-        stopTimer2();
-        pin_dir = value;
-        wait_us( 2 );
-        startTimer2();
-    }
-       
+        case 'H':                        // Establecer nueva frecuencia
+            setPTO( value );
+            break;
+    
+        case 'K':
+            setPTO( value * 1000 );
+            break;
         
-    //Encender el Servo
-    else if( command == 'S')
-        pin_son = value;
-    
-    //Setear la velocidad de Posicionamiento 
-    else if( command == 'V')
-      fq_posicion = value;
-    
-    // Generar un numero definido de pulsos a la velocidad de posicionamiento  
-    else if( command == 'G' )  
-    {
-        int pulsos = value;       //Numero de pulsos a generar
-        float t_alto = pulsos / fq_posicion;  //Tiempo que debe ser generado el tren de pulsos.
+        case 'A':                       // Cambiar voltaje de salida
+            aout = (float)( value + 10000.0 ) / 20000.0;
+            break;
+        
+        case 'D':                       // Cambiar la direccion
+            stopTimer2();
+            pin_dir = value;
+            wait_us( 2 );
+            startTimer2();
+            break;
+       
+        case 'V':                   //Setear la velocidad de Posicionamiento 
+            fq_posicion = value;
+            break;
+
+            // Generar un numero definido de pulsos a la velocidad de posicionamiento  
+        case 'G':
+            int pulsos = value;       //Numero de pulsos a generar
+            t_alto = pulsos / fq_posicion;  //Tiempo que debe ser generado el tren de pulsos.
+        
+            stopTimer2();               //Deten el tren de pulsos
+            setPTO( fq_posicion );      //Nueva frecuencia de salida
+            startTimer2();              //Inicia el tren de pulsos
+            wait( t_alto );             //Espera hasta llegar a la posicion
+            stopTimer2();               //Posicion alcanzada ALTO. 
+            break;
+        
+        case 'E':                       //Leer encoder
+            if( isPC )
+                pc.printf( "%d",read_encoder() );
+            else
+                RS_232.printf( "%d",read_encoder() );
+            break;
     
-        stopTimer2();               //Deten el tren de pulsos
-        setPTO( fq_posicion );      //Nueva frecuencia de salida
-        startTimer2();              //Inicia el tren de pulsos
-        wait( t_alto );             //Espera hasta llegar a la posicion
-        stopTimer2();               //Posicion alcanzada ALTO. 
+        case 'Z':           //Limpiar contador encoder
+            clear_encoder();
+            break;
+
+        case 'S':               //Encender el Servo
+            pin_son = value;
+            break;
     }
-    
-    //Leer encoder
-    else if( command == 'E' )
-    {
-        if( isPC )
-            pc.printf( "%d",read_encoder() );
-        else
-            RS_232.printf( "%d",read_encoder() );
-    }
-    
-    //Limpiar contador encoder
-    else if( command == 'Z' )
-        clear_encoder();
+         
 }
 
 
@@ -249,13 +311,9 @@
             fgets( notstr, 4, fp );
             fgets( notstr, 4, fp );
             fgets( notstr, 4, fp );
-            
             fgets( baud,  40, fp );
-            
-            printf("Cerrando archivo...\n");
             fclose(fp);
-         
-            
+                     
             // Extraer los valores numericos
             sscanf( baud,"%*s %d",&baudios );
             
@@ -291,7 +349,7 @@
 void setANG_eth( char * input, char * output )
 {
     long int pulsos = atol( input );       //Numero de pulsos a generar
-    float t_alto = pulsos / fq_posicion;  //Tiempo que debe ser generado el tren de pulsos.
+    t_alto = pulsos / fq_posicion;  //Tiempo que debe ser generado el tren de pulsos.
     
     stopTimer2();               //Deten el tren de pulsos
     setPTO( fq_posicion );      //Nueva frecuencia de salida
@@ -365,7 +423,6 @@
         sprintf( output,"%d", read_encoder() );
     else
         sprintf( output,"AL" );
-    
 }
 
 
@@ -380,6 +437,32 @@
     
 }
 
+void getRPM_eth( char * input, char * output )
+{
+    int ang = 0;
+    float rpm;
+    
+    clear_encoder();                  // Encerar encoder
+    crono.reset();                   // Encerar el cronometro;
+    
+    while( ang == 0 )              // Esperar hasta q exista lectura del encoder
+        ang = read_encoder();     // Leer nueva posicion
+        
+    crono.stop();               // detener cronometro
+    rpm = ( ang * 500 ) / (crono.read_ms() * 3 );
+    
+    //  grados      1000 ms    60 s     1 Rev       500  Rev
+    //  ------   = -------- * ------ * ---------- = -------- 
+    //    ms          1s       1min     360 grad     3   Min
+    
+    if( pin_alm == 0 )
+        sprintf( output,"Ok" );
+    else
+        sprintf( output,"%f", rpm );
+    
+    
+}
+