Daiki Morita / Mbed OS UDP_ROS

UDP ROS Communication program

main.cpp@0:c231ce428ec0, 2021-03-02 (annotated)

Committer:
daikinect
Date:
Tue Mar 02 01:55:28 2021 +0900
Revision:
0:c231ce428ec0
Child:
1:ed2c64c754f1
init

Who changed what in which revision?

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