Daiki Morita / Mbed OS UDP_ROS

UDP ROS Communication program

main.cpp@4:4b38a500167b, 2021-03-02 (annotated)

Committer:
daikinect
Date:
Tue Mar 02 02:15:24 2021 +0900
Branch:
test
Revision:
4:4b38a500167b
Parent:
2:ad02959c2003
Child:
5:2b2cf11243b1
????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daikinect 2:ad02959c2003 1 #include "EthernetInterface.h"
daikinect 0:c231ce428ec0 2 #include "mbed.h"
daikinect 4:4b38a500167b 3
daikinect 0:c231ce428ec0 4 //イーサネット関連
daikinect 0:c231ce428ec0 5 const char *myIP = "192.168.0.13";
daikinect 0:c231ce428ec0 6 const char *mask = "255.255.255.0";
daikinect 0:c231ce428ec0 7 const char *pcIP = "192.168.0.10";
daikinect 0:c231ce428ec0 8 const int myPORT = 8080, pcPORT = 8080;
daikinect 0:c231ce428ec0 9 EthernetInterface eth;
daikinect 0:c231ce428ec0 10 UDPSocket socket;
daikinect 0:c231ce428ec0 11 SocketAddress myAddr(myIP, myPORT);
daikinect 0:c231ce428ec0 12 SocketAddress pcAddr(pcIP, pcPORT);
daikinect 0:c231ce428ec0 13 #define MAX_LEN 1208
daikinect 0:c231ce428ec0 14 char txdata[MAX_LEN];
daikinect 4:4b38a500167b 15 char rxdata[MAX_LEN];
daikinect 4:4b38a500167b 16
daikinect 4:4b38a500167b 17 void initialize_txdata() { // 0123...'A''B''C'...
daikinect 4:4b38a500167b 18 for (int i = 0; i < MAX_LEN; i++)
daikinect 4:4b38a500167b 19 txdata[i] = i % 0x100;
daikinect 4:4b38a500167b 20 }
daikinect 4:4b38a500167b 21
daikinect 0:c231ce428ec0 22 int network_init() {
daikinect 4:4b38a500167b 23 initialize_txdata();
daikinect 0:c231ce428ec0 24 // network ip set
daikinect 0:c231ce428ec0 25 eth.set_network(myIP, mask, "");
daikinect 0:c231ce428ec0 26 // connection test (return 0 @status is OK)
daikinect 0:c231ce428ec0 27 if (eth.connect() != 0) { // occor error
daikinect 0:c231ce428ec0 28 printf("eth connetction error\n");
daikinect 0:c231ce428ec0 29 return -1;
daikinect 0:c231ce428ec0 30 }
daikinect 0:c231ce428ec0 31 socket.open(&eth);
daikinect 0:c231ce428ec0 32 return true;
daikinect 0:c231ce428ec0 33 }
daikinect 0:c231ce428ec0 34
daikinect 0:c231ce428ec0 35 //スレッド管理
daikinect 0:c231ce428ec0 36 Thread control_thread(osPriorityRealtime);
daikinect 0:c231ce428ec0 37 #define CONTROL_THREAD_dt 1ms
daikinect 0:c231ce428ec0 38 Thread communication_thread(osPriorityHigh);
daikinect 0:c231ce428ec0 39 #define COMMUNICATION_THREAD_dt 1ms
daikinect 0:c231ce428ec0 40 // Thread main_thread(osPriorityNormal);
daikinect 0:c231ce428ec0 41 #define NORMAL_THREAD_dt 2000ms
daikinect 0:c231ce428ec0 42
daikinect 0:c231ce428ec0 43 //表示LED
daikinect 0:c231ce428ec0 44 DigitalOut led1(LED1);
daikinect 0:c231ce428ec0 45 DigitalOut led2(LED2);
daikinect 0:c231ce428ec0 46 DigitalOut led3(LED3);
daikinect 0:c231ce428ec0 47
daikinect 0:c231ce428ec0 48 //制御ループ
daikinect 0:c231ce428ec0 49 void control() {
daikinect 0:c231ce428ec0 50 while (1) {
daikinect 0:c231ce428ec0 51 led2 = !led2;
daikinect 0:c231ce428ec0 52 ThisThread::sleep_for(CONTROL_THREAD_dt);
daikinect 0:c231ce428ec0 53 }
daikinect 0:c231ce428ec0 54 }
daikinect 0:c231ce428ec0 55
daikinect 0:c231ce428ec0 56 //通信ループ
daikinect 0:c231ce428ec0 57 void communication() {
daikinect 0:c231ce428ec0 58 while (1) {
daikinect 0:c231ce428ec0 59 led3 = !led3;
daikinect 0:c231ce428ec0 60
daikinect 0:c231ce428ec0 61 if (0 > socket.sendto(pcAddr, txdata, sizeof(txdata))) {
daikinect 0:c231ce428ec0 62 printf("eth sending error\n");
daikinect 0:c231ce428ec0 63 }
daikinect 0:c231ce428ec0 64 // printf("sended\n");
daikinect 0:c231ce428ec0 65 ThisThread::sleep_for(COMMUNICATION_THREAD_dt);
daikinect 0:c231ce428ec0 66 }
daikinect 0:c231ce428ec0 67 }
daikinect 0:c231ce428ec0 68
daikinect 0:c231ce428ec0 69 //その他ループ
daikinect 0:c231ce428ec0 70 int main() {
daikinect 0:c231ce428ec0 71 network_init();
daikinect 0:c231ce428ec0 72 control_thread.start(control);
daikinect 0:c231ce428ec0 73 communication_thread.start(communication);
daikinect 0:c231ce428ec0 74 while (1) { // normal loop
daikinect 0:c231ce428ec0 75 led1 = !led1;
daikinect 0:c231ce428ec0 76 ThisThread::sleep_for(NORMAL_THREAD_dt);
daikinect 0:c231ce428ec0 77 }
daikinect 0:c231ce428ec0 78 }