aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
main.cpp@2:086272a2da1c, 2018-12-07 (annotated)
- Committer:
- nakedt555
- Date:
- Fri Dec 07 22:34:31 2018 +0000
- Revision:
- 2:086272a2da1c
- Parent:
- 1:bdd17feaa4ce
- Child:
- 3:a45557a0dcb8
12/08 work log;
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 | 2:086272a2da1c | 10 | //prototype |
nakedt555 | 2:086272a2da1c | 11 | void odom_update_callback(); |
nakedt555 | 2:086272a2da1c | 12 | |
nakedt555 | 1:bdd17feaa4ce | 13 | //Create subscliber |
nakedt555 | 1:bdd17feaa4ce | 14 | ros::Subscriber<std_msgs::Empty> ack_sub("nucleo/ack_from_pc", &ack_from_pc_cb); |
nakedt555 | 0:10f626cf3ec4 | 15 | |
nakedt555 | 2:086272a2da1c | 16 | //Initialize GPIO |
nakedt555 | 0:10f626cf3ec4 | 17 | DigitalOut myled(LED1); |
nakedt555 | 0:10f626cf3ec4 | 18 | DigitalOut led0(PC_0); |
nakedt555 | 0:10f626cf3ec4 | 19 | DigitalOut led1(PC_1); |
nakedt555 | 0:10f626cf3ec4 | 20 | DigitalOut led2(PB_0); |
nakedt555 | 0:10f626cf3ec4 | 21 | DigitalIn mysw(PC_13); |
nakedt555 | 0:10f626cf3ec4 | 22 | |
nakedt555 | 2:086272a2da1c | 23 | //Initialize ODOM |
nakedt555 | 2:086272a2da1c | 24 | Odom odom; |
nakedt555 | 2:086272a2da1c | 25 | |
nakedt555 | 0:10f626cf3ec4 | 26 | int main() { |
nakedt555 | 1:bdd17feaa4ce | 27 | //Initialize TIM |
nakedt555 | 1:bdd17feaa4ce | 28 | Timer GetTick; |
nakedt555 | 1:bdd17feaa4ce | 29 | GetTick.start(); |
nakedt555 | 1:bdd17feaa4ce | 30 | uint32_t debug_ts = 0; |
nakedt555 | 1:bdd17feaa4ce | 31 | uint32_t ros_loop_ts = 0; |
nakedt555 | 2:086272a2da1c | 32 | |
nakedt555 | 2:086272a2da1c | 33 | //Set TIM IT Callback |
nakedt555 | 2:086272a2da1c | 34 | Ticker odom_tick; |
nakedt555 | 2:086272a2da1c | 35 | odom_tick.attach(&odom_update_callback, 0.02); |
nakedt555 | 1:bdd17feaa4ce | 36 | |
nakedt555 | 1:bdd17feaa4ce | 37 | //Initialize ROS |
nakedt555 | 1:bdd17feaa4ce | 38 | My_Ros my_ros; |
nakedt555 | 1:bdd17feaa4ce | 39 | my_ros.set_subscliber(ack_sub); |
nakedt555 | 1:bdd17feaa4ce | 40 | |
nakedt555 | 0:10f626cf3ec4 | 41 | while(1) { |
nakedt555 | 1:bdd17feaa4ce | 42 | //For debug |
nakedt555 | 1:bdd17feaa4ce | 43 | if(debug_ts <= GetTick.read_ms()){ |
nakedt555 | 1:bdd17feaa4ce | 44 | debug_ts = GetTick.read_ms() + 20; |
nakedt555 | 1:bdd17feaa4ce | 45 | led0 = !led0; |
nakedt555 | 1:bdd17feaa4ce | 46 | } |
nakedt555 | 1:bdd17feaa4ce | 47 | |
nakedt555 | 1:bdd17feaa4ce | 48 | //For myRos loop |
nakedt555 | 1:bdd17feaa4ce | 49 | if(ros_loop_ts <= GetTick.read_ms()){ |
nakedt555 | 1:bdd17feaa4ce | 50 | ros_loop_ts = GetTick.read_ms() + 10; |
nakedt555 | 1:bdd17feaa4ce | 51 | my_ros.loop(); |
nakedt555 | 1:bdd17feaa4ce | 52 | } |
nakedt555 | 2:086272a2da1c | 53 | |
nakedt555 | 2:086272a2da1c | 54 | |
nakedt555 | 2:086272a2da1c | 55 | //角度計算用:ポーリングに置いとく |
nakedt555 | 2:086272a2da1c | 56 | odom.update_angle(); |
nakedt555 | 0:10f626cf3ec4 | 57 | } |
nakedt555 | 0:10f626cf3ec4 | 58 | } |
nakedt555 | 2:086272a2da1c | 59 | |
nakedt555 | 2:086272a2da1c | 60 | void odom_update_callback(){ |
nakedt555 | 2:086272a2da1c | 61 | odom.update_it_cb(); |
nakedt555 | 2:086272a2da1c | 62 | } |