File content as of revision 0:88bfb16a6ad5:

/*------------------------------------------------------------------------------
 * LPC1768_OC_MULTIMODAL 
 *
 * <DEBUG PORT>
 * d19 MAIL-LOOP TOGGLE
 * d20 UART1
 * d21 UART3
 * d22 ERROR mutil-plexer etc.
 * d23 
 * d24 UDP RECEIVE
 * d25 UDP SEND
 * d26 I2C
 */
#include "mbed.h"
#include "UDPSocket.h"
#include "fdc.h"
#include "EthernetInterface.h"

#define SERIAL_CH  2
#define I2C_CH     2

// Constants
static const int NUM_AXES1 = 5;
static const int NUM_AXES2 = 4;

//static const int TX_BYTE_SIZE = 3;
//static const int RX_BYTE_SIZE = 3;
//static const int RX_RETRY_MAX = 200;
static const int NUM_AXES = NUM_AXES1 + NUM_AXES2;
//static const int UDP_TX_SIZE = sizeof(float) * NUM_AXES; // send to pc
//static const int UDP_RX_SIZE = sizeof(float) * NUM_AXES; // recv from pc
//static const int USB_BAUDRATE = 9600;
static const int ICS_BAUDRATE = 1250000;
static const int POS_MID = 7500;
static const int POS_FREE = 255;
static const uint8_t POS_COMMAND = 0x80;
static const char * NETMASK = "255.255.0.0";
static const char * GATEWAY = "0.0.0.0";
static const char * PC_ADDRESS = "192.168.1.2";
static const char * MY_ADDRESS = "192.168.1.3";
static const int PC_RX_PORT = 5000; // from pc
static const int PC_TX_PORT = 5001; // to pc
//static const int MY_PORT = 6007;
static const float MAX_DIFF_DEG = 10;

RawSerial *p_serial1, *p_serial2;;
I2C *p_i2c1, *p_i2c2;
UDPSocket s_sock;
UDPSocket r_sock;

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

// finish
uint8_t g_finish = 0; // b0:uart1  b1:uart2  b2:i2c1  b3:i2c2
uint8_t g_next = 0;

// finish bit
#define BIT_SERIAL1  0x01
#define BIT_SERIAL2  0x02
#define BIT_I2C1     0x04
#define BIT_I2C2     0x08


// udp data
typedef struct {
    uint16_t seq;
    uint16_t dmy;
    float motorData[NUM_AXES]; // 9*4=36byte
} data_t;
union RxUnion {
    data_t d;
    char buf[sizeof(data_t)];
};
typedef struct {
    uint16_t seq;
    uint16_t dmy;
    float motorData[NUM_AXES]; // 9*4=36byte
    float sensorData[FDC_NUM][NUM_DEN]; // 3*4*4=48byte
} data2_t;
union TxUnion {
    data2_t d;
    char buf[sizeof(data2_t)];
};
RxUnion g_rx = {0};
TxUnion g_tx = {0};

Endpoint ep_pc_tx, ep_pc_rx;

// for motor value
uint8_t g_res1[3], g_res2[3]; // motor response
uint8_t g_n1, g_n2; // motor response index
int g_echoCount1, g_echoCount2; // motor command echo counter
uint8_t g_id1, g_id2; // motor id
char g_cmd1[4], g_cmd2[4]; // motor command

static inline void GenCommand(uint8_t id/*1,2,...*/, float cmd_deg, char *cmd/*OUT*/){
    int32_t cmd_fit = POS_MID - int(cmd_deg * 1000 / 34.0);
    cmd[0] = uint8_t(POS_COMMAND | id);
    cmd[1] = uint8_t(cmd_fit >> 7);
    cmd[2] = uint8_t(cmd_fit & 0x7F);
    cmd[3] = 0; // null
}
static inline float ParseResponse(uint8_t id/*1,2,...*/, uint8_t *p, float pre_deg){
    int act_fit = ((*(p+1) & 0x7F) << 7) + (*(p+2) & 0x7F);
    float act_deg = 34 * (POS_MID - act_fit) / 1000.0;
    act_deg = fabs(act_deg - pre_deg) > MAX_DIFF_DEG ? pre_deg : act_deg;
    return act_deg;
}

/*Ticker ticker;
int g_tmCnt = 0;
uint8_t g_tmReq = 0;
static void tick_handler(){
    if(--g_tmCnt <= 0){
        if(g_tmReq == 1){
            g_tmReq = 0;
            led4 = 1;
        }
    }
}*/

