Diff: main.cpp

Revision:

3:55227f9877e0

Parent:

2:82fdfeec5799

Child:

4:7564d0ffd595

diff -r 82fdfeec5799 -r 55227f9877e0 main.cpp
--- a/main.cpp	Wed May 27 07:50:09 2020 +0000
+++ b/main.cpp	Wed May 27 09:41:53 2020 +0000
@@ -124,13 +124,15 @@
 ntp_packet in_data;         // para recibir fecha y hora del servidor
 
 // VARIABLES DE COMUNICACION RS485
-volatile bool f_rs485_busy = false;  // comunicacion ocupada
+volatile bool f_rs485_busy = false;             // comunicacion ocupada
+volatile bool recibidoEstadoServo = false;      //Si ha recibido estado del otro servo se pone a true para sincronizacion
+
 
 // TIMERS
 Timer timer;
 Ticker millis;          // Interrupcion para cuenta de milisegundos
-Ticker tick250us;           // Interrupcion de 250us para com rs485
-
+Ticker tickRS485us;     // Interrupcion de 100us para com rs485
+Timer timerRS485;       // Timer para sincronizacion por rs485 de los mandos
 
 
 
@@ -198,7 +200,13 @@
     pc.printf("\r\n\r\n");
     pc.printf("RESET\r\n\r\n");
     pc.printf("PROGRAMA CONTROL SERVO V0.1\r\n");
-    pc.printf("PRUEBAS DE COMUNICACION POR UDP\r\n");
+    pc.printf("CONTROL DE SERVO DE ");
+    if(SEL0.read()==0)  // Si es servo de babor = 0
+        pc.printf("BABOR\r\n");
+    else
+        pc.printf("ESTRIBOR\r\n");
+    
+
 
     // Open Ethernet connection
     int c=0;
@@ -245,8 +253,10 @@
     // sock.attach(&onUDPSocketEvent);
 
     timer.start();
-    millis.attach(&cuenta_ms, 0.001);   // the address of the function to be attached (cuenta_ms) and the interval (1 miliseconds)
-    tick250us.attach(&int250us, 0.0001);       // Interrupcion de 250us para comunicaciones por RS485 con mando
+    millis.attach(&cuenta_ms, 0.001);               // the address of the function to be attached (cuenta_ms) and the interval (1 miliseconds)
+    tickRS485us.attach(&SerialTxRS485, 0.0001);     // Interrupcion de 100us para comunicaciones por RS485 con mando
+    timerRS485.start();                             // Timer para sincronizacion desde recepcion del otro control servo
+
     led1 = led2 = led3 =1;
     wait(0.5);
     led1 = led2 = led3 =0;
@@ -334,10 +344,23 @@
             procesa_posicion_timones();     // Pocesa activación y posición timones, llamada cada 250ms
             actualiza_timones();            // ACTUALIZA TIMONES
             procesa_rumbo_GPS();            // SIMULA RUMBO SEGUN TIMON PARA GPS
-            envia_estado_mando();           // cada 250ms
         }
 
-                
+        if(recibidoEstadoServo==true){            //Si ha recibido estado del otro servo está a true para sincronizacion
+            if(timerRS485.read_ms()>=110){
+                envia_estado_mando();
+                recibidoEstadoServo = false;
+                timerRS485.reset();
+            }
+        }
+        else{
+            int espera = (SEL0.read()==0)? 240 :260;
+            if(timerRS485.read_ms()>=espera){      // Si es servo de babor = 240ms, estribor = 260ms, evita colision
+                envia_estado_mando();
+                timerRS485.reset();
+            }
+        }
+
 //        control_PWR_servo =  (pulsador.read() == 1)?0:1;
         control_PWR_servo = 0;  // encendido