Diff: setup.cpp

Revision:

32:4483d6c225e5

Parent:

31:7e2cdd547cb2

Child:

33:e6ff02c3e0f5

--- a/setup.cpp	Sat May 03 00:14:17 2014 +0000
+++ b/setup.cpp	Wed May 07 01:05:05 2014 +0000
@@ -37,9 +37,11 @@
 
 int fq_actual = 0;    // Ultimo valor seteado para el tren de pulsos
 
+int bandera_inicio = 0;
+
 int velocidad_rpm()
 {
-    return encoder.GetPosition();
+    return encoder.CalculateRPM( encoder.GetVelocityCap(), 360 );
 }
 
 void clear_encoder()
@@ -108,6 +110,7 @@
         {
             setPTO( value );
             fq_actual = value;
+            bandera_inicio = 0;  //En caso que haya estado yendo a HOME, cancelar
                 
             // Envia un OK de comando recibido
                 if( isPC )
@@ -121,6 +124,8 @@
         {
             setPTO( value * 1000 );
             fq_actual = value;
+            bandera_inicio = 0;  //En caso que haya estado yendo a HOME, cancelar
+
             // Envia un OK de comando recibido
                 if( isPC )
                     pc.printf("OK\r\n");
@@ -147,6 +152,7 @@
             stopTimer2();
             pin_dir = value;
             wait_us( 2 );
+            bandera_inicio = 0;  //En caso que haya estado yendo a HOME, cancelar
 
             if ( fq_actual != 0 )
             {
@@ -180,8 +186,10 @@
             float pulsos = value;       //Numero de pulsos a generar
             t_alto = (float)(pulsos / fq_posicion);  //Tiempo que debe ser generado el tren de pulsos.
             
+            bandera_inicio = 0;  //En caso que haya estado yendo a HOME, cancelar
+
             //DEBUG
-            pc.printf("Tiempo en timer en seg = %f", t_alto);
+            //pc.printf("Tiempo en timer en seg = %f", t_alto);
             
             stopTimer2();               //Deten el tren de pulsos
             setPTO( fq_posicion );      //Nueva frecuencia de salida
@@ -205,9 +213,10 @@
         {
             float pulsos = value * 1000;       //Numero de pulsos a generar
             t_alto = (float)(pulsos / fq_posicion);  //Tiempo que debe ser generado el tren de pulsos.
-            
+            bandera_inicio = 0;  //En caso que haya estado yendo a HOME, cancelar
+
             //DEBUG
-            pc.printf("Tiempo en timer en seg = %f", t_alto);
+            //pc.printf("Tiempo en timer en seg = %f", t_alto);
             
             stopTimer2();               //Deten el tren de pulsos
             setPTO( fq_posicion );      //Nueva frecuencia de salida
@@ -231,9 +240,10 @@
         {
             float pulsos = value * 1000000;       //Numero de pulsos a generar
             t_alto = (float)(pulsos / fq_posicion);  //Tiempo que debe ser generado el tren de pulsos.
-            
+            bandera_inicio = 0;  //En caso que haya estado yendo a HOME, cancelar
+
             //DEBUG
-            pc.printf("Tiempo en timer en seg = %f", t_alto);
+            //pc.printf("Tiempo en timer en seg = %f", t_alto);
             
             stopTimer2();               //Deten el tren de pulsos
             setPTO( fq_posicion );      //Nueva frecuencia de salida
@@ -277,6 +287,21 @@
         case 'S':               //Encender el Servo
 
             pin_son = value;
+            bandera_inicio = 0;  //En caso que haya estado yendo a HOME, cancelar
+
+            // Envia un OK de comando recibido
+                if( isPC )
+                    pc.printf("OK\r\n");
+                else
+                    RS_232.printf("OK\r\n");
+
+            break;
+            
+            case 'I':           //Ir al inicio del recorrido
+
+            pin_dir = 1;              //Mover hacia el motor
+            bandera_inicio = 1;      // Informar a ISR_Adv_motor() acerca del Homing
+            setPTO( value ); // a la velocidad seteada 
 
             // Envia un OK de comando recibido
                 if( isPC )
@@ -295,6 +320,7 @@
                     RS_232.printf("NA\r\n");
 
             break;
+
     }
          
 }
@@ -340,9 +366,9 @@
 void ISR_Alm_encoder()
 {
     if( isPC )
-        pc.printf("A0\r\n");
+        pc.printf("A3\r\n");
     else
-        RS_232.printf("A0\r\n");
+        RS_232.printf("A3\r\n");
 }
 
 
@@ -351,10 +377,14 @@
  */
 void ISR_Alm_motor()
 {
-    if( isPC )
-        pc.printf("A0\r\n");
-    else
-        RS_232.printf("A0\r\n");
+    wait_us( 50 );
+    if ( limite_4 == 1)
+    {
+        if( isPC )
+            pc.printf("A0\r\n");
+        else
+            RS_232.printf("A0\r\n");
+    }
 }
 
 
@@ -363,11 +393,15 @@
  */
 void ISR_Adv_encoder()
 {
+    wait_ms( 50 );
+    if ( limite_2 == 1)
+    {
     
-    if( isPC )
-        pc.printf("A2\r\n");
-    else
-        RS_232.printf("A2\r\n");
+        if( isPC )
+            pc.printf("A2\r\n");
+        else
+            RS_232.printf("A2\r\n");
+    }
 
 }
 
@@ -377,10 +411,41 @@
  */
 void ISR_Adv_motor()
 {
-    if( isPC )
-        pc.printf("A1\r\n");
-    else
-        RS_232.printf("A1\r\n");
+    if ( bandera_inicio == 1 )
+    {
+        setPTO( 0 );   //detener el carro
+        pin_dir = 0;   //hacer que se aleje del motor
+
+
+            stopTimer2();               //Deten el tren de pulsos
+            setPTO( 20000 );            //Nueva frecuencia de salida
+            startTimer2();              //Inicia el tren de pulsos
+            wait_ms( 100 );             //Espera hasta llegar a la posicion
+            stopTimer2();               //Posicion alcanzada ALTO. 
+                
+        setPTO (0);  // Detener el carro cuando este en posición valida
+        wait_ms(100);
+        //Encerar el contador de encoder y de velocidad
+        encoder.Reset(QEI_RESET_POS);
+        encoder.Reset(QEI_RESET_VEL);
+
+        bandera_inicio = 0;   //Limpiar la bandera
+
+        // Envia un IN de proceso terminado
+        if( isPC )
+            pc.printf("IN\r\n");
+        else
+            RS_232.printf("IN\r\n");
+    }
+
+    else{
+
+        if( isPC )
+            pc.printf("A1\r\n");
+        else
+            RS_232.printf("A1\r\n");
+    }
+
 }
 
 
@@ -754,6 +819,42 @@
 }
 
 
+void setHOME_eth( char * input, char * output )
+{
+    int value = atoi( input );
+    pin_dir = 1;              //Mover hacia el motor
+    bandera_inicio = 1;      // Informar a ISR_Adv_motor() acerca del Homing
+    setPTO( value * 1000 ); // a la velocidad seteada en KiloHertzios                      
+    
+    if( pin_alm == 0 )
+    {
+        if ( limite_2 == 1 )    // Alarma muy cerca al encoder
+        {
+            sprintf( output,"A2\r\n" );
+        }
+        
+        if ( limite_3 == 1 )    // Alarma muy cerca al encoder
+        {
+            if (bandera_inicio == 1)
+            {
+                sprintf( output,"IN\r\n" );
+            }
+            sprintf( output,"A1\r\n" );
+        }
+        if ( limite_1 == 1 || limite_4 == 1 )    // ERROR DE POSICION
+        {
+            sprintf( output,"A0\r\n" );
+        }
+        
+        else
+            sprintf( output,"OK\r\n" );
+    }
+    else
+        sprintf( output,"AL" );
+    
+    
+}
+