Fully integrated ToF/IMU codes

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Revision:
5:90bf5f0d86e9
Parent:
0:7e6b349182bc
Child:
6:e9b1684a9c00
diff -r 7f22873e729b -r 90bf5f0d86e9 main.cpp
--- a/main.cpp	Mon Aug 13 18:23:02 2018 +0000
+++ b/main.cpp	Mon Aug 13 21:54:37 2018 +0000
@@ -7,10 +7,11 @@
 DigitalOut on(D1);
 DigitalOut up(D2);
 DigitalOut down(D3);
-
+]
 bool manual = false;
 
 Serial pc(USBTX, USBRX, 57600);
+
 Timer t;
 EventQueue queue;
 
@@ -18,6 +19,13 @@
 Wheelchair smart(xDir,yDir, &pc, &t);
 Thread thread;
 
+void messageCb(const std_msgs::String& command){
+    myled = 1;   // turn on the led
+    commandRead.data = command.data;
+}
+
+ros::Subscriber<std_msgs::String> sub("toggle_led", &messageCb);
+
 int main(void)
 {
     queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
@@ -100,3 +108,7 @@
 
 }
 
+
+
+
+