Revision 8:b4465148d206, committed 2021-09-07

Comitter:

dapi08

Date:

Tue Sep 07 10:28:08 2021 +0000

Parent:

7:60d157ef1134

Commit message:

SERVOS_V0_3 test de control

Changed in this revision

--- a/main.cpp	Tue Jul 27 14:03:30 2021 +0000
+++ b/main.cpp	Tue Sep 07 10:28:08 2021 +0000
@@ -2,7 +2,8 @@
  CONTROL SERVOS PARA BARCO
  (C)2021 Pedro M. Campos
  -210726 En pruebas para instalación
- 
+ -210907 V0_3 Centrado por opto en volante, manda pulso de centrado al servo
+ -
 *****************************************************/
 
 
@@ -70,10 +71,12 @@
 AnalogIn SRV_VEL(PA_3);                 // Entrada analogica de VELOCIDAD del servo
 AnalogIn SRV_TRQ(PC_0);                 // Entrada analogica de TORQUE del servo
 
-
 // ENTRADAS GENERALES
 DigitalIn pulsador(PC_13);              //Pulsador de usuario, 1=pulsado;
-DigitalIn centrado(PC_3);               //Señal del opto de volante centrado 1=centrado;
+
+// INTERRUPCIONES EXTERNAS
+InterruptIn centrado(PC_3);               //Señal del opto de volante centrado 1=centrado;
+
 
 
 // VARIABLES DE SISTEMA
@@ -83,7 +86,7 @@
 
 
 long t_ult_recepcion;   // guarda tiempo ultima recepción
-//byte nMando;            // numero de mando 0=babor 1=estribor
+//byte nMando;          // numero de mando 0=babor 1=estribor
 int servo;              // servo recibido en ultima comunicacion
 volatile float latitud;
 volatile float longitud;
@@ -146,6 +149,9 @@
 
 byte estado_old = DIR_REPOSO;  // ultimo estado enviado
 
+unsigned int t_centrado = 0;     // tiempo de indicacion de paso por cero del volante
+
+
 
 // TIMERS
 Timer timer;
@@ -164,6 +170,16 @@
 void cuenta_ms() {
     tiempo_ms++;
     posicion_servo();        // AJUSTA POSICION SERVO SI ES NECESARIO
+    if(t_centrado > 0){      // Indica por led paso por centro del volante
+        SERVO_CLR_COUNT = 1;    // Salida de control CLR_COUNT del servo (J7-1) a 1
+        led3 = 1;
+        t_centrado--;
+    }
+    else{
+        SERVO_CLR_COUNT = 0;    // Salida de control CLR_COUNT del servo (J7-1) a 0
+//        led3 = 1;
+        }
+
     f_ms = true;             // se pone en cada interrupcion de ms
 }
 
@@ -202,35 +218,50 @@
     datos_servo.rumbo -= datos_servo.timon_babor * FACTOR_INC_RUMBO_BABOR;
     datos_servo.rumbo += datos_servo.timon_estribor * FACTOR_INC_RUMBO_ESTRIBOR;
     //    datos_servo.rumbo = flimit_decimales(datos_servo.rumbo, 2);
-    if(datos_servo.rumbo>360.0){
-        datos_servo.rumbo -= 360.0;
+    if(datos_servo.rumbo>360.0f){
+        datos_servo.rumbo -= 360.0f;
     }
-    else if(datos_servo.rumbo<0.0){
-        datos_servo.rumbo += 360.0;
+    else if(datos_servo.rumbo<0.0f){
+        datos_servo.rumbo += 360.0f;
     }
 }
 
 
 
-/////////////////////////////////////////////////////////
+// *****************************************************************************
+// Interrupcion de paso por cero del volante
+// *****************************************************************************
+void int_centrado(void)
+{
+    t_centrado = 20;
+}
+
+
+
+//******************************************************************************
+//******************************************************************************
 // PROGRAMA PRINCIPAL 
+//******************************************************************************
+//******************************************************************************
 int main() {
-int nc=0; 
-char out_buffer_2[50];
+//int nc=0; 
+//char out_buffer_2[50];
 static int posicion_mando_old = 0;
 static bool f_envia_trama_modbus = false;
 
+    /* Registra la interrupcion externa del paso por cero del volante*/
+    centrado.rise(&int_centrado);
+
     // Bring up the ethernet interface
     pc.printf("\r\n\r\n");
     pc.printf("RESET\r\n\r\n");
-    pc.printf("PROGRAMA CONTROL SERVO V0.1\r\n");
+    pc.printf("PROGRAMA CONTROL SERVO V0.3.001\r\n");
     pc.printf("CONTROL DE SERVO DE ");
     babor_estribor = SEL0.read();
     if(babor_estribor == 0)  // Si es servo de babor = 0
         pc.printf("BABOR\r\n");
     else
         pc.printf("ESTRIBOR\r\n");
-    
 
 
     // Open Ethernet connection
@@ -329,7 +360,7 @@
                 envia_posicion_mando(datos_servo.posicion_mando);
             posicion_mando_old = datos_servo.posicion_mando;
         }
-            
+
         // PROCESA CADA SEGUNDO
         if(((tiempo_ms+500)%1000)==0){     // Envia cada segundo
 
@@ -360,6 +391,14 @@
 */
         //PROCESO CADA 100ms
         if((tiempo_ms%100) == 0){
+            if (centrado.read() == 0) { // Any of the 2 IOs is low
+                led3 = 0; // Toggle LED state
+            }
+            else{
+                led3 = 1; // Toggle LED state
+            }
+
+            
 //            procesa_rumbo_GPS();            // SIMULA RUMBO SEGUN TIMON PARA GPS
 //            procesa_posicion_timones();   // Pocesa activación y posición timones, llamada cada 100ms
         }
@@ -429,5 +468,5 @@
 //        Thread::wait(10000);
     }
 
-    return 0;
+//    return 0;
 }