![](/media/cache/profiles/cc38c8ffd23cef2e3e5c21d20c1db636.jpg.50x50_q85.png)
UDP ROS Communication program
main.cpp@4:4b38a500167b, 2021-03-02 (annotated)
- Committer:
- daikinect
- Date:
- Tue Mar 02 02:15:24 2021 +0900
- Branch:
- test
- Revision:
- 4:4b38a500167b
- Parent:
- 2:ad02959c2003
- Child:
- 5:2b2cf11243b1
????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
daikinect | 2:ad02959c2003 | 1 | #include "EthernetInterface.h" |
daikinect | 0:c231ce428ec0 | 2 | #include "mbed.h" |
daikinect | 4:4b38a500167b | 3 | |
daikinect | 0:c231ce428ec0 | 4 | //イーサネット関連 |
daikinect | 0:c231ce428ec0 | 5 | const char *myIP = "192.168.0.13"; |
daikinect | 0:c231ce428ec0 | 6 | const char *mask = "255.255.255.0"; |
daikinect | 0:c231ce428ec0 | 7 | const char *pcIP = "192.168.0.10"; |
daikinect | 0:c231ce428ec0 | 8 | const int myPORT = 8080, pcPORT = 8080; |
daikinect | 0:c231ce428ec0 | 9 | EthernetInterface eth; |
daikinect | 0:c231ce428ec0 | 10 | UDPSocket socket; |
daikinect | 0:c231ce428ec0 | 11 | SocketAddress myAddr(myIP, myPORT); |
daikinect | 0:c231ce428ec0 | 12 | SocketAddress pcAddr(pcIP, pcPORT); |
daikinect | 0:c231ce428ec0 | 13 | #define MAX_LEN 1208 |
daikinect | 0:c231ce428ec0 | 14 | char txdata[MAX_LEN]; |
daikinect | 4:4b38a500167b | 15 | char rxdata[MAX_LEN]; |
daikinect | 4:4b38a500167b | 16 | |
daikinect | 4:4b38a500167b | 17 | void initialize_txdata() { // 0123...'A''B''C'... |
daikinect | 4:4b38a500167b | 18 | for (int i = 0; i < MAX_LEN; i++) |
daikinect | 4:4b38a500167b | 19 | txdata[i] = i % 0x100; |
daikinect | 4:4b38a500167b | 20 | } |
daikinect | 4:4b38a500167b | 21 | |
daikinect | 0:c231ce428ec0 | 22 | int network_init() { |
daikinect | 4:4b38a500167b | 23 | initialize_txdata(); |
daikinect | 0:c231ce428ec0 | 24 | // network ip set |
daikinect | 0:c231ce428ec0 | 25 | eth.set_network(myIP, mask, ""); |
daikinect | 0:c231ce428ec0 | 26 | // connection test (return 0 @status is OK) |
daikinect | 0:c231ce428ec0 | 27 | if (eth.connect() != 0) { // occor error |
daikinect | 0:c231ce428ec0 | 28 | printf("eth connetction error\n"); |
daikinect | 0:c231ce428ec0 | 29 | return -1; |
daikinect | 0:c231ce428ec0 | 30 | } |
daikinect | 0:c231ce428ec0 | 31 | socket.open(ð); |
daikinect | 0:c231ce428ec0 | 32 | return true; |
daikinect | 0:c231ce428ec0 | 33 | } |
daikinect | 0:c231ce428ec0 | 34 | |
daikinect | 0:c231ce428ec0 | 35 | //スレッド管理 |
daikinect | 0:c231ce428ec0 | 36 | Thread control_thread(osPriorityRealtime); |
daikinect | 0:c231ce428ec0 | 37 | #define CONTROL_THREAD_dt 1ms |
daikinect | 0:c231ce428ec0 | 38 | Thread communication_thread(osPriorityHigh); |
daikinect | 0:c231ce428ec0 | 39 | #define COMMUNICATION_THREAD_dt 1ms |
daikinect | 0:c231ce428ec0 | 40 | // Thread main_thread(osPriorityNormal); |
daikinect | 0:c231ce428ec0 | 41 | #define NORMAL_THREAD_dt 2000ms |
daikinect | 0:c231ce428ec0 | 42 | |
daikinect | 0:c231ce428ec0 | 43 | //表示LED |
daikinect | 0:c231ce428ec0 | 44 | DigitalOut led1(LED1); |
daikinect | 0:c231ce428ec0 | 45 | DigitalOut led2(LED2); |
daikinect | 0:c231ce428ec0 | 46 | DigitalOut led3(LED3); |
daikinect | 0:c231ce428ec0 | 47 | |
daikinect | 0:c231ce428ec0 | 48 | //制御ループ |
daikinect | 0:c231ce428ec0 | 49 | void control() { |
daikinect | 0:c231ce428ec0 | 50 | while (1) { |
daikinect | 0:c231ce428ec0 | 51 | led2 = !led2; |
daikinect | 0:c231ce428ec0 | 52 | ThisThread::sleep_for(CONTROL_THREAD_dt); |
daikinect | 0:c231ce428ec0 | 53 | } |
daikinect | 0:c231ce428ec0 | 54 | } |
daikinect | 0:c231ce428ec0 | 55 | |
daikinect | 0:c231ce428ec0 | 56 | //通信ループ |
daikinect | 0:c231ce428ec0 | 57 | void communication() { |
daikinect | 0:c231ce428ec0 | 58 | while (1) { |
daikinect | 0:c231ce428ec0 | 59 | led3 = !led3; |
daikinect | 0:c231ce428ec0 | 60 | |
daikinect | 0:c231ce428ec0 | 61 | if (0 > socket.sendto(pcAddr, txdata, sizeof(txdata))) { |
daikinect | 0:c231ce428ec0 | 62 | printf("eth sending error\n"); |
daikinect | 0:c231ce428ec0 | 63 | } |
daikinect | 0:c231ce428ec0 | 64 | // printf("sended\n"); |
daikinect | 0:c231ce428ec0 | 65 | ThisThread::sleep_for(COMMUNICATION_THREAD_dt); |
daikinect | 0:c231ce428ec0 | 66 | } |
daikinect | 0:c231ce428ec0 | 67 | } |
daikinect | 0:c231ce428ec0 | 68 | |
daikinect | 0:c231ce428ec0 | 69 | //その他ループ |
daikinect | 0:c231ce428ec0 | 70 | int main() { |
daikinect | 0:c231ce428ec0 | 71 | network_init(); |
daikinect | 0:c231ce428ec0 | 72 | control_thread.start(control); |
daikinect | 0:c231ce428ec0 | 73 | communication_thread.start(communication); |
daikinect | 0:c231ce428ec0 | 74 | while (1) { // normal loop |
daikinect | 0:c231ce428ec0 | 75 | led1 = !led1; |
daikinect | 0:c231ce428ec0 | 76 | ThisThread::sleep_for(NORMAL_THREAD_dt); |
daikinect | 0:c231ce428ec0 | 77 | } |
daikinect | 0:c231ce428ec0 | 78 | } |