main.cpp

00001 /****************************************************
00002  CONTROL SERVOS PARA BARCO
00003  (C)2021 Pedro M. Campos
00004  -210726 En pruebas para instalación
00005  -210907 V0_3 Centrado por opto en volante, manda pulso de centrado al servo
00006  -
00007 *****************************************************/
00008 
00009 
00010 
00011 #include "declaraciones.h"
00012 #include "EthernetInterface.h"
00013 #include "gps.h"
00014 
00015 #define IP         "192.168.1.247"
00016 #define GATEWAY    "192.168.1.1"
00017 #define MASK       "255.255.255.0"
00018 
00019  
00020 // Network interface
00021 EthernetInterface net;
00022 
00023 
00024 //GPS gps(PC_10, PC_11, PF_8);
00025 
00026 // CANALES DE COMUNICACION
00027 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
00028 Serial serGPS(PC_10, PC_11, 9600);
00029 RawSerial serSERVO(PC_12, PD_2, 115200);
00030 Serial serRS485(PD_5, PD_6, 115200);
00031 
00032 // SALIDAS PARA LEDS
00033 DigitalOut led1(LED1, 0);               //LED1
00034 DigitalOut led2(LED2, 0);
00035 DigitalOut led3(LED3, 0);
00036 
00037 // SALIDAS DE CONTROL COMUNICACIONES
00038 DigitalOut RS485_DIR(PE_3, 0);          //DIR RS485 a RX
00039 DigitalOut RS485_OE(PF_7, 0);           //OE RS485 a tri-state
00040 
00041 // SALIDAS PARA CONTROL DEL SERVO
00042 DigitalOut control_PWR_servo(PF_9, 1);  // Alimentacion del inverter para 220V del servo
00043 DigitalOut SERVO_ON(PG_1, 0);           // Salida de control ON del servo (J7-2)
00044 DigitalOut SERVO_CLR_COUNT(PG_0, 0);    // Salida de control CLR_COUNT del servo (J7-1)
00045 DigitalOut SERVO_ALARM_CLR(PF_0, 0);    // Salida de control ALARM_CLR del servo (J7-4)
00046 DigitalOut SERVO_CONTROL_MODE(PC_6, 0); // Salida de control CONTROL_MODE del servo (J7-3)
00047 DigitalOut SERVO_INC_POS(PB_10, 0);     // Salida de control INC_POS del servo (J1-2)
00048 DigitalOut SERVO_DEC_POS(PB_11, 0);     // Salida de control DEC_POS del servo (J1-3)
00049 DigitalOut SERVO_AUX_OUT(PA_15, 0);     // Salida de control AUX_OUT del servo (J7-10)
00050 
00051 // ENTRADAS DE ESTADO DEL SERVO
00052 DigitalIn SERVO_RDY_IN(PF_13);          //Entrada desde SERVO_RDY, 1=activa;
00053 DigitalIn SERVO_AL_IN(PE_9);            //Entrada desde SERVO_AL, 1=activa;
00054 DigitalIn SERVO_POS_IN(PE_11);          //Entrada desde SERVO_POS, 1=activa;
00055 DigitalIn SERVO_TORQ_L_IN(PF_14);       //Entrada desde SERVO_TORQ_L, 1=activa;
00056 DigitalIn SERVO_Z_SPEED_IN(PE_13);      //Entrada desde SERVO_Z_SPEED, 1=activa;
00057 DigitalIn SRV_D_VEL(PF_2);              //Entrada desde SERVO signo nivel de velocidad 1=positivo
00058 DigitalIn SRV_D_TRQ(PF_1);              //Entrada desde SERVO signo nivel de torque 1=positivo
00059 
00060 // ENTRADAS DEL SELECTOR
00061 DigitalIn SEL0(PC_8);                   //Selector 1 = BABOR/ESTRIBOR, ON(0)=BABOR OFF(1)=ESTRIBOR
00062 DigitalIn SEL1(PC_9);                   //Selector 2
00063 DigitalIn SEL2(PG_2);                   //Selector 3
00064 DigitalIn SEL3(PG_3);                   //Selector 4
00065 
00066 // SALIDAS PWM
00067 PwmOut SRV_C_TRQ_P(PD_13);              // Salida PWM para control del torque POSITIVO del SERVO
00068 PwmOut SRV_C_TRQ_N(PA_0);               // Salida PWM para control del torque NEGATIVO del SERVO
00069 
00070 // ENTRADAS ANALOGICAS
00071 AnalogIn SRV_VEL(PA_3);                 // Entrada analogica de VELOCIDAD del servo
00072 AnalogIn SRV_TRQ(PC_0);                 // Entrada analogica de TORQUE del servo
00073 
00074 // ENTRADAS GENERALES
00075 DigitalIn pulsador(PC_13);              //Pulsador de usuario, 1=pulsado;
00076 
00077 // INTERRUPCIONES EXTERNAS
00078 InterruptIn centrado(PC_3);               //Señal del opto de volante centrado 1=centrado;
00079 
00080 
00081 
00082 // VARIABLES DE SISTEMA
00083 volatile unsigned long tiempo_ms = 0;
00084 volatile bool f_ms = false;             // se pone en cada interrupcion de ms
00085 int babor_estribor = 0;                 // si 0=control servo babor, si 1=control servo estribor
00086 
00087 
00088 long t_ult_recepcion;   // guarda tiempo ultima recepción
00089 //byte nMando;          // numero de mando 0=babor 1=estribor
00090 int servo;              // servo recibido en ultima comunicacion
00091 volatile float latitud;
00092 volatile float longitud;
00093 volatile long fecha_hora;
00094 volatile int n_satelites;
00095 volatile float HDOP;
00096 volatile int rumbo;
00097 volatile int velocidad;  // MNPH
00098 volatile int timon_babor;
00099 volatile int timon_estribor;
00100 volatile enEstadoDireccion estado_direccion;
00101 volatile byte EstadoAnteriorServo;       //Guarda estado anterior del servo para salir de piloto-automatico o rumbo
00102 volatile int retardoActualizacion;       // para no visualizar estado durante este tiempo en ms
00103 volatile bool act_display;               // flag para actualizar display
00104 volatile bool trama_estado_valida;
00105 volatile bool trama_posicion_valida;
00106 
00107 
00108 // VARIABLE CONTROL SERVO
00109 enEstadoServo estado_servo;
00110 int16_t velocidad_servo;
00111 int16_t torque_servo;
00112 long desviacion_servo;
00113 volatile int pos_actual_servo = 0;
00114 volatile int pos_objetivo_servo = 0;
00115 volatile unsigned long tempo_espera = 0;
00116 
00117 int torque_max;                 // torque máximo calculado
00118 int out_torque = TORQUE_MINIMO; // nivel de torque de TORQUE_MINIMO a TORQUE_MAXIMO
00119 
00120 int cons_timon_babor = 0;       // consigna % de timon babor bajado
00121 int cons_timon_estribor =  0;   // consigna % de timon estribor bajado
00122 
00123 float diferencia_rumbo = 0.0;   // Diferencia al Rumbo deseado
00124 
00125 st_datos_servo datos_servo;       // ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
00126 st_datos_GPS datos_GPS;           // ESTRUCTURA DE DATOS DE ESTADO DEL GPS
00127 volatile st_datos_servo datos_otro_servo;  // ESTRUCTURA DE VARIABLES DE ESTADO DEL OTRO SERVO DE DIRECCION
00128 volatile bool f_actualiza_tim_trans=false; // Pone flag para actualizar timones y transmision
00129 
00130 
00131 
00132 // VARIABLES CONFIGURACION DESDE ORDENADOR
00133 int cfg_hora = 7200;  // Suma dos horas a UTC
00134 
00135 
00136 // VARIABLES GENERALES
00137 float ana_velocidad = 0.0;
00138 float ana_torque = 0.0;
00139 bool newtime = false;
00140 ntp_packet in_data;         // para recibir fecha y hora del servidor
00141 
00142 // VARIABLES DE COMUNICACION RS485
00143 volatile bool f_rs485_busy = false;             // comunicacion ocupada
00144 volatile bool recibidoEstadoServo = false;      //Si ha recibido estado del otro servo se pone a true para sincronizacion
00145 
00146 // VARIABLES RECIBIDAS DESDE MANDO
00147 int estado_mando = 0;
00148 byte pulsadores = 0;    // Estado pulsadores del mando
00149 
00150 byte estado_old = DIR_REPOSO;  // ultimo estado enviado
00151 
00152 unsigned int t_centrado = 0;     // tiempo de indicacion de paso por cero del volante
00153 
00154 
00155 
00156 // TIMERS
00157 Timer timer;
00158 Ticker tick_millis;     // Interrupcion para cuenta de milisegundos
00159 Ticker tickRS485us;     // Interrupcion de 100us para com rs485
00160 Timer timerRS485;       // Timer para sincronizacion por rs485 de los mandos
00161 
00162 
00163 
00164 
00165 
00166 
00167 // RUTINAS
00168 
00169 // CUENTA MILISEGUNDOS DESDE RESET
00170 void cuenta_ms() {
00171     tiempo_ms++;
00172     posicion_servo();        // AJUSTA POSICION SERVO SI ES NECESARIO
00173     if(t_centrado > 0){      // Indica por led paso por centro del volante
00174         SERVO_CLR_COUNT = 1;    // Salida de control CLR_COUNT del servo (J7-1) a 1
00175         led3 = 1;
00176         t_centrado--;
00177     }
00178     else{
00179         SERVO_CLR_COUNT = 0;    // Salida de control CLR_COUNT del servo (J7-1) a 0
00180 //        led3 = 1;
00181         }
00182 
00183     f_ms = true;             // se pone en cada interrupcion de ms
00184 }
00185 
00186 
00187 // devuelve cuenta de tiempo en ms
00188 unsigned long millis(void){     // devuelve cuenta de tiempo en ms
00189     return tiempo_ms;
00190 }
00191 
00192 
00193 
00194 
00195 /*
00196 /////////////////////////////////////////////////////////
00197 // PROCESA RECEPCION DESDE SERIAL SERVO
00198 void onSerialServo(void){  // Procesa recepcion SERVO
00199 
00200     while (serSERVO.readable()) {
00201         char c = serSERVO.getc();
00202 //        pc.putc(c);
00203     }
00204 }
00205 */
00206 
00207 
00208 
00209 
00210 
00211 #define FACTOR_INC_RUMBO_BABOR    0.015
00212 #define FACTOR_INC_RUMBO_ESTRIBOR 0.015
00213 
00214 // PARA SIMULACION
00215 // Pocesa rumbo_GPS, llamada cada 250ms
00216 void  procesa_rumbo_GPS(void){
00217 
00218     datos_servo.rumbo -= datos_servo.timon_babor * FACTOR_INC_RUMBO_BABOR;
00219     datos_servo.rumbo += datos_servo.timon_estribor * FACTOR_INC_RUMBO_ESTRIBOR;
00220     //    datos_servo.rumbo = flimit_decimales(datos_servo.rumbo, 2);
00221     if(datos_servo.rumbo>360.0f){
00222         datos_servo.rumbo -= 360.0f;
00223     }
00224     else if(datos_servo.rumbo<0.0f){
00225         datos_servo.rumbo += 360.0f;
00226     }
00227 }
00228 
00229 
00230 
00231 // *****************************************************************************
00232 // Interrupcion de paso por cero del volante
00233 // *****************************************************************************
00234 void int_centrado(void)
00235 {
00236     t_centrado = 20;
00237 }
00238 
00239 
00240 
00241 //******************************************************************************
00242 //******************************************************************************
00243 // PROGRAMA PRINCIPAL 
00244 //******************************************************************************
00245 //******************************************************************************
00246 int main() {
00247 //int nc=0; 
00248 //char out_buffer_2[50];
00249 static int posicion_mando_old = 0;
00250 static bool f_envia_trama_modbus = false;
00251 
00252     /* Registra la interrupcion externa del paso por cero del volante*/
00253     centrado.rise(&int_centrado);
00254 
00255     // Bring up the ethernet interface
00256     pc.printf("\r\n\r\n");
00257     pc.printf("RESET\r\n\r\n");
00258     pc.printf("PROGRAMA CONTROL SERVO V0.3.001\r\n");
00259     pc.printf("CONTROL DE SERVO DE ");
00260     babor_estribor = SEL0.read();
00261     if(babor_estribor == 0)  // Si es servo de babor = 0
00262         pc.printf("BABOR\r\n");
00263     else
00264         pc.printf("ESTRIBOR\r\n");
00265 
00266 
00267     // Open Ethernet connection
00268     int c=0;
00269     do{
00270         net.disconnect();
00271         wait(2.0);     // espera de 1s
00272         int j = net.set_network(IP, MASK, GATEWAY);
00273         printf("set IP status: %i\r\n", j);
00274 
00275         c=net.connect();
00276         if(0 != c) {
00277             printf("Error connecting\r\n");
00278     //        return -1;
00279         }
00280     }while(c != 0);
00281  
00282     // Show the network address
00283     const char *ip = net.get_ip_address();
00284     printf("IP address is: %s\r\n", ip ? ip : "No IP");
00285 
00286     Thread t; // can pass in the stack size here if you run out of memory
00287     t.start(&udp_main);
00288 
00289      
00290     // Inicializa datos_servo ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
00291     datos_servo.n_servo = babor_estribor;
00292     datos_servo.estado = 0;
00293     datos_servo.latitud = 0.0;
00294     datos_servo.longitud = 0.0;
00295     datos_servo.fecha_hora = 0;
00296     datos_servo.velocidad = 0.0;          // MNPH
00297     datos_servo.rumbo = 0.0;
00298     datos_servo.ch_radio = 0;
00299     datos_servo.trim_timon_bab = 0;
00300     datos_servo.trim_trans_bab = 6;
00301     datos_servo.trim_timon_estr = 0;
00302     datos_servo.trim_trans_estr = 6;
00303     datos_servo.timon_babor = 0;          // %de timon babor bajado, para 100% 5s, 1% = 50ms
00304     datos_servo.timon_estribor = 0;       // %de timon estribor bajado, para 100% 5s
00305     datos_servo.rumbo_fijado = 0.0;       // Rumbo fijado al pulsar botón para RUMBO FIJO
00306     datos_servo.rumbo_piloto_auto = 0.0;  // Rumbo fijado por piloto automático
00307     
00308     // Set handler to be called when UDP socket event occurrs.
00309     // sock.attach(&onUDPSocketEvent);
00310 
00311     timer.start();
00312     tick_millis.attach(&cuenta_ms, 0.001);          // the address of the function to be attached (cuenta_ms) and the interval (1 miliseconds)
00313     tickRS485us.attach(&SerialTxRS485, 0.0001);     // Interrupcion de 100us para comunicaciones por RS485 con mando
00314     timerRS485.start();                             // Timer para sincronizacion desde recepcion del otro control servo
00315 
00316     led1 = led2 = led3 =1;
00317     wait(0.5);
00318     led1 = led2 = led3 =0;
00319 
00320 //    serSERVO.attach(&onSerialServo);  // Interrupcion de serial Servo
00321     serRS485.attach(&onSerialRxRS485, Serial::RxIrq);  // Interrupcion de RX serial RS485 (MANDOS)
00322 //    serRS485.attach(&onSerialTxRS485, Serial::TxIrq);  // Interrupcion de TX serial RS485 (MANDOS)
00323 
00324     serGPS.attach(&onSerialGPS);      // Interrupcion de serial GPS
00325 
00326     RS485_OE = 1;   // habilita conversor 3.3V a 5V para RS485
00327 
00328 /*
00329     SRV_C_TRQ_P.period(0.001f);          // 1 kHz or 1 ms
00330     SRV_C_TRQ_N.period(0.001f);          // 1 kHz or 1 ms
00331     SRV_C_TRQ_P=0;
00332     SRV_C_TRQ_N=0;
00333 */
00334 
00335     SERVO_ON = 1;       // Pone servo en ON
00336    
00337 //    uint8_t i=0;
00338 
00339 //    SetDateTime(2020, 4, 25, 10, 00, 0);   // Actualiza hora del RTC
00340 //    ShowDateTime();
00341     
00342     while(1){
00343         
00344         while(f_ms!=true);     // espera interrupcion de 1 ms
00345         f_ms = false;
00346 
00347         procesaGPS();          // Comprueba trama pendiente de GPS y la procesa
00348 
00349         if(datos_servo.estado == DIR_MASTER || datos_servo.estado == DIR_REPOSO)
00350             datos_servo.posicion_mando = desviacion_servo;
00351         else
00352             datos_servo.posicion_mando = pos_objetivo_servo;  // PARA PILOTO AUTOMATICO
00353         
00354         if(datos_servo.posicion_mando != posicion_mando_old){ // Si cambia la posicion del mando envia trama
00355             if(abs(desviacion_servo) >= 50 && datos_servo.estado == DIR_REPOSO && datos_otro_servo.estado == DIR_REPOSO){
00356                 datos_servo.estado = DIR_MASTER;
00357             }
00358         
00359             if(datos_servo.estado == DIR_MASTER || datos_servo.estado == DIR_RUMBO || datos_servo.estado == DIR_PILOTO)
00360                 envia_posicion_mando(datos_servo.posicion_mando);
00361             posicion_mando_old = datos_servo.posicion_mando;
00362         }
00363 
00364         // PROCESA CADA SEGUNDO
00365         if(((tiempo_ms+500)%1000)==0){     // Envia cada segundo
00366 
00367 //            test_modbus();                 // TEST PARA TRAMA MODBUS
00368 //            getTime();
00369 
00370 //            SetDateTime(2020, 4, 25, 10, 00, 0);   // Actualiza hora del RTC
00371 //            ShowDateTime();
00372 //            pc.printf("tiempo_ms:%d\r\n", tiempo_ms);
00373             
00374 //                pc.printf("RDY:%u AL:%u POS:%u TORQ_L:%u Z_SPEED:%u \r\n", SERVO_RDY_IN.read(), SERVO_AL_IN.read(), SERVO_POS_IN.read(), SERVO_TORQ_L_IN.read(), SERVO_Z_SPEED_IN.read());
00375 //            pc.printf("velocidad:%3.3f torque:%3.3f fix:%d satelites:%s\r\n", ana_velocidad, ana_torque, gps.have_fix, gps.NmeaGpsData.NmeaSatelliteTracked);
00376 
00377 //            procesa_servo();                       // lee estado del servo, procesa torque y actualiza
00378 //            envia_posicion_mando(datos_servo.posicion_mando);
00379 
00380 //            printf("tiempo_ms=%d pos_objetivo_servo=%d SEL0=%d ESTADO=%d\r\n", tiempo_ms, pos_objetivo_servo, babor_estribor, datos_servo.estado);
00381         }
00382 
00383 /*
00384         //PROCESO CADA 10ms
00385         if((tiempo_ms%10) == 0){
00386             ana_velocidad = SRV_VEL.read(); // 1.0=VCC, lee entradas analogicas
00387             ana_torque = SRV_TRQ.read();
00388             SRV_C_TRQ_P.pulsewidth(ana_velocidad*0.001f);   // Ajusta PWM de salida DAC
00389             SRV_C_TRQ_N.pulsewidth(ana_torque*0.001f);
00390         }
00391 */
00392         //PROCESO CADA 100ms
00393         if((tiempo_ms%100) == 0){
00394             if (centrado.read() == 0) { // Any of the 2 IOs is low
00395                 led3 = 0; // Toggle LED state
00396             }
00397             else{
00398                 led3 = 1; // Toggle LED state
00399             }
00400 
00401             
00402 //            procesa_rumbo_GPS();            // SIMULA RUMBO SEGUN TIMON PARA GPS
00403 //            procesa_posicion_timones();   // Pocesa activación y posición timones, llamada cada 100ms
00404         }
00405 
00406         //PROCESO CADA 250ms
00407         if((tiempo_ms%250) == 0){
00408             procesa_servo();                // lee estado del servo, procesa torque y actualiza
00409             procesa_estado();               // PROCESA CAMBIO DE ESTADO
00410             procesa_piloto_automatico();    // procesa piloto automático
00411             procesa_posicion_timones();     // Pocesa activación y posición timones, llamada cada 250ms
00412             actualiza_timones();            // ACTUALIZA TIMONES
00413             procesa_rumbo_GPS();            // SIMULA RUMBO SEGUN TIMON PARA GPS
00414             sincroniza_servos();            // SINCRONIZA LOS DOS SERVOS SEGUN SU ESTADO
00415         }
00416 
00417         if(recibidoEstadoServo==true){            //Si ha recibido estado del otro servo está a true para sincronizacion
00418             if(timerRS485.read_ms()>60 && f_envia_trama_modbus == true){
00419                 envia_trama_modbus();          // ENVIA TRAMA MODBUS RS485
00420                 f_envia_trama_modbus = false;
00421             }
00422             if(timerRS485.read_ms()>=120){
00423                 envia_estado_mando();
00424                 recibidoEstadoServo = false;
00425                 f_envia_trama_modbus = true;
00426                 timerRS485.reset();
00427             }
00428         }
00429         else{
00430             int espera = (babor_estribor==0)? 240 :260;
00431             if(timerRS485.read_ms()>espera){      // Si es servo de babor = 240ms, estribor = 260ms, evita colision
00432                 envia_estado_mando();
00433                 recibidoEstadoServo = false;
00434                 f_envia_trama_modbus = true;
00435                 timerRS485.reset();
00436             }
00437             else{
00438                 if(timerRS485.read_ms()>(espera-60) && f_envia_trama_modbus == true){
00439                     envia_trama_modbus();          // ENVIA TRAMA MODBUS RS485
00440                     f_envia_trama_modbus = false;
00441                 }
00442             }
00443         }
00444 
00445 //        control_PWR_servo =  (pulsador.read() == 1)?0:1;
00446         control_PWR_servo = 0;  // encendido
00447         
00448         // PROCESO CADA 10 SEGUNDOS         
00449         if((tiempo_ms%10000)==0){
00450 
00451 /*
00452             if(pos_objetivo_servo == 0){
00453                 pos_objetivo_servo = 200;
00454                 escribe_parametro_servo(0, 13, TORQUE_MAXIMO);
00455                 }
00456             else if(pos_objetivo_servo == 200){
00457                 pos_objetivo_servo = -200;
00458                 escribe_parametro_servo(0, 13, TORQUE_MAXIMO);
00459                 }
00460             else if(pos_objetivo_servo == -200){
00461                 pos_objetivo_servo = 0;
00462                 escribe_parametro_servo(0, 13, TORQUE_MINIMO);
00463                 }
00464  //           printf("\r\n\r\npos_objetivo_servo=%d\r\n\r\n",pos_objetivo_servo);
00465  */
00466         }
00467 
00468 //        Thread::wait(10000);
00469     }
00470 
00471 //    return 0;
00472 }