File content as of revision 4:7564d0ffd595:

#include "declaraciones.h"
#include "EthernetInterface.h"
#include "gps.h"

#define IP         "192.168.1.247"
#define GATEWAY    "192.168.1.1"
#define MASK       "255.255.255.0"

 
// Network interface
EthernetInterface net;


//GPS gps(PC_10, PC_11, PF_8);

// CANALES DE COMUNICACION
Serial pc(SERIAL_TX, SERIAL_RX, 115200);
Serial serGPS(PC_10, PC_11, 9600);
RawSerial serSERVO(PC_12, PD_2, 115200);
Serial serRS485(PD_5, PD_6, 115200);

// SALIDAS PARA LEDS
DigitalOut led1(LED1, 0);               //LED1
DigitalOut led2(LED2, 0);
DigitalOut led3(LED3, 0);

// SALIDAS DE CONTROL COMUNICACIONES
DigitalOut RS485_DIR(PE_3, 0);          //DIR RS485 a RX
DigitalOut RS485_OE(PF_7, 0);           //OE RS485 a tri-state

// SALIDAS PARA CONTROL DEL SERVO
DigitalOut control_PWR_servo(PF_9, 1);  // Alimentacion del inverter para 220V del servo
DigitalOut SERVO_ON(PG_1, 0);           // Salida de control ON del servo (J7-2)
DigitalOut SERVO_CLR_COUNT(PG_0, 0);    // Salida de control CLR_COUNT del servo (J7-1)
DigitalOut SERVO_ALARM_CLR(PF_0, 0);    // Salida de control ALARM_CLR del servo (J7-4)
DigitalOut SERVO_CONTROL_MODE(PC_6, 0); // Salida de control CONTROL_MODE del servo (J7-3)
DigitalOut SERVO_INC_POS(PB_10, 0);     // Salida de control INC_POS del servo (J1-2)
DigitalOut SERVO_DEC_POS(PB_11, 0);     // Salida de control DEC_POS del servo (J1-3)
DigitalOut SERVO_AUX_OUT(PA_15, 0);     // Salida de control AUX_OUT del servo (J7-10)

// ENTRADAS DE ESTADO DEL SERVO
DigitalIn SERVO_RDY_IN(PF_13);          //Entrada desde SERVO_RDY, 1=activa;
DigitalIn SERVO_AL_IN(PE_9);            //Entrada desde SERVO_AL, 1=activa;
DigitalIn SERVO_POS_IN(PE_11);          //Entrada desde SERVO_POS, 1=activa;
DigitalIn SERVO_TORQ_L_IN(PF_14);       //Entrada desde SERVO_TORQ_L, 1=activa;
DigitalIn SERVO_Z_SPEED_IN(PE_13);      //Entrada desde SERVO_Z_SPEED, 1=activa;
DigitalIn SRV_D_VEL(PF_2);              //Entrada desde SERVO signo nivel de velocidad 1=positivo
DigitalIn SRV_D_TRQ(PF_1);              //Entrada desde SERVO signo nivel de torque 1=positivo

// ENTRADAS DEL SELECTOR
DigitalIn SEL0(PC_8);                   //Selector 1 = BABOR/ESTRIBOR, ON(0)=BABOR OFF(1)=ESTRIBOR
DigitalIn SEL1(PC_9);                   //Selector 2
DigitalIn SEL2(PG_2);                   //Selector 3
DigitalIn SEL3(PG_3);                   //Selector 4

// SALIDAS PWM
PwmOut SRV_C_TRQ_P(PD_13);              // Salida PWM para control del torque POSITIVO del SERVO
PwmOut SRV_C_TRQ_N(PA_0);               // Salida PWM para control del torque NEGATIVO del SERVO

// ENTRADAS ANALOGICAS
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;



// 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
byte nMando;            // numero de mando 0=babor 1=estribor
int servo;              // servo recibido en ultima comunicacion
volatile float latitud;
volatile float longitud;
volatile long fecha_hora;
volatile int n_satelites;
volatile float HDOP;
volatile int rumbo;
volatile int velocidad;  // MNPH
volatile int timon_babor;
volatile int timon_estribor;
volatile enEstadoDireccion estado_direccion;
volatile byte EstadoAnteriorServo;       //Guarda estado anterior del servo para salir de piloto-automatico o rumbo
volatile byte estado_otro_servo;         // Estado del otro servo
volatile int retardoActualizacion;       // para no visualizar estado durante este tiempo en ms
volatile bool act_display;               // flag para actualizar display
volatile bool trama_estado_valida;
volatile bool trama_posicion_valida;


