Diff: main.cpp

Revision:

7:13dd93a0efe8

Parent:

5:e6f9768e1438

Child:

8:efe5a5b1f10a

--- a/main.cpp	Thu Jun 27 04:08:38 2019 +0000
+++ b/main.cpp	Thu Jun 27 04:58:29 2019 +0000
@@ -1,10 +1,25 @@
 #include "mbed.h"
 #include "motor.h"
+#include <ros.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Int32.h>
+
+std_msgs::Float64 cum_dist;
+std_msgs::Float32 rela_dist;
+std_msgs::Int32 curr_rpm;
+
+
+ros::NodeHandle nh ;
+ros::Publisher cum_dist_pub("cum_dist", &cum_dist);
+ros::Publisher rela_dist_pub("rela_dist", &rela_dist);
+ros::Publisher rpm_pub("rpm", &curr_rpm);
 
 MotorCtl motor1(D3,D12,D4,D5);
 InterruptIn tachoINT1(D4);
 InterruptIn tachoINT2(D5);
 Serial pc(USBTX,USBRX,115200);
+
 char rx_buffer[10];
 int index=0;
 volatile int flag;
@@ -45,6 +60,11 @@
 
 int main()
 {
+    nh.initNode();
+    nh.advertise(cum_dist_pub);
+    nh.advertise(rela_dist_pub);
+    nh.advertise(rpm_pub);
+    
     tachoINT1.fall(&update_current);
     tachoINT1.rise(&update_current);
     tachoINT2.fall(&update_current);
@@ -62,9 +82,17 @@
             rpm = motor1.getRPM();
             cumdistance = motor1.CalculateCumDis(); //  누적거리
             reladistance = motor1.CalculateRelaDis(); // 상대거리
-
-
-            printf("Rpm: %f\r\n",distance);
+    
+            cum_dist.data = cumdistance;
+            cum_dist_pub.publish(&cum_dist);
+            
+            rela_dist.data = reladistance;
+            rela_dist_pub.publish(&rela_dist);
+            
+            curr_rpm.data = rpm;
+            rpm_pub.publish(&curr_rpm);            
+            
+            printf("Rpm: %f\r\n",rpm);
             printf("cumdistance: %f\r\n",cumdistance);
             printf("reladistance: %f\r\n",reladistance);
             wait(1);