Daiki Morita / Mbed OS UDP_ROS

UDP ROS Communication program

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

Committer:
daikinect
Date:
Tue Mar 02 02:06:00 2021 +0900
Revision:
2:ad02959c2003
Parent:
1:ed2c64c754f1
Child:
4:4b38a500167b
fix inclde file name

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