Diff: main.cpp

Revision:

1:ea4150fafa3b

Parent:

0:62729afe5606

Child:

2:af928306ca36

diff -r 62729afe5606 -r ea4150fafa3b main.cpp
--- a/main.cpp	Thu Aug 10 09:07:54 2017 +0000
+++ b/main.cpp	Mon Aug 14 09:12:34 2017 +0000
@@ -1,13 +1,22 @@
 /*
  * rosserial Publisher for encoder
  * Publish: distance(cm)
+ * Subscribe: --once
  */
 
 #include"mbed.h"
 #include <ros.h>
 #include <std_msgs/Float32.h>
+#include <std_msgs/Empty.h>
 #include "QEI.h"
 
+using namespace std_msgs;
+using namespace ros;
+
+DigitalOut myled(LED1);
+
+#define BAUD 115200
+
 // GYO
 Serial gy25(PA_9, PA_10, 115200);
 float g[3];
@@ -47,28 +56,14 @@
 ros::NodeHandle  nh;
 
 // instantiate the publisher
-std_msgs::Float32 X1, X2, Y1, Y2, G1, G2, G3;
-ros::Publisher pub_X1("X1", &X1);
-ros::Publisher pub_X2("X2", &X2);
-ros::Publisher pub_Y1("Y1", &Y1);
-ros::Publisher pub_Y2("Y2", &Y2);
-ros::Publisher pub_G1("G1", &G1);
-ros::Publisher pub_G2("G2", &G2);
-ros::Publisher pub_G3("G3", &G3);
-
-// initializing node
-void nodeInit() {
-    nh.getHardware()->setBaud(115200);
-    nh.initNode();
-    
-    nh.advertise(pub_X1);
-    nh.advertise(pub_X2);
-    nh.advertise(pub_Y1);
-    nh.advertise(pub_Y2);
-    nh.advertise(pub_G1);
-    nh.advertise(pub_G3);
-    nh.advertise(pub_G2);
-}
+Float32 X1, X2, Y1, Y2, G1, G2, G3;
+Publisher pub_X1("X1", &X1);
+Publisher pub_X2("X2", &X2);
+Publisher pub_Y1("Y1", &Y1);
+Publisher pub_Y2("Y2", &Y2);
+Publisher pub_G1("G1", &G1);
+Publisher pub_G2("G2", &G2);
+Publisher pub_G3("G3", &G3);
 
 // BUTTON for reset
 InterruptIn button(USER_BUTTON);
@@ -84,6 +79,30 @@
     En_Y2.reset();
 }
 
+// for reset
+void messageCb(const Empty& toggle_msg){
+    pressed();
+    myled = !myled;   // blink the led
+}
+
+Subscriber<Empty> sub("toggle_led", &messageCb);
+
+// initializing node
+void nodeInit() {
+    nh.getHardware()->setBaud(BAUD);
+    nh.initNode();
+    
+    nh.subscribe(sub);
+    
+    nh.advertise(pub_X1);
+    nh.advertise(pub_X2);
+    nh.advertise(pub_Y1);
+    nh.advertise(pub_Y2);
+    nh.advertise(pub_G1);
+    nh.advertise(pub_G3);
+    nh.advertise(pub_G2);
+}
+
 int main() {
     nodeInit();