Masayoshi Abe / Mbed 2 deprecated WireFSHandControl

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed mbed-rtos EthernetInterface

main.cpp@0:88bfb16a6ad5, 2020-04-24 (annotated)

Committer:
Masayoshi
Date:
Fri Apr 24 03:11:11 2020 +0000
Revision:
0:88bfb16a6ad5
KYOSO edition

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Masayoshi 0:88bfb16a6ad5 1 /*------------------------------------------------------------------------------
Masayoshi 0:88bfb16a6ad5 2 * LPC1768_OC_MULTIMODAL
Masayoshi 0:88bfb16a6ad5 3 *
Masayoshi 0:88bfb16a6ad5 4 * <DEBUG PORT>
Masayoshi 0:88bfb16a6ad5 5 * d19 MAIL-LOOP TOGGLE
Masayoshi 0:88bfb16a6ad5 6 * d20 UART1
Masayoshi 0:88bfb16a6ad5 7 * d21 UART3
Masayoshi 0:88bfb16a6ad5 8 * d22 ERROR mutil-plexer etc.
Masayoshi 0:88bfb16a6ad5 9 * d23
Masayoshi 0:88bfb16a6ad5 10 * d24 UDP RECEIVE
Masayoshi 0:88bfb16a6ad5 11 * d25 UDP SEND
Masayoshi 0:88bfb16a6ad5 12 * d26 I2C
Masayoshi 0:88bfb16a6ad5 13 */
Masayoshi 0:88bfb16a6ad5 14 #include "mbed.h"
Masayoshi 0:88bfb16a6ad5 15 #include "UDPSocket.h"
Masayoshi 0:88bfb16a6ad5 16 #include "fdc.h"
Masayoshi 0:88bfb16a6ad5 17 #include "EthernetInterface.h"
Masayoshi 0:88bfb16a6ad5 18
Masayoshi 0:88bfb16a6ad5 19 #define SERIAL_CH 2
Masayoshi 0:88bfb16a6ad5 20 #define I2C_CH 2
Masayoshi 0:88bfb16a6ad5 21
Masayoshi 0:88bfb16a6ad5 22 // Constants
Masayoshi 0:88bfb16a6ad5 23 static const int NUM_AXES1 = 5;
Masayoshi 0:88bfb16a6ad5 24 static const int NUM_AXES2 = 4;
Masayoshi 0:88bfb16a6ad5 25
Masayoshi 0:88bfb16a6ad5 26 //static const int TX_BYTE_SIZE = 3;
Masayoshi 0:88bfb16a6ad5 27 //static const int RX_BYTE_SIZE = 3;
Masayoshi 0:88bfb16a6ad5 28 //static const int RX_RETRY_MAX = 200;
Masayoshi 0:88bfb16a6ad5 29 static const int NUM_AXES = NUM_AXES1 + NUM_AXES2;
Masayoshi 0:88bfb16a6ad5 30 //static const int UDP_TX_SIZE = sizeof(float) * NUM_AXES; // send to pc
Masayoshi 0:88bfb16a6ad5 31 //static const int UDP_RX_SIZE = sizeof(float) * NUM_AXES; // recv from pc
Masayoshi 0:88bfb16a6ad5 32 //static const int USB_BAUDRATE = 9600;
Masayoshi 0:88bfb16a6ad5 33 static const int ICS_BAUDRATE = 1250000;
Masayoshi 0:88bfb16a6ad5 34 static const int POS_MID = 7500;
Masayoshi 0:88bfb16a6ad5 35 static const int POS_FREE = 255;
Masayoshi 0:88bfb16a6ad5 36 static const uint8_t POS_COMMAND = 0x80;
Masayoshi 0:88bfb16a6ad5 37 static const char * NETMASK = "255.255.0.0";
Masayoshi 0:88bfb16a6ad5 38 static const char * GATEWAY = "0.0.0.0";
Masayoshi 0:88bfb16a6ad5 39 static const char * PC_ADDRESS = "192.168.1.2";
Masayoshi 0:88bfb16a6ad5 40 static const char * MY_ADDRESS = "192.168.1.3";
Masayoshi 0:88bfb16a6ad5 41 static const int PC_RX_PORT = 5000; // from pc
Masayoshi 0:88bfb16a6ad5 42 static const int PC_TX_PORT = 5001; // to pc
Masayoshi 0:88bfb16a6ad5 43 //static const int MY_PORT = 6007;
Masayoshi 0:88bfb16a6ad5 44 static const float MAX_DIFF_DEG = 10;
Masayoshi 0:88bfb16a6ad5 45
Masayoshi 0:88bfb16a6ad5 46 RawSerial *p_serial1, *p_serial2;;
Masayoshi 0:88bfb16a6ad5 47 I2C *p_i2c1, *p_i2c2;
Masayoshi 0:88bfb16a6ad5 48 UDPSocket s_sock;
Masayoshi 0:88bfb16a6ad5 49 UDPSocket r_sock;
Masayoshi 0:88bfb16a6ad5 50
Masayoshi 0:88bfb16a6ad5 51 DigitalOut led1(LED1);
Masayoshi 0:88bfb16a6ad5 52 DigitalOut led2(LED2);
Masayoshi 0:88bfb16a6ad5 53 DigitalOut led3(LED3);
Masayoshi 0:88bfb16a6ad5 54 DigitalOut led4(LED4);
Masayoshi 0:88bfb16a6ad5 55
Masayoshi 0:88bfb16a6ad5 56 // finish
Masayoshi 0:88bfb16a6ad5 57 uint8_t g_finish = 0; // b0:uart1 b1:uart2 b2:i2c1 b3:i2c2
Masayoshi 0:88bfb16a6ad5 58 uint8_t g_next = 0;
Masayoshi 0:88bfb16a6ad5 59
Masayoshi 0:88bfb16a6ad5 60 // finish bit
Masayoshi 0:88bfb16a6ad5 61 #define BIT_SERIAL1 0x01
Masayoshi 0:88bfb16a6ad5 62 #define BIT_SERIAL2 0x02
Masayoshi 0:88bfb16a6ad5 63 #define BIT_I2C1 0x04
Masayoshi 0:88bfb16a6ad5 64 #define BIT_I2C2 0x08
Masayoshi 0:88bfb16a6ad5 65
Masayoshi 0:88bfb16a6ad5 66
Masayoshi 0:88bfb16a6ad5 67 // udp data
Masayoshi 0:88bfb16a6ad5 68 typedef struct {
Masayoshi 0:88bfb16a6ad5 69 uint16_t seq;
Masayoshi 0:88bfb16a6ad5 70 uint16_t dmy;
Masayoshi 0:88bfb16a6ad5 71 float motorData[NUM_AXES]; // 9*4=36byte
Masayoshi 0:88bfb16a6ad5 72 } data_t;
Masayoshi 0:88bfb16a6ad5 73 union RxUnion {
Masayoshi 0:88bfb16a6ad5 74 data_t d;
Masayoshi 0:88bfb16a6ad5 75 char buf[sizeof(data_t)];
Masayoshi 0:88bfb16a6ad5 76 };
Masayoshi 0:88bfb16a6ad5 77 typedef struct {
Masayoshi 0:88bfb16a6ad5 78 uint16_t seq;
Masayoshi 0:88bfb16a6ad5 79 uint16_t dmy;
Masayoshi 0:88bfb16a6ad5 80 float motorData[NUM_AXES]; // 9*4=36byte
Masayoshi 0:88bfb16a6ad5 81 float sensorData[FDC_NUM][NUM_DEN]; // 3*4*4=48byte
Masayoshi 0:88bfb16a6ad5 82 } data2_t;
Masayoshi 0:88bfb16a6ad5 83 union TxUnion {
Masayoshi 0:88bfb16a6ad5 84 data2_t d;
Masayoshi 0:88bfb16a6ad5 85 char buf[sizeof(data2_t)];
Masayoshi 0:88bfb16a6ad5 86 };
Masayoshi 0:88bfb16a6ad5 87 RxUnion g_rx = {0};
Masayoshi 0:88bfb16a6ad5 88 TxUnion g_tx = {0};
Masayoshi 0:88bfb16a6ad5 89
Masayoshi 0:88bfb16a6ad5 90 Endpoint ep_pc_tx, ep_pc_rx;
Masayoshi 0:88bfb16a6ad5 91
Masayoshi 0:88bfb16a6ad5 92 // for motor value
Masayoshi 0:88bfb16a6ad5 93 uint8_t g_res1[3], g_res2[3]; // motor response
Masayoshi 0:88bfb16a6ad5 94 uint8_t g_n1, g_n2; // motor response index
Masayoshi 0:88bfb16a6ad5 95 int g_echoCount1, g_echoCount2; // motor command echo counter
Masayoshi 0:88bfb16a6ad5 96 uint8_t g_id1, g_id2; // motor id
Masayoshi 0:88bfb16a6ad5 97 char g_cmd1[4], g_cmd2[4]; // motor command
Masayoshi 0:88bfb16a6ad5 98
Masayoshi 0:88bfb16a6ad5 99 static inline void GenCommand(uint8_t id/*1,2,...*/, float cmd_deg, char *cmd/*OUT*/){
Masayoshi 0:88bfb16a6ad5 100 int32_t cmd_fit = POS_MID - int(cmd_deg * 1000 / 34.0);
Masayoshi 0:88bfb16a6ad5 101 cmd[0] = uint8_t(POS_COMMAND | id);
Masayoshi 0:88bfb16a6ad5 102 cmd[1] = uint8_t(cmd_fit >> 7);
Masayoshi 0:88bfb16a6ad5 103 cmd[2] = uint8_t(cmd_fit & 0x7F);
Masayoshi 0:88bfb16a6ad5 104 cmd[3] = 0; // null
Masayoshi 0:88bfb16a6ad5 105 }
Masayoshi 0:88bfb16a6ad5 106 static inline float ParseResponse(uint8_t id/*1,2,...*/, uint8_t *p, float pre_deg){
Masayoshi 0:88bfb16a6ad5 107 int act_fit = ((*(p+1) & 0x7F) << 7) + (*(p+2) & 0x7F);
Masayoshi 0:88bfb16a6ad5 108 float act_deg = 34 * (POS_MID - act_fit) / 1000.0;
Masayoshi 0:88bfb16a6ad5 109 act_deg = fabs(act_deg - pre_deg) > MAX_DIFF_DEG ? pre_deg : act_deg;
Masayoshi 0:88bfb16a6ad5 110 return act_deg;
Masayoshi 0:88bfb16a6ad5 111 }
Masayoshi 0:88bfb16a6ad5 112
Masayoshi 0:88bfb16a6ad5 113 /*Ticker ticker;
Masayoshi 0:88bfb16a6ad5 114 int g_tmCnt = 0;
Masayoshi 0:88bfb16a6ad5 115 uint8_t g_tmReq = 0;
Masayoshi 0:88bfb16a6ad5 116 static void tick_handler(){
Masayoshi 0:88bfb16a6ad5 117 if(--g_tmCnt <= 0){
Masayoshi 0:88bfb16a6ad5 118 if(g_tmReq == 1){
Masayoshi 0:88bfb16a6ad5 119 g_tmReq = 0;
Masayoshi 0:88bfb16a6ad5 120 led4 = 1;
Masayoshi 0:88bfb16a6ad5 121 }
Masayoshi 0:88bfb16a6ad5 122 }
Masayoshi 0:88bfb16a6ad5 123 }*/
Masayoshi 0:88bfb16a6ad5 124
Masayoshi 0:88bfb16a6ad5 125 static inline void wait_us(uint32_t microsec){
Masayoshi 0:88bfb16a6ad5 126 wait(microsec * 1e-6);
Masayoshi 0:88bfb16a6ad5 127 }
Masayoshi 0:88bfb16a6ad5 128 static inline void Serial1_puts(float val){
Masayoshi 0:88bfb16a6ad5 129 GenCommand(g_id1+1, val, g_cmd1); // cmd is 3byte + NULL
Masayoshi 0:88bfb16a6ad5 130 g_echoCount1 = 0;
Masayoshi 0:88bfb16a6ad5 131 g_n1 = 0;
Masayoshi 0:88bfb16a6ad5 132 p_serial1->putc(g_cmd1[0]); // async
Masayoshi 0:88bfb16a6ad5 133 //p_serial1->putc(g_cmd1[1]); // async
Masayoshi 0:88bfb16a6ad5 134 //p_serial1->putc(g_cmd1[2]); // async
Masayoshi 0:88bfb16a6ad5 135 }
Masayoshi 0:88bfb16a6ad5 136 static inline void Serial2_puts(float val){
Masayoshi 0:88bfb16a6ad5 137 GenCommand(g_id2+1, val, g_cmd2); // cmd is 3byte + NULL
Masayoshi 0:88bfb16a6ad5 138 g_echoCount2 = 0;
Masayoshi 0:88bfb16a6ad5 139 g_n2 = 0;
Masayoshi 0:88bfb16a6ad5 140 p_serial2->putc(g_cmd2[0]); // async
Masayoshi 0:88bfb16a6ad5 141 //p_serial2->putc(g_cmd2[1]); // async
Masayoshi 0:88bfb16a6ad5 142 //p_serial2->putc(g_cmd2[2]); // async
Masayoshi 0:88bfb16a6ad5 143 }
Masayoshi 0:88bfb16a6ad5 144
Masayoshi 0:88bfb16a6ad5 145 static inline void Serial1_Rx(){
Masayoshi 0:88bfb16a6ad5 146 g_next++;
Masayoshi 0:88bfb16a6ad5 147 // first is echoback. second is motor response.
Masayoshi 0:88bfb16a6ad5 148 uint8_t c = p_serial1->getc();
Masayoshi 0:88bfb16a6ad5 149 if(g_echoCount1 < 3){ // echoback is 3byte
Masayoshi 0:88bfb16a6ad5 150 if(++g_echoCount1 < 3){
Masayoshi 0:88bfb16a6ad5 151 p_serial1->putc(g_cmd1[g_echoCount1]); // async
Masayoshi 0:88bfb16a6ad5 152 }
Masayoshi 0:88bfb16a6ad5 153 return;
Masayoshi 0:88bfb16a6ad5 154 }
Masayoshi 0:88bfb16a6ad5 155 /*if(g_echoCount1-- > 0){ // echoback is 3byte
Masayoshi 0:88bfb16a6ad5 156 return;
Masayoshi 0:88bfb16a6ad5 157 }*/
Masayoshi 0:88bfb16a6ad5 158
Masayoshi 0:88bfb16a6ad5 159 // motor response.
Masayoshi 0:88bfb16a6ad5 160 do {
Masayoshi 0:88bfb16a6ad5 161 g_res1[g_n1++] = c;
Masayoshi 0:88bfb16a6ad5 162 if(p_serial1->readable()) {
Masayoshi 0:88bfb16a6ad5 163 c = p_serial1->getc();
Masayoshi 0:88bfb16a6ad5 164 } else break;
Masayoshi 0:88bfb16a6ad5 165 } while(1);
Masayoshi 0:88bfb16a6ad5 166
Masayoshi 0:88bfb16a6ad5 167 if(g_n1 == 3){ // response is 3byte
Masayoshi 0:88bfb16a6ad5 168 // receive data set pre_deg
Masayoshi 0:88bfb16a6ad5 169 g_tx.d.motorData[g_id1] = ParseResponse(g_id1+1, g_res1, g_tx.d.motorData[g_id1]);
Masayoshi 0:88bfb16a6ad5 170 g_id1++; // 0-4
Masayoshi 0:88bfb16a6ad5 171
Masayoshi 0:88bfb16a6ad5 172 // last check
Masayoshi 0:88bfb16a6ad5 173 if(g_id1 == NUM_AXES1){ // motor num is 5
Masayoshi 0:88bfb16a6ad5 174 g_finish |= BIT_SERIAL1; // receive finished
Masayoshi 0:88bfb16a6ad5 175 //d20=1;d20=0;
Masayoshi 0:88bfb16a6ad5 176 // next uart start
Masayoshi 0:88bfb16a6ad5 177 g_id2 = NUM_AXES1; // 5-8
Masayoshi 0:88bfb16a6ad5 178 Serial2_puts(g_rx.d.motorData[g_id2]); // serial2 start
Masayoshi 0:88bfb16a6ad5 179 } else {
Masayoshi 0:88bfb16a6ad5 180 // next id send
Masayoshi 0:88bfb16a6ad5 181 Serial1_puts(g_rx.d.motorData[g_id1]);
Masayoshi 0:88bfb16a6ad5 182 }
Masayoshi 0:88bfb16a6ad5 183 }
Masayoshi 0:88bfb16a6ad5 184 }
Masayoshi 0:88bfb16a6ad5 185 static inline void Serial2_Rx(){
Masayoshi 0:88bfb16a6ad5 186 g_next++;
Masayoshi 0:88bfb16a6ad5 187 // first is echoback. second is motor response.
Masayoshi 0:88bfb16a6ad5 188 uint8_t c = p_serial2->getc();
Masayoshi 0:88bfb16a6ad5 189 if(g_echoCount2 < 3){ // echoback is 3byte
Masayoshi 0:88bfb16a6ad5 190 if(++g_echoCount2 < 3){
Masayoshi 0:88bfb16a6ad5 191 p_serial2->putc(g_cmd2[g_echoCount2]); // async
Masayoshi 0:88bfb16a6ad5 192 }
Masayoshi 0:88bfb16a6ad5 193 return;
Masayoshi 0:88bfb16a6ad5 194 }
Masayoshi 0:88bfb16a6ad5 195 /*if(g_echoCount2-- > 0){ // echoback is 3byte
Masayoshi 0:88bfb16a6ad5 196 return;
Masayoshi 0:88bfb16a6ad5 197 }*/
Masayoshi 0:88bfb16a6ad5 198
Masayoshi 0:88bfb16a6ad5 199 // motor response.
Masayoshi 0:88bfb16a6ad5 200 do {
Masayoshi 0:88bfb16a6ad5 201 g_res2[g_n2++] = c;
Masayoshi 0:88bfb16a6ad5 202 if(p_serial2->readable()) {
Masayoshi 0:88bfb16a6ad5 203 c = p_serial2->getc();
Masayoshi 0:88bfb16a6ad5 204 } else break;
Masayoshi 0:88bfb16a6ad5 205 } while(1);
Masayoshi 0:88bfb16a6ad5 206
Masayoshi 0:88bfb16a6ad5 207 if(g_n2 == 3){ // response is 3byte
Masayoshi 0:88bfb16a6ad5 208 // receive data set pre_deg
Masayoshi 0:88bfb16a6ad5 209 g_tx.d.motorData[g_id2] = ParseResponse(g_id2+1, g_res2, g_tx.d.motorData[g_id2]);
Masayoshi 0:88bfb16a6ad5 210 g_id2++; // 0-3
Masayoshi 0:88bfb16a6ad5 211
Masayoshi 0:88bfb16a6ad5 212 // last check
Masayoshi 0:88bfb16a6ad5 213 if(g_id2 == NUM_AXES1 + NUM_AXES2){ // motor num is 4
Masayoshi 0:88bfb16a6ad5 214 g_finish |= BIT_SERIAL2; // receive finished
Masayoshi 0:88bfb16a6ad5 215 //d20=1;d20=0;
Masayoshi 0:88bfb16a6ad5 216 } else {
Masayoshi 0:88bfb16a6ad5 217 // next id send
Masayoshi 0:88bfb16a6ad5 218 Serial2_puts(g_rx.d.motorData[g_id2]);
Masayoshi 0:88bfb16a6ad5 219 }
Masayoshi 0:88bfb16a6ad5 220 }
Masayoshi 0:88bfb16a6ad5 221 }
Masayoshi 0:88bfb16a6ad5 222
Masayoshi 0:88bfb16a6ad5 223
Masayoshi 0:88bfb16a6ad5 224
Masayoshi 0:88bfb16a6ad5 225 static inline void setupUdp(void) {
Masayoshi 0:88bfb16a6ad5 226 EthernetInterface eth;
Masayoshi 0:88bfb16a6ad5 227 //serial_usb.printf("Setup UDP\n");
Masayoshi 0:88bfb16a6ad5 228 eth.init(MY_ADDRESS, NETMASK, GATEWAY);
Masayoshi 0:88bfb16a6ad5 229
Masayoshi 0:88bfb16a6ad5 230 //serial_usb.printf("eth.init()\n");
Masayoshi 0:88bfb16a6ad5 231 int ret;
Masayoshi 0:88bfb16a6ad5 232 do {
Masayoshi 0:88bfb16a6ad5 233 ret = eth.connect(15000); // use dhcp
Masayoshi 0:88bfb16a6ad5 234 } while(ret < 0);
Masayoshi 0:88bfb16a6ad5 235 //serial_usb.printf("eth.connect()\n");
Masayoshi 0:88bfb16a6ad5 236
Masayoshi 0:88bfb16a6ad5 237 //const char *ip = eth.getIPAddress();
Masayoshi 0:88bfb16a6ad5 238 //printf("IP: %s\n", ip ? ip : "No IP");
Masayoshi 0:88bfb16a6ad5 239 r_sock.bind(PC_RX_PORT); //rx
Masayoshi 0:88bfb16a6ad5 240 s_sock.bind(PC_TX_PORT); //tx
Masayoshi 0:88bfb16a6ad5 241 //serial_usb.printf("Port: %d\n", MY_PORT);
Masayoshi 0:88bfb16a6ad5 242 }
Masayoshi 0:88bfb16a6ad5 243
Masayoshi 0:88bfb16a6ad5 244 static inline void UdpReceive(){
Masayoshi 0:88bfb16a6ad5 245 static uint16_t seq = 0;
Masayoshi 0:88bfb16a6ad5 246 //char buf[128];
Masayoshi 0:88bfb16a6ad5 247 int ret = r_sock.receiveFrom(ep_pc_rx, g_rx.buf, sizeof(g_rx.buf));
Masayoshi 0:88bfb16a6ad5 248 if(ret > 0){
Masayoshi 0:88bfb16a6ad5 249 //UDP-RECEIVE
Masayoshi 0:88bfb16a6ad5 250 d24=1;d24=0;
Masayoshi 0:88bfb16a6ad5 251
Masayoshi 0:88bfb16a6ad5 252 //__disable_irq();
Masayoshi 0:88bfb16a6ad5 253 //memcpy(g_rx.buf, buf, sizeof(g_rx.buf));
Masayoshi 0:88bfb16a6ad5 254 //__enable_irq();
Masayoshi 0:88bfb16a6ad5 255
Masayoshi 0:88bfb16a6ad5 256 if(g_rx.d.seq != seq){ // seq error
Masayoshi 0:88bfb16a6ad5 257 g_tx.d.dmy++; // seq error count
Masayoshi 0:88bfb16a6ad5 258 //ERROR
Masayoshi 0:88bfb16a6ad5 259 d22=1;d22=0;
Masayoshi 0:88bfb16a6ad5 260 }
Masayoshi 0:88bfb16a6ad5 261 if(g_rx.d.dmy == 1){ // err count clear request
Masayoshi 0:88bfb16a6ad5 262 g_rx.d.dmy = 0;
Masayoshi 0:88bfb16a6ad5 263 g_tx.d.dmy = 0; // err count clear
Masayoshi 0:88bfb16a6ad5 264 }
Masayoshi 0:88bfb16a6ad5 265 seq = g_rx.d.seq + 1; // next sequence
Masayoshi 0:88bfb16a6ad5 266 }
Masayoshi 0:88bfb16a6ad5 267 }
Masayoshi 0:88bfb16a6ad5 268
Masayoshi 0:88bfb16a6ad5 269
Masayoshi 0:88bfb16a6ad5 270 int main() {
Masayoshi 0:88bfb16a6ad5 271 float val;
Masayoshi 0:88bfb16a6ad5 272 int rate = 3; // Measuring rate -- 1:100S/s, 2:200S/s, 3:400S/s
Masayoshi 0:88bfb16a6ad5 273 int capdac[4] = {-1,-1,-1,-1}; // -1: CAPDAC disable, >=0: CAPDAC value
Masayoshi 0:88bfb16a6ad5 274 bool switchResult = true;
Masayoshi 0:88bfb16a6ad5 275
Masayoshi 0:88bfb16a6ad5 276 /*--- Initialize -------------------------------------------------------------*/
Masayoshi 0:88bfb16a6ad5 277 //ticker.attach(&tick_handler, 0.01); // 10us
Masayoshi 0:88bfb16a6ad5 278
Masayoshi 0:88bfb16a6ad5 279 #if 1
Masayoshi 0:88bfb16a6ad5 280 // udp initialized.
Masayoshi 0:88bfb16a6ad5 281 ep_pc_rx.set_address(PC_ADDRESS, PC_RX_PORT);
Masayoshi 0:88bfb16a6ad5 282 ep_pc_tx.set_address(PC_ADDRESS, PC_TX_PORT);
Masayoshi 0:88bfb16a6ad5 283 setupUdp();
Masayoshi 0:88bfb16a6ad5 284 r_sock.set_blocking(false, 0); // non-blocking
Masayoshi 0:88bfb16a6ad5 285 s_sock.set_blocking(true, 1500); // blocking
Masayoshi 0:88bfb16a6ad5 286 #endif
Masayoshi 0:88bfb16a6ad5 287 #if 1
Masayoshi 0:88bfb16a6ad5 288 // serial initialized
Masayoshi 0:88bfb16a6ad5 289 p_serial1 = new RawSerial(p13, p14); // uart1
Masayoshi 0:88bfb16a6ad5 290 p_serial1->baud(ICS_BAUDRATE);
Masayoshi 0:88bfb16a6ad5 291 p_serial1->attach(Serial1_Rx, RawSerial::RxIrq);
Masayoshi 0:88bfb16a6ad5 292 p_serial1->format(8, Serial::Even, 1);
Masayoshi 0:88bfb16a6ad5 293 p_serial2 = new RawSerial(p17, p18); // uart3
Masayoshi 0:88bfb16a6ad5 294 p_serial2->baud(ICS_BAUDRATE);
Masayoshi 0:88bfb16a6ad5 295 p_serial2->attach(Serial2_Rx, RawSerial::RxIrq);
Masayoshi 0:88bfb16a6ad5 296 p_serial2->format(8, Serial::Even, 1);
Masayoshi 0:88bfb16a6ad5 297 #endif
Masayoshi 0:88bfb16a6ad5 298 #if 1
Masayoshi 0:88bfb16a6ad5 299 // i2c initialized
Masayoshi 0:88bfb16a6ad5 300 p_i2c1 = new I2C(p9, p10); // i2c1 sda,scl
Masayoshi 0:88bfb16a6ad5 301 p_i2c1->frequency(400000);
Masayoshi 0:88bfb16a6ad5 302 p_i2c2 = new I2C(p28, p27); // i2c2 sda,scl
Masayoshi 0:88bfb16a6ad5 303 p_i2c2->frequency(400000);
Masayoshi 0:88bfb16a6ad5 304 #endif
Masayoshi 0:88bfb16a6ad5 305 #if 1
Masayoshi 0:88bfb16a6ad5 306 // Initialize I2C Interface
Masayoshi 0:88bfb16a6ad5 307 do{
Masayoshi 0:88bfb16a6ad5 308 for(int i=0; i < FDC_NUM; i++) {
Masayoshi 0:88bfb16a6ad5 309 I2C *i2c = (i == 2)? p_i2c2 : p_i2c1;
Masayoshi 0:88bfb16a6ad5 310
Masayoshi 0:88bfb16a6ad5 311 // MULTI PLEXER
Masayoshi 0:88bfb16a6ad5 312 if(i != 2){
Masayoshi 0:88bfb16a6ad5 313 switchResult = switchI2c(i2c, i); // MUXのch0にI2Cを切替
Masayoshi 0:88bfb16a6ad5 314 if(!switchResult) break;
Masayoshi 0:88bfb16a6ad5 315 }
Masayoshi 0:88bfb16a6ad5 316
Masayoshi 0:88bfb16a6ad5 317 if (initFdc(i2c)) {
Masayoshi 0:88bfb16a6ad5 318 config(i2c, rate, capdac);
Masayoshi 0:88bfb16a6ad5 319 //pc.printf("ch %d_FDC1004 is ready.\r\n", i);
Masayoshi 0:88bfb16a6ad5 320 } else {
Masayoshi 0:88bfb16a6ad5 321 //pc.printf("ch %d_FDC1004 is not ready. Please check and restart.\r\n", i);
Masayoshi 0:88bfb16a6ad5 322 switchResult = false;
Masayoshi 0:88bfb16a6ad5 323 wait(1.0);
Masayoshi 0:88bfb16a6ad5 324 }
Masayoshi 0:88bfb16a6ad5 325 } // end_for
Masayoshi 0:88bfb16a6ad5 326 }while(!switchResult);
Masayoshi 0:88bfb16a6ad5 327 #endif
Masayoshi 0:88bfb16a6ad5 328
Masayoshi 0:88bfb16a6ad5 329 // debug port
Masayoshi 0:88bfb16a6ad5 330 d26 = 0;
Masayoshi 0:88bfb16a6ad5 331 d25 = 0;
Masayoshi 0:88bfb16a6ad5 332 d24 = 0;
Masayoshi 0:88bfb16a6ad5 333 d23 = 0;
Masayoshi 0:88bfb16a6ad5 334 d22 = 0;
Masayoshi 0:88bfb16a6ad5 335 d21 = 0;
Masayoshi 0:88bfb16a6ad5 336 d20 = 0;
Masayoshi 0:88bfb16a6ad5 337 d19 = 0;
Masayoshi 0:88bfb16a6ad5 338
Masayoshi 0:88bfb16a6ad5 339 g_finish = 0;
Masayoshi 0:88bfb16a6ad5 340 uint16_t seq = 0;
Masayoshi 0:88bfb16a6ad5 341
Masayoshi 0:88bfb16a6ad5 342 /*--- MAIN LOOP --------------------------------------------------------------*/
Masayoshi 0:88bfb16a6ad5 343 while(1) {
Masayoshi 0:88bfb16a6ad5 344 d19=!d19; //main-loop
Masayoshi 0:88bfb16a6ad5 345
Masayoshi 0:88bfb16a6ad5 346 /*--- UDP RECEIVE (NON-BLOCKING) ---------------------------------------------*/
Masayoshi 0:88bfb16a6ad5 347 #if 1
Masayoshi 0:88bfb16a6ad5 348 d21=1;
Masayoshi 0:88bfb16a6ad5 349 UdpReceive();
Masayoshi 0:88bfb16a6ad5 350 d21=0;
Masayoshi 0:88bfb16a6ad5 351 #endif
Masayoshi 0:88bfb16a6ad5 352
Masayoshi 0:88bfb16a6ad5 353 /*--- SERIAL -----------------------------------------------------------------*/
Masayoshi 0:88bfb16a6ad5 354 #if 1
Masayoshi 0:88bfb16a6ad5 355 g_next = 0;
Masayoshi 0:88bfb16a6ad5 356 g_id1 = 0; //0-4
Masayoshi 0:88bfb16a6ad5 357 Serial1_puts(g_rx.d.motorData[g_id1]); // serial1 start
Masayoshi 0:88bfb16a6ad5 358 #endif
Masayoshi 0:88bfb16a6ad5 359 /*--- I2C --------------------------------------------------------------------*/
Masayoshi 0:88bfb16a6ad5 360 #if 1
Masayoshi 0:88bfb16a6ad5 361 // Acquire sensor values
Masayoshi 0:88bfb16a6ad5 362 do{
Masayoshi 0:88bfb16a6ad5 363 for (int j=0; j<FDC_NUM ;j++) {
Masayoshi 0:88bfb16a6ad5 364 I2C *i2c = (j == 2)? p_i2c2 : p_i2c1;
Masayoshi 0:88bfb16a6ad5 365 // MULTI PLEXER
Masayoshi 0:88bfb16a6ad5 366 if(j != 2){
Masayoshi 0:88bfb16a6ad5 367 switchResult = switchI2c(i2c, j);
Masayoshi 0:88bfb16a6ad5 368 if(!switchResult) break;
Masayoshi 0:88bfb16a6ad5 369 }
Masayoshi 0:88bfb16a6ad5 370 senseCapacitance(i2c, g_tx.d.sensorData[j], capdac);
Masayoshi 0:88bfb16a6ad5 371 }
Masayoshi 0:88bfb16a6ad5 372 }while(!switchResult);
Masayoshi 0:88bfb16a6ad5 373 //d20=1;d20=0;
Masayoshi 0:88bfb16a6ad5 374 //g_finish |= BIT_I2C1;
Masayoshi 0:88bfb16a6ad5 375 //g_finish |= BIT_I2C2;
Masayoshi 0:88bfb16a6ad5 376 #endif
Masayoshi 0:88bfb16a6ad5 377 /*--- UDP SEND (BLOCKING) ----------------------------------------------------*/
Masayoshi 0:88bfb16a6ad5 378 #if 1
Masayoshi 0:88bfb16a6ad5 379 if((g_finish & 0x03) == 0x03){ // all data gathered
Masayoshi 0:88bfb16a6ad5 380 L001:
Masayoshi 0:88bfb16a6ad5 381 d20=1;
Masayoshi 0:88bfb16a6ad5 382 g_tx.d.seq = seq++;
Masayoshi 0:88bfb16a6ad5 383 int ret = s_sock.sendTo(ep_pc_tx, g_tx.buf, sizeof(g_tx.buf));
Masayoshi 0:88bfb16a6ad5 384 d20=0;
Masayoshi 0:88bfb16a6ad5 385 if(ret != sizeof(g_tx.buf)){ // send error
Masayoshi 0:88bfb16a6ad5 386 //SEND-ERROR
Masayoshi 0:88bfb16a6ad5 387 d22=1;d22=0;
Masayoshi 0:88bfb16a6ad5 388 }
Masayoshi 0:88bfb16a6ad5 389 } else {
Masayoshi 0:88bfb16a6ad5 390 //GATHER-ERROR
Masayoshi 0:88bfb16a6ad5 391 led1=!led1;
Masayoshi 0:88bfb16a6ad5 392 //memset(&g_tx.d.motorData, 0, sizeof(g_tx.d.motorData)); // motor not needed
Masayoshi 0:88bfb16a6ad5 393 memset(&g_tx.d.sensorData, 0, sizeof(g_tx.d.sensorData)); // sensor is ZERO.
Masayoshi 0:88bfb16a6ad5 394 goto L001;
Masayoshi 0:88bfb16a6ad5 395 }
Masayoshi 0:88bfb16a6ad5 396 g_finish = 0;
Masayoshi 0:88bfb16a6ad5 397 #endif
Masayoshi 0:88bfb16a6ad5 398
Masayoshi 0:88bfb16a6ad5 399 } // end_while(1)
Masayoshi 0:88bfb16a6ad5 400 }
Masayoshi 0:88bfb16a6ad5 401