// VARIABLE CONTROL SERVO
enEstadoServo estado_servo;
int16_t velocidad_servo;
int16_t torque_servo;
long desviacion_servo;
volatile int pos_actual_servo = 0;
volatile int pos_objetivo_servo = 0;
volatile unsigned long tempo_espera = 0;

int torque_max;                 // torque máximo calculado
int out_torque = TORQUE_MINIMO; // nivel de torque de TORQUE_MINIMO a TORQUE_MAXIMO

int cons_timon_babor = 0;       // consigna % de timon babor bajado
int cons_timon_estribor =  0;   // consigna % de timon estribor bajado

float diferencia_rumbo = 0.0;   // Diferencia al Rumbo deseado

st_datos_servo datos_servo;       // ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
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


// VARIABLES GENERALES
float ana_velocidad = 0.0;
float ana_torque = 0.0;
bool newtime = false;
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 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 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






// RUTINAS

// CUENTA MILISEGUNDOS DESDE RESET
void cuenta_ms() {
    tiempo_ms++;
    posicion_servo();        // AJUSTA POSICION SERVO SI ES NECESARIO
    f_ms = true;             // se pone en cada interrupcion de ms
}


// devuelve cuenta de tiempo en ms
unsigned long millis(void){     // devuelve cuenta de tiempo en ms
    return tiempo_ms;
}




/*
/////////////////////////////////////////////////////////
// PROCESA RECEPCION DESDE SERIAL SERVO
void onSerialServo(void){  // Procesa recepcion SERVO

    while (serSERVO.readable()) {
        char c = serSERVO.getc();
//        pc.putc(c);
    }
}
*/





#define FACTOR_INC_RUMBO_BABOR    0.015
#define FACTOR_INC_RUMBO_ESTRIBOR 0.015

// PARA SIMULACION
// Pocesa rumbo_GPS, llamada cada 250ms
void  procesa_rumbo_GPS(void){

    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;
    }
    else if(datos_servo.rumbo<0.0){
        datos_servo.rumbo += 360.0;
    }
}



