Diff: main.cpp

Revision:

1:bdd17feaa4ce

Parent:

0:10f626cf3ec4

Child:

2:086272a2da1c

--- a/main.cpp	Thu Dec 06 10:22:27 2018 +0000
+++ b/main.cpp	Fri Dec 07 20:59:56 2018 +0000
@@ -1,6 +1,14 @@
 #include "mbed.h"
 
 #include "myRos.h"
+#include <ros.h>
+#include <ros/time.h>
+#include <std_msgs/Empty.h>
+
+#include "odom.h"
+
+//Create subscliber
+ros::Subscriber<std_msgs::Empty> ack_sub("nucleo/ack_from_pc", &ack_from_pc_cb);
 
 DigitalOut myled(LED1);
 DigitalOut  led0(PC_0);
@@ -9,12 +17,28 @@
 DigitalIn   mysw(PC_13);
 
 int main() {
+    //Initialize TIM
+    Timer GetTick;
+    GetTick.start();
+    uint32_t debug_ts = 0;
+    uint32_t ros_loop_ts = 0;
+        
+    //Initialize ROS
+    My_Ros my_ros;
+    my_ros.set_subscliber(ack_sub);
     
-    myRos my_ros;
-    
+
     while(1) {
-        led0 = !led0; // LED is ON
-        wait(0.2); // 200 ms
-        my_ros.loop();
+        //For debug
+        if(debug_ts <= GetTick.read_ms()){
+            debug_ts = GetTick.read_ms() + 20;
+            led0 = !led0;
+        }
+
+        //For myRos loop
+        if(ros_loop_ts <= GetTick.read_ms()){        
+            ros_loop_ts = GetTick.read_ms() + 10;
+            my_ros.loop();
+        }
     }
 }