main.cpp

00001 /*------------------------------------------------------------------------------
00002  * LPC1768_OC_MULTIMODAL 
00003  *
00004  * <DEBUG PORT>
00005  * d19 MAIL-LOOP TOGGLE
00006  * d20 UART1
00007  * d21 UART3
00008  * d22 ERROR mutil-plexer etc.
00009  * d23 
00010  * d24 UDP RECEIVE
00011  * d25 UDP SEND
00012  * d26 I2C
00013  */
00014 #include "mbed.h"
00015 #include "UDPSocket.h"
00016 #include "fdc.h"
00017 #include "EthernetInterface.h"
00018 
00019 #define SERIAL_CH  2
00020 #define I2C_CH     2
00021 
00022 // Constants
00023 static const int NUM_AXES1 = 5;
00024 static const int NUM_AXES2 = 4;
00025 
00026 //static const int TX_BYTE_SIZE = 3;
00027 //static const int RX_BYTE_SIZE = 3;
00028 //static const int RX_RETRY_MAX = 200;
00029 static const int NUM_AXES = NUM_AXES1 + NUM_AXES2;
00030 //static const int UDP_TX_SIZE = sizeof(float) * NUM_AXES; // send to pc
00031 //static const int UDP_RX_SIZE = sizeof(float) * NUM_AXES; // recv from pc
00032 //static const int USB_BAUDRATE = 9600;
00033 static const int ICS_BAUDRATE = 1250000;
00034 static const int POS_MID = 7500;
00035 static const int POS_FREE = 255;
00036 static const uint8_t POS_COMMAND = 0x80;
00037 static const char * NETMASK = "255.255.0.0";
00038 static const char * GATEWAY = "0.0.0.0";
00039 static const char * PC_ADDRESS = "192.168.1.2";
00040 static const char * MY_ADDRESS = "192.168.1.3";
00041 static const int PC_RX_PORT = 5000; // from pc
00042 static const int PC_TX_PORT = 5001; // to pc
00043 //static const int MY_PORT = 6007;
00044 static const float MAX_DIFF_DEG = 10;
00045 
00046 RawSerial *p_serial1, *p_serial2;;
00047 I2C *p_i2c1, *p_i2c2;
00048 UDPSocket s_sock;
00049 UDPSocket r_sock;
00050 
00051 DigitalOut led1(LED1);
00052 DigitalOut led2(LED2);
00053 DigitalOut led3(LED3);
00054 DigitalOut led4(LED4);
00055 
00056 // finish
00057 uint8_t g_finish = 0; // b0:uart1  b1:uart2  b2:i2c1  b3:i2c2
00058 uint8_t g_next = 0;
00059 
00060 // finish bit
00061 #define BIT_SERIAL1  0x01
00062 #define BIT_SERIAL2  0x02
00063 #define BIT_I2C1     0x04
00064 #define BIT_I2C2     0x08
00065 
00066 
00067 // udp data
00068 typedef struct {
00069     uint16_t seq;
00070     uint16_t dmy;
00071     float motorData[NUM_AXES]; // 9*4=36byte
00072 } data_t;
00073 union RxUnion {
00074     data_t d;
00075     char buf[sizeof(data_t)];
00076 };
00077 typedef struct {
00078     uint16_t seq;
00079     uint16_t dmy;
00080     float motorData[NUM_AXES]; // 9*4=36byte
00081     float sensorData[FDC_NUM][NUM_DEN]; // 3*4*4=48byte
00082 } data2_t;
00083 union TxUnion {
00084     data2_t d;
00085     char buf[sizeof(data2_t)];
00086 };
00087 RxUnion g_rx = {0};
00088 TxUnion g_tx = {0};
00089 
00090 Endpoint ep_pc_tx, ep_pc_rx;
00091 
00092 // for motor value
00093 uint8_t g_res1[3], g_res2[3]; // motor response
00094 uint8_t g_n1, g_n2; // motor response index
00095 int g_echoCount1, g_echoCount2; // motor command echo counter
00096 uint8_t g_id1, g_id2; // motor id
00097 char g_cmd1[4], g_cmd2[4]; // motor command
00098 
00099 static inline void GenCommand(uint8_t id/*1,2,...*/, float cmd_deg, char *cmd/*OUT*/){
00100     int32_t cmd_fit = POS_MID - int(cmd_deg * 1000 / 34.0);
00101     cmd[0] = uint8_t(POS_COMMAND | id);
00102     cmd[1] = uint8_t(cmd_fit >> 7);
00103     cmd[2] = uint8_t(cmd_fit & 0x7F);
00104     cmd[3] = 0; // null
00105 }
00106 static inline float ParseResponse(uint8_t id/*1,2,...*/, uint8_t *p, float pre_deg){
00107     int act_fit = ((*(p+1) & 0x7F) << 7) + (*(p+2) & 0x7F);
00108     float act_deg = 34 * (POS_MID - act_fit) / 1000.0;
00109     act_deg = fabs(act_deg - pre_deg) > MAX_DIFF_DEG ? pre_deg : act_deg;
00110     return act_deg;
00111 }
00112 
00113 /*Ticker ticker;
00114 int g_tmCnt = 0;
00115 uint8_t g_tmReq = 0;
00116 static void tick_handler(){
00117     if(--g_tmCnt <= 0){
00118         if(g_tmReq == 1){
00119             g_tmReq = 0;
00120             led4 = 1;
00121         }
00122     }
00123 }*/
00124 
00125 static inline void wait_us(uint32_t microsec){
00126     wait(microsec * 1e-6);
00127 }
00128 static inline void Serial1_puts(float val){
00129     GenCommand(g_id1+1, val, g_cmd1); // cmd is 3byte + NULL
00130     g_echoCount1 = 0;
00131     g_n1 = 0;
00132     p_serial1->putc(g_cmd1[0]); // async
00133     //p_serial1->putc(g_cmd1[1]); // async
00134     //p_serial1->putc(g_cmd1[2]); // async
00135 }
00136 static inline void Serial2_puts(float val){
00137     GenCommand(g_id2+1, val, g_cmd2); // cmd is 3byte + NULL
00138     g_echoCount2 = 0;
00139     g_n2 = 0;
00140     p_serial2->putc(g_cmd2[0]); // async
00141     //p_serial2->putc(g_cmd2[1]); // async
00142     //p_serial2->putc(g_cmd2[2]); // async
00143 }
00144  
00145 static inline void Serial1_Rx(){
00146     g_next++;
00147     // first is echoback. second is motor response.
00148     uint8_t c = p_serial1->getc();
00149     if(g_echoCount1 < 3){ // echoback is 3byte
00150         if(++g_echoCount1 < 3){
00151             p_serial1->putc(g_cmd1[g_echoCount1]); // async    
00152         }
00153         return;
00154     }
00155     /*if(g_echoCount1-- > 0){ // echoback is 3byte
00156         return;
00157     }*/
00158     
00159     // motor response.
00160     do { 
00161         g_res1[g_n1++] = c;
00162         if(p_serial1->readable()) {
00163             c = p_serial1->getc();
00164         } else break;
00165     } while(1);
00166     
00167     if(g_n1 == 3){ // response is 3byte
00168         // receive data set                                      pre_deg
00169         g_tx.d.motorData[g_id1] = ParseResponse(g_id1+1, g_res1, g_tx.d.motorData[g_id1]);
00170         g_id1++; // 0-4
00171         
00172         // last check
00173         if(g_id1 == NUM_AXES1){ // motor num is 5
00174             g_finish |= BIT_SERIAL1; // receive finished
00175 //d20=1;d20=0;
00176             // next uart start
00177             g_id2 = NUM_AXES1; // 5-8
00178             Serial2_puts(g_rx.d.motorData[g_id2]); // serial2 start            
00179         } else {
00180             // next id send
00181             Serial1_puts(g_rx.d.motorData[g_id1]);
00182         }
00183     }
00184 }
00185 static inline void Serial2_Rx(){
00186     g_next++;
00187     // first is echoback. second is motor response.
00188     uint8_t c = p_serial2->getc();
00189     if(g_echoCount2 < 3){ // echoback is 3byte
00190         if(++g_echoCount2 < 3){
00191             p_serial2->putc(g_cmd2[g_echoCount2]); // async    
00192         }
00193         return;
00194     }
00195     /*if(g_echoCount2-- > 0){ // echoback is 3byte
00196         return;
00197     }*/
00198    
00199     // motor response. 
00200     do { 
00201         g_res2[g_n2++] = c;
00202         if(p_serial2->readable()) {
00203             c = p_serial2->getc();
00204         } else break;
00205     } while(1);
00206     
00207     if(g_n2 == 3){ // response is 3byte
00208         // receive data set                                      pre_deg
00209         g_tx.d.motorData[g_id2] = ParseResponse(g_id2+1, g_res2, g_tx.d.motorData[g_id2]);
00210         g_id2++; // 0-3
00211         
00212         // last check
00213         if(g_id2 == NUM_AXES1 + NUM_AXES2){ // motor num is 4
00214             g_finish |= BIT_SERIAL2; // receive finished
00215 //d20=1;d20=0;
00216         } else {
00217             // next id send
00218             Serial2_puts(g_rx.d.motorData[g_id2]);
00219         }
00220     }
00221 }
00222 
00223 
00224 
00225 static inline void setupUdp(void) {
00226     EthernetInterface eth;
00227     //serial_usb.printf("Setup UDP\n");
00228     eth.init(MY_ADDRESS, NETMASK, GATEWAY);
00229 
00230     //serial_usb.printf("eth.init()\n");
00231     int ret;
00232     do {
00233         ret = eth.connect(15000); // use dhcp
00234     } while(ret < 0);
00235     //serial_usb.printf("eth.connect()\n");
00236  
00237     //const char *ip = eth.getIPAddress();
00238     //printf("IP: %s\n", ip ? ip : "No IP");
00239     r_sock.bind(PC_RX_PORT); //rx
00240     s_sock.bind(PC_TX_PORT); //tx 
00241     //serial_usb.printf("Port: %d\n", MY_PORT);
00242 }
00243 
00244 static inline void UdpReceive(){
00245     static uint16_t seq = 0;
00246     //char buf[128];
00247     int ret = r_sock.receiveFrom(ep_pc_rx, g_rx.buf, sizeof(g_rx.buf));
00248     if(ret > 0){
00249 //UDP-RECEIVE
00250 d24=1;d24=0;
00251 
00252         //__disable_irq();
00253         //memcpy(g_rx.buf, buf, sizeof(g_rx.buf));
00254         //__enable_irq();
00255 
00256         if(g_rx.d.seq != seq){ // seq error
00257             g_tx.d.dmy++; // seq error count
00258 //ERROR
00259 d22=1;d22=0;
00260         }
00261         if(g_rx.d.dmy == 1){ // err count clear request
00262             g_rx.d.dmy = 0;
00263             g_tx.d.dmy = 0; // err count clear
00264         }
00265         seq = g_rx.d.seq + 1; // next sequence
00266     }
00267 }
00268 
00269 
00270 int main() {
00271     float val;
00272     int rate = 3; // Measuring rate -- 1:100S/s, 2:200S/s, 3:400S/s
00273     int capdac[4] = {-1,-1,-1,-1}; // -1: CAPDAC disable, >=0: CAPDAC value
00274     bool switchResult = true;
00275 
00276 /*--- Initialize -------------------------------------------------------------*/
00277     //ticker.attach(&tick_handler, 0.01); // 10us
00278     
00279 #if 1
00280     // udp initialized.
00281     ep_pc_rx.set_address(PC_ADDRESS, PC_RX_PORT);
00282     ep_pc_tx.set_address(PC_ADDRESS, PC_TX_PORT);
00283     setupUdp();
00284     r_sock.set_blocking(false, 0); // non-blocking
00285     s_sock.set_blocking(true, 1500); // blocking
00286 #endif
00287 #if 1
00288     // serial initialized
00289     p_serial1 = new RawSerial(p13, p14); // uart1
00290     p_serial1->baud(ICS_BAUDRATE);
00291     p_serial1->attach(Serial1_Rx, RawSerial::RxIrq);
00292     p_serial1->format(8, Serial::Even, 1);
00293     p_serial2 = new RawSerial(p17, p18); // uart3
00294     p_serial2->baud(ICS_BAUDRATE);
00295     p_serial2->attach(Serial2_Rx, RawSerial::RxIrq);
00296     p_serial2->format(8, Serial::Even, 1);
00297 #endif
00298 #if 1
00299     // i2c initialized
00300     p_i2c1 = new I2C(p9, p10); // i2c1 sda,scl
00301     p_i2c1->frequency(400000);
00302     p_i2c2 = new I2C(p28, p27); // i2c2 sda,scl
00303     p_i2c2->frequency(400000);
00304 #endif
00305 #if 1    
00306     // Initialize I2C Interface
00307     do{
00308         for(int i=0; i < FDC_NUM; i++) {
00309             I2C *i2c = (i == 2)? p_i2c2 : p_i2c1;
00310 
00311             // MULTI PLEXER
00312             if(i != 2){
00313                 switchResult = switchI2c(i2c, i); // MUXのch0にI2Cを切替
00314                 if(!switchResult) break;
00315             }
00316             
00317             if (initFdc(i2c)) {
00318                 config(i2c, rate, capdac);
00319                 //pc.printf("ch %d_FDC1004 is ready.\r\n", i);
00320             } else {
00321                 //pc.printf("ch %d_FDC1004 is not ready. Please check and restart.\r\n", i);
00322                 switchResult = false;
00323                 wait(1.0);
00324             }
00325         } // end_for
00326     }while(!switchResult);
00327 #endif
00328 
00329     // debug port
00330     d26 = 0;
00331     d25 = 0;
00332     d24 = 0;
00333     d23 = 0;
00334     d22 = 0;
00335     d21 = 0;
00336     d20 = 0;
00337     d19 = 0;
00338     
00339     g_finish = 0;
00340     uint16_t seq = 0;
00341 
00342 /*--- MAIN LOOP --------------------------------------------------------------*/
00343     while(1) {
00344         d19=!d19; //main-loop
00345         
00346 /*--- UDP RECEIVE (NON-BLOCKING) ---------------------------------------------*/
00347 #if 1
00348 d21=1;
00349         UdpReceive();
00350 d21=0;
00351 #endif
00352 
00353 /*--- SERIAL -----------------------------------------------------------------*/
00354 #if 1
00355         g_next = 0;
00356         g_id1 = 0; //0-4
00357         Serial1_puts(g_rx.d.motorData[g_id1]);   // serial1 start    
00358 #endif
00359 /*--- I2C --------------------------------------------------------------------*/
00360 #if 1
00361         // Acquire sensor values
00362         do{
00363             for (int j=0; j<FDC_NUM ;j++) {
00364                 I2C *i2c = (j == 2)? p_i2c2 : p_i2c1;
00365                 // MULTI PLEXER
00366                 if(j != 2){
00367                     switchResult = switchI2c(i2c, j);
00368                     if(!switchResult) break;
00369                 }
00370                 senseCapacitance(i2c, g_tx.d.sensorData[j], capdac);               
00371             }
00372         }while(!switchResult);
00373 //d20=1;d20=0;
00374         //g_finish |= BIT_I2C1;
00375         //g_finish |= BIT_I2C2;
00376 #endif
00377 /*--- UDP SEND (BLOCKING) ----------------------------------------------------*/
00378 #if 1
00379         if((g_finish & 0x03) == 0x03){ // all data gathered
00380 L001:
00381 d20=1;
00382             g_tx.d.seq = seq++;
00383             int ret = s_sock.sendTo(ep_pc_tx, g_tx.buf, sizeof(g_tx.buf));
00384 d20=0;
00385             if(ret != sizeof(g_tx.buf)){ // send error
00386 //SEND-ERROR
00387 d22=1;d22=0;
00388             }
00389         } else {
00390 //GATHER-ERROR
00391 led1=!led1;
00392             //memset(&g_tx.d.motorData, 0, sizeof(g_tx.d.motorData)); // motor not needed
00393             memset(&g_tx.d.sensorData, 0, sizeof(g_tx.d.sensorData)); // sensor is ZERO.
00394             goto L001;
00395         }
00396         g_finish = 0;
00397 #endif            
00398   
00399     } // end_while(1)
00400 }
00401