Diff: main.cpp

Revision:

4:7564d0ffd595

Parent:

3:55227f9877e0

Child:

5:420cb56093d3

--- a/main.cpp	Wed May 27 09:41:53 2020 +0000
+++ b/main.cpp	Thu Jun 18 07:31:09 2020 +0000
@@ -70,6 +70,7 @@
 // VARIABLES DE SISTEMA
 volatile unsigned long tiempo_ms = 0;
 volatile bool f_ms = false;             // se pone en cada interrupcion de ms
+int babor_estribor = 0;                 // si 0=control servo babor, si 1=control servo estribor
 
 
 long t_ult_recepcion;   // guarda tiempo ultima recepción
@@ -92,6 +93,7 @@
 volatile bool trama_estado_valida;
 volatile bool trama_posicion_valida;
 
+
 // VARIABLE CONTROL SERVO
 enEstadoServo estado_servo;
 int16_t velocidad_servo;
@@ -113,6 +115,7 @@
 st_datos_GPS datos_GPS;           // ESTRUCTURA DE DATOS DE ESTADO DEL GPS
 
 
+
 // VARIABLES CONFIGURACION DESDE ORDENADOR
 int cfg_hora = 7200;  // Suma dos horas a UTC
 
@@ -127,10 +130,16 @@
 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
 
+// VARIABLES RECIBIDAS DESDE MANDO
+int estado_mando = 0;
+byte pulsadores = 0;    // Estado pulsadores del mando
+
+byte estado_old = DIR_REPOSO;  // ultimo estado enviado
+
 
 // TIMERS
 Timer timer;
-Ticker millis;          // Interrupcion para cuenta de milisegundos
+Ticker tick_millis;     // Interrupcion para cuenta de milisegundos
 Ticker tickRS485us;     // Interrupcion de 100us para com rs485
 Timer timerRS485;       // Timer para sincronizacion por rs485 de los mandos
 
@@ -139,7 +148,6 @@
 
 
 
-
 // RUTINAS
 
 // CUENTA MILISEGUNDOS DESDE RESET
@@ -150,6 +158,12 @@
 }
 
 
+// devuelve cuenta de tiempo en ms
+unsigned long millis(void){     // devuelve cuenta de tiempo en ms
+    return tiempo_ms;
+}
+
+
 
 
 /*
@@ -201,7 +215,8 @@
     pc.printf("RESET\r\n\r\n");
     pc.printf("PROGRAMA CONTROL SERVO V0.1\r\n");
     pc.printf("CONTROL DE SERVO DE ");
-    if(SEL0.read()==0)  // Si es servo de babor = 0
+    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");
@@ -232,7 +247,7 @@
 
      
     // Inicializa datos_servo ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
-    datos_servo.n_servo = SEL0.read();
+    datos_servo.n_servo = babor_estribor;
     datos_servo.estado = 0;
     datos_servo.latitud = 0.0;
     datos_servo.longitud = 0.0;
@@ -253,7 +268,7 @@
     // 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)
+    tick_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
 
@@ -262,7 +277,7 @@
     led1 = led2 = led3 =0;
 
 //    serSERVO.attach(&onSerialServo);  // Interrupcion de serial Servo
-//    serRS485.attach(&onSerialRxRS485, RxIrq);  // Interrupcion de RX serial RS485 (MANDOS)
+    serRS485.attach(&onSerialRxRS485, Serial::RxIrq);  // Interrupcion de RX serial RS485 (MANDOS)
 //    serRS485.attach(&onSerialTxRS485, Serial::TxIrq);  // Interrupcion de TX serial RS485 (MANDOS)
 
     serGPS.attach(&onSerialGPS);      // Interrupcion de serial GPS
@@ -320,7 +335,7 @@
 //            procesa_servo();                       // lee estado del servo, procesa torque y actualiza
 //            envia_posicion_mando(datos_servo.posicion_mando);
 
-//            printf("tiempo_ms=%d pos_objetivo_servo=%d SEL0=%d ESTADO=%d\r\n", tiempo_ms, pos_objetivo_servo, SEL0.read(), datos_servo.estado);
+//            printf("tiempo_ms=%d pos_objetivo_servo=%d SEL0=%d ESTADO=%d\r\n", tiempo_ms, pos_objetivo_servo, babor_estribor, datos_servo.estado);
         }
 
 /*
@@ -341,6 +356,8 @@
         //PROCESO CADA 250ms
         if((tiempo_ms%250) == 0){
             procesa_servo();                // lee estado del servo, procesa torque y actualiza
+            procesa_estado();               // PROCESA CAMBIO DE ESTADO
+            procesa_piloto_automatico();    // procesa piloto automático
             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
@@ -354,7 +371,7 @@
             }
         }
         else{
-            int espera = (SEL0.read()==0)? 240 :260;
+            int espera = (babor_estribor==0)? 240 :260;
             if(timerRS485.read_ms()>=espera){      // Si es servo de babor = 240ms, estribor = 260ms, evita colision
                 envia_estado_mando();
                 timerRS485.reset();