static inline void wait_us(uint32_t microsec){
    wait(microsec * 1e-6);
}
static inline void Serial1_puts(float val){
    GenCommand(g_id1+1, val, g_cmd1); // cmd is 3byte + NULL
    g_echoCount1 = 0;
    g_n1 = 0;
    p_serial1->putc(g_cmd1[0]); // async
    //p_serial1->putc(g_cmd1[1]); // async
    //p_serial1->putc(g_cmd1[2]); // async
}
static inline void Serial2_puts(float val){
    GenCommand(g_id2+1, val, g_cmd2); // cmd is 3byte + NULL
    g_echoCount2 = 0;
    g_n2 = 0;
    p_serial2->putc(g_cmd2[0]); // async
    //p_serial2->putc(g_cmd2[1]); // async
    //p_serial2->putc(g_cmd2[2]); // async
}
 
static inline void Serial1_Rx(){
    g_next++;
    // first is echoback. second is motor response.
    uint8_t c = p_serial1->getc();
    if(g_echoCount1 < 3){ // echoback is 3byte
        if(++g_echoCount1 < 3){
            p_serial1->putc(g_cmd1[g_echoCount1]); // async    
        }
        return;
    }
    /*if(g_echoCount1-- > 0){ // echoback is 3byte
        return;
    }*/
    
    // motor response.
    do { 
        g_res1[g_n1++] = c;
        if(p_serial1->readable()) {
            c = p_serial1->getc();
        } else break;
    } while(1);
    
    if(g_n1 == 3){ // response is 3byte
        // receive data set                                      pre_deg
        g_tx.d.motorData[g_id1] = ParseResponse(g_id1+1, g_res1, g_tx.d.motorData[g_id1]);
        g_id1++; // 0-4
        
        // last check
        if(g_id1 == NUM_AXES1){ // motor num is 5
            g_finish |= BIT_SERIAL1; // receive finished
//d20=1;d20=0;
            // next uart start
            g_id2 = NUM_AXES1; // 5-8
            Serial2_puts(g_rx.d.motorData[g_id2]); // serial2 start            
        } else {
            // next id send
            Serial1_puts(g_rx.d.motorData[g_id1]);
        }
    }
}
static inline void Serial2_Rx(){
    g_next++;
    // first is echoback. second is motor response.
    uint8_t c = p_serial2->getc();
    if(g_echoCount2 < 3){ // echoback is 3byte
        if(++g_echoCount2 < 3){
            p_serial2->putc(g_cmd2[g_echoCount2]); // async    
        }
        return;
    }
    /*if(g_echoCount2-- > 0){ // echoback is 3byte
        return;
    }*/
   
    // motor response. 
    do { 
        g_res2[g_n2++] = c;
        if(p_serial2->readable()) {
            c = p_serial2->getc();
        } else break;
    } while(1);
    
    if(g_n2 == 3){ // response is 3byte
        // receive data set                                      pre_deg
        g_tx.d.motorData[g_id2] = ParseResponse(g_id2+1, g_res2, g_tx.d.motorData[g_id2]);
        g_id2++; // 0-3
        
        // last check
        if(g_id2 == NUM_AXES1 + NUM_AXES2){ // motor num is 4
            g_finish |= BIT_SERIAL2; // receive finished
//d20=1;d20=0;
        } else {
            // next id send
            Serial2_puts(g_rx.d.motorData[g_id2]);
        }
    }
}



static inline void setupUdp(void) {
    EthernetInterface eth;
    //serial_usb.printf("Setup UDP\n");
    eth.init(MY_ADDRESS, NETMASK, GATEWAY);

    //serial_usb.printf("eth.init()\n");
    int ret;
    do {
        ret = eth.connect(15000); // use dhcp
    } while(ret < 0);
    //serial_usb.printf("eth.connect()\n");
 
    //const char *ip = eth.getIPAddress();
    //printf("IP: %s\n", ip ? ip : "No IP");
    r_sock.bind(PC_RX_PORT); //rx
    s_sock.bind(PC_TX_PORT); //tx 
    //serial_usb.printf("Port: %d\n", MY_PORT);
}

static inline void UdpReceive(){
    static uint16_t seq = 0;
    //char buf[128];
    int ret = r_sock.receiveFrom(ep_pc_rx, g_rx.buf, sizeof(g_rx.buf));
    if(ret > 0){
//UDP-RECEIVE
d24=1;d24=0;

        //__disable_irq();
        //memcpy(g_rx.buf, buf, sizeof(g_rx.buf));
        //__enable_irq();

        if(g_rx.d.seq != seq){ // seq error
            g_tx.d.dmy++; // seq error count
//ERROR
d22=1;d22=0;
        }
        if(g_rx.d.dmy == 1){ // err count clear request
            g_rx.d.dmy = 0;
            g_tx.d.dmy = 0; // err count clear
        }
        seq = g_rx.d.seq + 1; // next sequence
    }
}


