UDP ROS Communication program

main.cpp

Committer:
daikinect
Date:
2021-03-02
Branch:
test
Revision:
5:2b2cf11243b1
Parent:
4:4b38a500167b

File content as of revision 5:2b2cf11243b1:

#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;
EthernetInterface eth;
UDPSocket socket;
SocketAddress myAddr(myIP, myPORT);
SocketAddress pcAddr(pcIP, pcPORT);
#define MAX_LEN 1208
char txdata[MAX_LEN];
char rxdata[MAX_LEN];
long counter = 0;

void initialize_txdata() { // 0123...'A''B''C'...
  for (int i = 0; i < MAX_LEN; i++)
    txdata[i] = (i + counter) % 0x100;
  counter++;
}

int network_init() {
  initialize_txdata();
  // 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;
    initialize_txdata();
    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);
  }
}