Daiki Morita / Mbed OS UDP_ROS

UDP ROS Communication program

main.cpp@5:2b2cf11243b1, 2021-03-02 (annotated)

Committer:
daikinect
Date:
Tue Mar 02 02:22:39 2021 +0900
Branch:
test
Revision:
5:2b2cf11243b1
Parent:
4:4b38a500167b 
date change test

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 5:2b2cf11243b1 16 long counter = 0;
daikinect 4:4b38a500167b 17
daikinect 4:4b38a500167b 18 void initialize_txdata() { // 0123...'A''B''C'...
daikinect 4:4b38a500167b 19 for (int i = 0; i < MAX_LEN; i++)
daikinect 5:2b2cf11243b1 20 txdata[i] = (i + counter) % 0x100;
daikinect 5:2b2cf11243b1 21 counter++;
daikinect 4:4b38a500167b 22 }
daikinect 4:4b38a500167b 23
daikinect 0:c231ce428ec0 24 int network_init() {
daikinect 4:4b38a500167b 25 initialize_txdata();
daikinect 0:c231ce428ec0 26 // network ip set
daikinect 0:c231ce428ec0 27 eth.set_network(myIP, mask, "");
daikinect 0:c231ce428ec0 28 // connection test (return 0 @status is OK)
daikinect 0:c231ce428ec0 29 if (eth.connect() != 0) { // occor error
daikinect 0:c231ce428ec0 30 printf("eth connetction error\n");
daikinect 0:c231ce428ec0 31 return -1;
daikinect 0:c231ce428ec0 32 }
daikinect 0:c231ce428ec0 33 socket.open(&eth);
daikinect 0:c231ce428ec0 34 return true;
daikinect 0:c231ce428ec0 35 }
daikinect 0:c231ce428ec0 36
daikinect 0:c231ce428ec0 37 //スレッド管理
daikinect 0:c231ce428ec0 38 Thread control_thread(osPriorityRealtime);
daikinect 0:c231ce428ec0 39 #define CONTROL_THREAD_dt 1ms
daikinect 0:c231ce428ec0 40 Thread communication_thread(osPriorityHigh);
daikinect 0:c231ce428ec0 41 #define COMMUNICATION_THREAD_dt 1ms
daikinect 0:c231ce428ec0 42 // Thread main_thread(osPriorityNormal);
daikinect 0:c231ce428ec0 43 #define NORMAL_THREAD_dt 2000ms
daikinect 0:c231ce428ec0 44
daikinect 0:c231ce428ec0 45 //表示LED
daikinect 0:c231ce428ec0 46 DigitalOut led1(LED1);
daikinect 0:c231ce428ec0 47 DigitalOut led2(LED2);
daikinect 0:c231ce428ec0 48 DigitalOut led3(LED3);
daikinect 0:c231ce428ec0 49
daikinect 0:c231ce428ec0 50 //制御ループ
daikinect 0:c231ce428ec0 51 void control() {
daikinect 0:c231ce428ec0 52 while (1) {
daikinect 0:c231ce428ec0 53 led2 = !led2;
daikinect 0:c231ce428ec0 54 ThisThread::sleep_for(CONTROL_THREAD_dt);
daikinect 0:c231ce428ec0 55 }
daikinect 0:c231ce428ec0 56 }
daikinect 0:c231ce428ec0 57
daikinect 0:c231ce428ec0 58 //通信ループ
daikinect 0:c231ce428ec0 59 void communication() {
daikinect 0:c231ce428ec0 60 while (1) {
daikinect 0:c231ce428ec0 61 led3 = !led3;
daikinect 5:2b2cf11243b1 62 initialize_txdata();
daikinect 0:c231ce428ec0 63 if (0 > socket.sendto(pcAddr, txdata, sizeof(txdata))) {
daikinect 0:c231ce428ec0 64 printf("eth sending error\n");
daikinect 0:c231ce428ec0 65 }
daikinect 0:c231ce428ec0 66 // printf("sended\n");
daikinect 0:c231ce428ec0 67 ThisThread::sleep_for(COMMUNICATION_THREAD_dt);
daikinect 0:c231ce428ec0 68 }
daikinect 0:c231ce428ec0 69 }
daikinect 0:c231ce428ec0 70
daikinect 0:c231ce428ec0 71 //その他ループ
daikinect 0:c231ce428ec0 72 int main() {
daikinect 0:c231ce428ec0 73 network_init();
daikinect 0:c231ce428ec0 74 control_thread.start(control);
daikinect 0:c231ce428ec0 75 communication_thread.start(communication);
daikinect 0:c231ce428ec0 76 while (1) { // normal loop
daikinect 0:c231ce428ec0 77 led1 = !led1;
daikinect 0:c231ce428ec0 78 ThisThread::sleep_for(NORMAL_THREAD_dt);
daikinect 0:c231ce428ec0 79 }
daikinect 0:c231ce428ec0 80 }