aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
main.cpp@1:bdd17feaa4ce, 2018-12-07 (annotated)
- 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?
User | Revision | Line number | New 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 | } |