File content as of revision 1:da6afd392792:

#include "mbed.h"
#include "EthernetInterface.h"


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

 
// Network interface
EthernetInterface net;

// CANALES DE COMUNICACION
Serial pc(SERIAL_TX, SERIAL_RX, 115200);
Serial serGPS(PC_10, PC_11, 9600);
Serial 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

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




long tiempo_ms = 0;
long tiempo = 0;

float velocidad = 0.0;
float torque = 0.0;


Timer timer;

Ticker flipper;

 
void flip() {
    led2 = !led2;
    led3 = !led3;
}


// Time protocol implementation : Address: time.nist.gov UDPPort: 37  
 
typedef struct {
    uint32_t secs;         // Transmit Time-stamp seconds.
}ntp_packet;


/////////////////////////////////////////////////////////
// PROCESA RECEPCION DESDE GPS
void procesa_recepcion(void){  // Procesa recepcion GPS
/*
  while (serGPS.readable()) {
    char c = serGPS.getc();
    pc.putc(c);
  }

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


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

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



/////////////////////////////////////////////////////////
// PROCESA RECEPCION DESDE SERIAL RS485
void onSerialRS485(void){  // Procesa recepcion RS485

    while (serRS485.readable()) {
        char c = serRS485.getc();
        pc.putc(c);
    }
}


 
int main() {
    
    // Bring up the ethernet interface
    pc.printf("PROGRAMA CONTROL SERVO V0.1\r\n");
    pc.printf("PRUEBAS DE COMUNICACION POR UDP\r\n");

    net.disconnect();
    int j = net.set_network(IP,MASK,GATEWAY);
    printf("set IP status: %i \r\n",j);

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

    timer.start();
    
    led2 = 1;
    led3 = 0;
    flipper.attach(&flip, 1.0); // the address of the function to be attached (flip) and the interval (2 seconds)
    wait(0.5);
     
    serSERVO.attach(&onSerialServo);  // Interrupcion de serial Servo
    serRS485.attach(&onSerialRS485);  // Interrupcion de serial RS485 (MANDOS)

    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;
    
    
//    uint8_t i=0;
    while(1){
        
        if(timer.read_us()>=250){
            procesa_recepcion();  // Procesa recepcion GPS
            tiempo++;
            timer.reset();
            if(tiempo%4000==0){     // Envia cada segundo
//                serSERVO.printf("PRUEBA DE COMUNICACION CON EL SERVO\r\n");
                RS485_DIR = 1;  // pone en TX
                wait(0.1);
                serRS485.printf("PRUEBA DE COMUNICACION POR RS485\r\n");
                wait(0.1);
                RS485_DIR = 0;  // pone en RX

//                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\r\n", velocidad, torque);

            }
            if(tiempo%40 == 0){
                velocidad = SRV_VEL.read(); // 1.0=VCC
                torque = SRV_TRQ.read();
                SRV_C_TRQ_P.pulsewidth(velocidad*0.001f);
                SRV_C_TRQ_N.pulsewidth(torque*0.001f);
            }
       }
 
        led1 = ((tiempo/4000)%2 == 0)?1:0;

        SERVO_INC_POS = led2;
        SERVO_DEC_POS = led3;

        SERVO_ON = led1;           // Salida de control ON del servo (J7-2)
        SERVO_CLR_COUNT = led2;    // Salida de control CLR_COUNT del servo (J7-1)
        SERVO_ALARM_CLR = led3;    // Salida de control ALARM_CLR del servo (J7-4)
        SERVO_CONTROL_MODE = led1; // Salida de control CONTROL_MODE del servo (J7-3)
        SERVO_AUX_OUT = led2;      // Salida de control AUX_OUT del servo (J7-10)

                
//        control_PWR_servo =  (pulsador.read() == 1)?0:1;
        control_PWR_servo = 0;  // encendido
                 
        if(tiempo >= 40000){
            tiempo = 0;

            char out_buffer[] = "time";
            char out_buffer_2[50];
            int nc=0; 
            if(0 > sock.sendto("time.nist.gov", 37, out_buffer, sizeof(out_buffer))) {
                printf("Error sending data\r\n");
                return -1;
            }
            
            ntp_packet in_data;
            int n = sock.recvfrom(&sockAddr, &in_data, sizeof(ntp_packet));
            in_data.secs = ntohl( in_data.secs ) - 2208988800;    // 1900-1970
            printf("\r\n\r\nTime Received %lu seconds since 1/01/1900 00:00 GMT\r\n", 
                                (uint32_t)in_data.secs);
            nc = sprintf(out_buffer_2, "%s", ctime(( const time_t* )&in_data.secs) );
            printf("Time = %s\r\n", out_buffer_2);
            
            printf("Time Server Address: %s Port: %d\r\n\r\n", 
                                       sockAddr.get_ip_address(), sockAddr.get_port());        
    
            if(0 > sock.sendto("192.168.1.37", 6000, out_buffer_2, nc)) {
                printf("\r\nError sending data 2\r\n\r\n");
                return -1;
            }
        }

//        Thread::wait(10000);
    }

    // Close the socket and bring down the network interface
    sock.close();
    net.disconnect();
    return 0;
}