/////////////////////////////////////////////////////////
// PROGRAMA PRINCIPAL 
int main() {
int nc=0; 
char out_buffer_2[50];
static int posicion_mando_old = 0;


    // 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("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
    int c=0;
    do{
        net.disconnect();
        wait(2.0);     // espera de 1s
        int j = net.set_network(IP, MASK, GATEWAY);
        printf("set IP status: %i\r\n", j);

        c=net.connect();
        if(0 != c) {
            printf("Error connecting\r\n");
    //        return -1;
        }
    }while(c != 0);
 
    // Show the network address
    const char *ip = net.get_ip_address();
    printf("IP address is: %s\r\n", ip ? ip : "No IP");

    Thread t; // can pass in the stack size here if you run out of memory
    t.start(&udp_main);

     
    // Inicializa datos_servo ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
    datos_servo.n_servo = babor_estribor;
    datos_servo.estado = 0;
    datos_servo.latitud = 0.0;
    datos_servo.longitud = 0.0;
    datos_servo.fecha_hora = 0;
    datos_servo.velocidad = 0.0;          // MNPH
    datos_servo.rumbo = 0.0;
    datos_servo.ch_radio = 0;
    datos_servo.trim_timon_bab = 0;
    datos_servo.trim_trans_bab = 6;
    datos_servo.trim_timon_estr = 0;
    datos_servo.trim_trans_estr = 6;
    datos_servo.timon_babor = 0;          // %de timon babor bajado, para 100% 5s, 1% = 50ms
    datos_servo.timon_estribor = 0;       // %de timon estribor bajado, para 100% 5s
    datos_servo.rumbo_fijado = 0.0;       // Rumbo fijado al pulsar botón para RUMBO FIJO
    datos_servo.rumbo_piloto_auto = 0.0;  // Rumbo fijado por piloto automático
    
    // Set handler to be called when UDP socket event occurrs.
    // sock.attach(&onUDPSocketEvent);

    timer.start();
    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

    led1 = led2 = led3 =1;
    wait(0.5);
    led1 = led2 = led3 =0;

//    serSERVO.attach(&onSerialServo);  // Interrupcion de serial Servo
    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

    RS485_OE = 1;   // habilita conversor 3.3V a 5V para RS485

/*
    SRV_C_TRQ_P.period(0.001f);          // 1 kHz or 1 ms
    SRV_C_TRQ_N.period(0.001f);          // 1 kHz or 1 ms
    SRV_C_TRQ_P=0;
    SRV_C_TRQ_N=0;
*/

    SERVO_ON = 1;       // Pone servo en ON
   
//    uint8_t i=0;

//    SetDateTime(2020, 4, 25, 10, 00, 0);   // Actualiza hora del RTC
//    ShowDateTime();
    
    while(1){
        
        while(f_ms!=true);     // espera interrupcion de 1 ms
        f_ms = false;

        procesaGPS();          // Comprueba trama pendiente de GPS y la procesa

        if(datos_servo.estado == DIR_MASTER || datos_servo.estado == DIR_REPOSO)
            datos_servo.posicion_mando = desviacion_servo;
        else
            datos_servo.posicion_mando = pos_objetivo_servo;  // PARA PILOTO AUTOMATICO
        
        if(datos_servo.posicion_mando != posicion_mando_old){ // Si cambia la posicion del mando envia trama
            if(abs(desviacion_servo) >= 50 && datos_servo.estado == DIR_REPOSO && estado_otro_servo == DIR_REPOSO){
                datos_servo.estado = DIR_MASTER;
            }
        
            if(datos_servo.estado == DIR_MASTER || datos_servo.estado == DIR_RUMBO || datos_servo.estado == DIR_PILOTO)
                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

//            getTime();

//            SetDateTime(2020, 4, 25, 10, 00, 0);   // Actualiza hora del RTC
//            ShowDateTime();
//            pc.printf("tiempo_ms:%d\r\n", tiempo_ms);
            
//                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());
//            pc.printf("velocidad:%3.3f torque:%3.3f fix:%d satelites:%s\r\n", ana_velocidad, ana_torque, gps.have_fix, gps.NmeaGpsData.NmeaSatelliteTracked);

//            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, babor_estribor, datos_servo.estado);
        }

/*
        //PROCESO CADA 10ms
        if((tiempo_ms%10) == 0){
            ana_velocidad = SRV_VEL.read(); // 1.0=VCC, lee entradas analogicas
            ana_torque = SRV_TRQ.read();
            SRV_C_TRQ_P.pulsewidth(ana_velocidad*0.001f);   // Ajusta PWM de salida DAC
            SRV_C_TRQ_N.pulsewidth(ana_torque*0.001f);
        }
*/
        //PROCESO CADA 100ms
        if((tiempo_ms%100) == 0){
//            procesa_rumbo_GPS();            // SIMULA RUMBO SEGUN TIMON PARA GPS
//            procesa_posicion_timones();   // Pocesa activación y posición timones, llamada cada 100ms
        }

        //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
        }

        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 = (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();
            }
        }

//        control_PWR_servo =  (pulsador.read() == 1)?0:1;
        control_PWR_servo = 0;  // encendido
        
        // PROCESO CADA 10 SEGUNDOS         
        if((tiempo_ms%10000)==0){

/*
            if(pos_objetivo_servo == 0){
                pos_objetivo_servo = 200;
                escribe_parametro_servo(0, 13, TORQUE_MAXIMO);
                }
            else if(pos_objetivo_servo == 200){
                pos_objetivo_servo = -200;
                escribe_parametro_servo(0, 13, TORQUE_MAXIMO);
                }
            else if(pos_objetivo_servo == -200){
                pos_objetivo_servo = 0;
                escribe_parametro_servo(0, 13, TORQUE_MINIMO);
                }
 //           printf("\r\n\r\npos_objetivo_servo=%d\r\n\r\n",pos_objetivo_servo);
 */
        }

//        Thread::wait(10000);
    }

    return 0;
}