int main() {
    float val;
    int rate = 3; // Measuring rate -- 1:100S/s, 2:200S/s, 3:400S/s
    int capdac[4] = {-1,-1,-1,-1}; // -1: CAPDAC disable, >=0: CAPDAC value
    bool switchResult = true;

/*--- Initialize -------------------------------------------------------------*/
    //ticker.attach(&tick_handler, 0.01); // 10us
    
#if 1
    // udp initialized.
    ep_pc_rx.set_address(PC_ADDRESS, PC_RX_PORT);
    ep_pc_tx.set_address(PC_ADDRESS, PC_TX_PORT);
    setupUdp();
    r_sock.set_blocking(false, 0); // non-blocking
    s_sock.set_blocking(true, 1500); // blocking
#endif
#if 1
    // serial initialized
    p_serial1 = new RawSerial(p13, p14); // uart1
    p_serial1->baud(ICS_BAUDRATE);
    p_serial1->attach(Serial1_Rx, RawSerial::RxIrq);
    p_serial1->format(8, Serial::Even, 1);
    p_serial2 = new RawSerial(p17, p18); // uart3
    p_serial2->baud(ICS_BAUDRATE);
    p_serial2->attach(Serial2_Rx, RawSerial::RxIrq);
    p_serial2->format(8, Serial::Even, 1);
#endif
#if 1
    // i2c initialized
    p_i2c1 = new I2C(p9, p10); // i2c1 sda,scl
    p_i2c1->frequency(400000);
    p_i2c2 = new I2C(p28, p27); // i2c2 sda,scl
    p_i2c2->frequency(400000);
#endif
#if 1    
    // Initialize I2C Interface
    do{
        for(int i=0; i < FDC_NUM; i++) {
            I2C *i2c = (i == 2)? p_i2c2 : p_i2c1;

            // MULTI PLEXER
            if(i != 2){
                switchResult = switchI2c(i2c, i); // MUXのch0にI2Cを切替
                if(!switchResult) break;
            }
            
            if (initFdc(i2c)) {
                config(i2c, rate, capdac);
                //pc.printf("ch %d_FDC1004 is ready.\r\n", i);
            } else {
                //pc.printf("ch %d_FDC1004 is not ready. Please check and restart.\r\n", i);
                switchResult = false;
                wait(1.0);
            }
        } // end_for
    }while(!switchResult);
#endif

    // debug port
    d26 = 0;
    d25 = 0;
    d24 = 0;
    d23 = 0;
    d22 = 0;
    d21 = 0;
    d20 = 0;
    d19 = 0;
    
    g_finish = 0;
    uint16_t seq = 0;

/*--- MAIN LOOP --------------------------------------------------------------*/
    while(1) {
        d19=!d19; //main-loop
        
/*--- UDP RECEIVE (NON-BLOCKING) ---------------------------------------------*/
#if 1
d21=1;
        UdpReceive();
d21=0;
#endif

/*--- SERIAL -----------------------------------------------------------------*/
#if 1
        g_next = 0;
        g_id1 = 0; //0-4
        Serial1_puts(g_rx.d.motorData[g_id1]);   // serial1 start    
#endif
/*--- I2C --------------------------------------------------------------------*/
#if 1
        // Acquire sensor values
        do{
            for (int j=0; j<FDC_NUM ;j++) {
                I2C *i2c = (j == 2)? p_i2c2 : p_i2c1;
                // MULTI PLEXER
                if(j != 2){
                    switchResult = switchI2c(i2c, j);
                    if(!switchResult) break;
                }
                senseCapacitance(i2c, g_tx.d.sensorData[j], capdac);               
            }
        }while(!switchResult);
//d20=1;d20=0;
        //g_finish |= BIT_I2C1;
        //g_finish |= BIT_I2C2;
#endif
/*--- UDP SEND (BLOCKING) ----------------------------------------------------*/
#if 1
        if((g_finish & 0x03) == 0x03){ // all data gathered
L001:
d20=1;
            g_tx.d.seq = seq++;
            int ret = s_sock.sendTo(ep_pc_tx, g_tx.buf, sizeof(g_tx.buf));
d20=0;
            if(ret != sizeof(g_tx.buf)){ // send error
//SEND-ERROR
d22=1;d22=0;
            }
        } else {
//GATHER-ERROR
led1=!led1;
            //memset(&g_tx.d.motorData, 0, sizeof(g_tx.d.motorData)); // motor not needed
            memset(&g_tx.d.sensorData, 0, sizeof(g_tx.d.sensorData)); // sensor is ZERO.
            goto L001;
        }
        g_finish = 0;
#endif            
  
    } // end_while(1)
}