aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

Committer:
nakedt555
Date:
Fri Dec 07 20:59:56 2018 +0000
Revision:
1:bdd17feaa4ce
Parent:
0:10f626cf3ec4
Child:
2:086272a2da1c
create odom_h;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nakedt555 0:10f626cf3ec4 1 #include "mbed.h"
nakedt555 0:10f626cf3ec4 2
nakedt555 0:10f626cf3ec4 3 #include "myRos.h"
nakedt555 1:bdd17feaa4ce 4 #include <ros.h>
nakedt555 1:bdd17feaa4ce 5 #include <ros/time.h>
nakedt555 1:bdd17feaa4ce 6 #include <std_msgs/Empty.h>
nakedt555 1:bdd17feaa4ce 7
nakedt555 1:bdd17feaa4ce 8 #include "odom.h"
nakedt555 1:bdd17feaa4ce 9
nakedt555 1:bdd17feaa4ce 10 //Create subscliber
nakedt555 1:bdd17feaa4ce 11 ros::Subscriber<std_msgs::Empty> ack_sub("nucleo/ack_from_pc", &ack_from_pc_cb);
nakedt555 0:10f626cf3ec4 12
nakedt555 0:10f626cf3ec4 13 DigitalOut myled(LED1);
nakedt555 0:10f626cf3ec4 14 DigitalOut led0(PC_0);
nakedt555 0:10f626cf3ec4 15 DigitalOut led1(PC_1);
nakedt555 0:10f626cf3ec4 16 DigitalOut led2(PB_0);
nakedt555 0:10f626cf3ec4 17 DigitalIn mysw(PC_13);
nakedt555 0:10f626cf3ec4 18
nakedt555 0:10f626cf3ec4 19 int main() {
nakedt555 1:bdd17feaa4ce 20 //Initialize TIM
nakedt555 1:bdd17feaa4ce 21 Timer GetTick;
nakedt555 1:bdd17feaa4ce 22 GetTick.start();
nakedt555 1:bdd17feaa4ce 23 uint32_t debug_ts = 0;
nakedt555 1:bdd17feaa4ce 24 uint32_t ros_loop_ts = 0;
nakedt555 1:bdd17feaa4ce 25
nakedt555 1:bdd17feaa4ce 26 //Initialize ROS
nakedt555 1:bdd17feaa4ce 27 My_Ros my_ros;
nakedt555 1:bdd17feaa4ce 28 my_ros.set_subscliber(ack_sub);
nakedt555 0:10f626cf3ec4 29
nakedt555 1:bdd17feaa4ce 30
nakedt555 0:10f626cf3ec4 31 while(1) {
nakedt555 1:bdd17feaa4ce 32 //For debug
nakedt555 1:bdd17feaa4ce 33 if(debug_ts <= GetTick.read_ms()){
nakedt555 1:bdd17feaa4ce 34 debug_ts = GetTick.read_ms() + 20;
nakedt555 1:bdd17feaa4ce 35 led0 = !led0;
nakedt555 1:bdd17feaa4ce 36 }
nakedt555 1:bdd17feaa4ce 37
nakedt555 1:bdd17feaa4ce 38 //For myRos loop
nakedt555 1:bdd17feaa4ce 39 if(ros_loop_ts <= GetTick.read_ms()){
nakedt555 1:bdd17feaa4ce 40 ros_loop_ts = GetTick.read_ms() + 10;
nakedt555 1:bdd17feaa4ce 41 my_ros.loop();
nakedt555 1:bdd17feaa4ce 42 }
nakedt555 0:10f626cf3ec4 43 }
nakedt555 0:10f626cf3ec4 44 }