Diff: main.cpp

Revision:

0:230d6c5f4ad7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 19 11:18:25 2019 +0000
@@ -0,0 +1,67 @@
+#include "mbed.h"
+#include <ros.h>
+#include <std_msgs/Int32MultiArray.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
+
+#define DATA_NUM 3
+
+ros::NodeHandle nh;
+
+Timeout toff;
+int recieve_data[3];
+bool cb_flg = false;
+
+Serial micon(D1,D0);
+
+std_msgs::Int32MultiArray send_data;
+std_msgs::String dbg_msg;
+ros::Publisher chatter("/Chatter", &send_data);
+ros::Publisher debuger("/debuger", &dbg_msg);
+
+void messageCb(const std_msgs::Int32MultiArray& msg){
+    int i;
+    for(i=0;i<DATA_NUM;i++){
+        recieve_data[i] = msg.data[i];
+    }
+    for(i=0;i<DATA_NUM;i++){
+        if(i+1<DATA_NUM) send_data.data[i] = recieve_data[i+1]; 
+        else send_data.data[i] = recieve_data[0];
+    }
+    chatter.publish(&send_data);
+    cb_flg = true;
+}
+
+ros::Subscriber<std_msgs::Int32MultiArray> sub("/data", &messageCb);
+        
+int main(){
+    micon.baud(115200);
+    micon.format(8,Serial::None,1);
+    send_data.data_length = DATA_NUM;
+    send_data.data = (int32_t *)malloc(sizeof(int32_t)*DATA_NUM);
+    int i,j;
+    for(i=0;i<DATA_NUM;i++){
+        send_data.data[i] = 0;
+    }
+    nh.getHardware()->setBaud(115200);
+    nh.initNode();
+    nh.subscribe(sub);
+    nh.advertise(debuger);
+    nh.advertise(chatter);
+
+    while(1){
+        dbg_msg.data = "success";
+        debuger.publish(&dbg_msg);
+        if(cb_flg==true){
+            for(j=0;j<DATA_NUM;j++){
+                for(i=0;i<4;i++){
+                    micon.putc((char)(send_data.data[j]>>(i*8) & 0xff));
+                    wait_ms(1);
+                }
+            }
+            cb_flg = false;
+        }
+        nh.spinOnce();
+        wait_ms(100);
+    }
+}
\ No newline at end of file