UDP ROS Communication program

main.cpp

Committer:
daikinect
Date:
2021-03-02
Revision:
2:ad02959c2003
Parent:
1:ed2c64c754f1
Child:
4:4b38a500167b

File content as of revision 2:ad02959c2003:

#include "EthernetInterface.h"
#include "mbed.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);
  }
}