UDP ROS Communication program

Revision:
0:c231ce428ec0
Child:
1:ed2c64c754f1
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 02 01:55:28 2021 +0900
@@ -0,0 +1,77 @@
+#include "ATHandler.h"
+#include "EthernetInterface.h"
+#include "SocketAddress.h"
+#include "UDPSocket.h"
+#include "mbed.h"
+#include "nsapi_types.h"
+
+//イーサネット関連
+const char *myIP = "192.168.0.13";
+const char *mask = "255.255.255.0";
+const char *pcIP = "192.168.0.10";
+const int myPORT = 8080, pcPORT = 8080;
+const int PORT = 8080;
+EthernetInterface eth;
+UDPSocket socket;
+SocketAddress sockAddr;
+SocketAddress myAddr(myIP, myPORT);
+SocketAddress pcAddr(pcIP, pcPORT);
+#define MAX_LEN 1208
+char txdata[MAX_LEN];
+int network_init() {
+  // network ip set
+  eth.set_network(myIP, mask, "");
+  // connection test (return 0 @status is OK)
+  if (eth.connect() != 0) { // occor error
+    printf("eth connetction error\n");
+    return -1;
+  }
+
+  socket.open(&eth);
+  return true;
+}
+
+//スレッド管理
+Thread control_thread(osPriorityRealtime);
+#define CONTROL_THREAD_dt 1ms
+Thread communication_thread(osPriorityHigh);
+#define COMMUNICATION_THREAD_dt 1ms
+// Thread main_thread(osPriorityNormal);
+#define NORMAL_THREAD_dt 2000ms
+
+//表示LED
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+
+//制御ループ
+void control() {
+  while (1) {
+    led2 = !led2;
+    ThisThread::sleep_for(CONTROL_THREAD_dt);
+  }
+}
+
+//通信ループ
+void communication() {
+  while (1) {
+    led3 = !led3;
+
+    if (0 > socket.sendto(pcAddr, txdata, sizeof(txdata))) {
+      printf("eth sending error\n");
+    }
+    // printf("sended\n");
+    ThisThread::sleep_for(COMMUNICATION_THREAD_dt);
+  }
+}
+
+//その他ループ
+int main() {
+  network_init();
+  control_thread.start(control);
+  communication_thread.start(communication);
+  while (1) { // normal loop
+    led1 = !led1;
+    ThisThread::sleep_for(NORMAL_THREAD_dt);
+  }
+}
\ No newline at end of file