aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: main.cpp
- Revision:
- 1:bdd17feaa4ce
- Parent:
- 0:10f626cf3ec4
- Child:
- 2:086272a2da1c
diff -r 10f626cf3ec4 -r bdd17feaa4ce main.cpp --- 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(); + } } }