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