aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
main.cpp@3:a45557a0dcb8, 2018-12-11 (annotated)
- Committer:
- nakedt555
- Date:
- Tue Dec 11 17:51:47 2018 +0000
- Revision:
- 3:a45557a0dcb8
- Parent:
- 2:086272a2da1c
- Child:
- 4:cf1a4e503974
12/10iikanji
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nakedt555 | 3:a45557a0dcb8 | 1 | #include <mbed.h> |
nakedt555 | 3:a45557a0dcb8 | 2 | #include <vector> |
nakedt555 | 0:10f626cf3ec4 | 3 | |
nakedt555 | 0:10f626cf3ec4 | 4 | #include "myRos.h" |
nakedt555 | 3:a45557a0dcb8 | 5 | #include "myCan.h" |
nakedt555 | 1:bdd17feaa4ce | 6 | #include <ros.h> |
nakedt555 | 1:bdd17feaa4ce | 7 | #include <ros/time.h> |
nakedt555 | 1:bdd17feaa4ce | 8 | #include <std_msgs/Empty.h> |
nakedt555 | 1:bdd17feaa4ce | 9 | #include "odom.h" |
nakedt555 | 1:bdd17feaa4ce | 10 | |
nakedt555 | 2:086272a2da1c | 11 | //prototype |
nakedt555 | 2:086272a2da1c | 12 | void odom_update_callback(); |
nakedt555 | 2:086272a2da1c | 13 | |
nakedt555 | 1:bdd17feaa4ce | 14 | //Create subscliber |
nakedt555 | 1:bdd17feaa4ce | 15 | ros::Subscriber<std_msgs::Empty> ack_sub("nucleo/ack_from_pc", &ack_from_pc_cb); |
nakedt555 | 0:10f626cf3ec4 | 16 | |
nakedt555 | 2:086272a2da1c | 17 | //Initialize GPIO |
nakedt555 | 0:10f626cf3ec4 | 18 | DigitalOut myled(LED1); |
nakedt555 | 0:10f626cf3ec4 | 19 | DigitalOut led0(PC_0); |
nakedt555 | 0:10f626cf3ec4 | 20 | DigitalOut led1(PC_1); |
nakedt555 | 0:10f626cf3ec4 | 21 | DigitalOut led2(PB_0); |
nakedt555 | 0:10f626cf3ec4 | 22 | DigitalIn mysw(PC_13); |
nakedt555 | 0:10f626cf3ec4 | 23 | |
nakedt555 | 3:a45557a0dcb8 | 24 | void led0_toggle(void) { led0 = !led0; } |
nakedt555 | 3:a45557a0dcb8 | 25 | void led1_toggle(void) { led1 = !led1; } |
nakedt555 | 3:a45557a0dcb8 | 26 | void led2_toggle(void) { led2 = !led2; } |
nakedt555 | 3:a45557a0dcb8 | 27 | |
nakedt555 | 3:a45557a0dcb8 | 28 | //Create Odom instance |
nakedt555 | 2:086272a2da1c | 29 | Odom odom; |
nakedt555 | 2:086272a2da1c | 30 | |
nakedt555 | 0:10f626cf3ec4 | 31 | int main() { |
nakedt555 | 1:bdd17feaa4ce | 32 | //Initialize TIM |
nakedt555 | 1:bdd17feaa4ce | 33 | Timer GetTick; |
nakedt555 | 1:bdd17feaa4ce | 34 | GetTick.start(); |
nakedt555 | 1:bdd17feaa4ce | 35 | uint32_t debug_ts = 0; |
nakedt555 | 1:bdd17feaa4ce | 36 | uint32_t ros_loop_ts = 0; |
nakedt555 | 2:086272a2da1c | 37 | |
nakedt555 | 1:bdd17feaa4ce | 38 | //Initialize ROS |
nakedt555 | 3:a45557a0dcb8 | 39 | My_Ros my_ros(&odom); |
nakedt555 | 1:bdd17feaa4ce | 40 | my_ros.set_subscliber(ack_sub); |
nakedt555 | 3:a45557a0dcb8 | 41 | |
nakedt555 | 3:a45557a0dcb8 | 42 | My_Can my_can(&odom); |
nakedt555 | 3:a45557a0dcb8 | 43 | my_can.initialize_amcl_attach(&my_ros, &My_Ros::enable_initialize_amcl); |
nakedt555 | 3:a45557a0dcb8 | 44 | my_can.led_toggle_attach(&led1_toggle); |
nakedt555 | 3:a45557a0dcb8 | 45 | |
nakedt555 | 3:a45557a0dcb8 | 46 | std::vector<Odom_Abstract*> odom_container; |
nakedt555 | 3:a45557a0dcb8 | 47 | odom_container.push_back(&my_ros); |
nakedt555 | 3:a45557a0dcb8 | 48 | odom_container.push_back(&my_can); |
nakedt555 | 1:bdd17feaa4ce | 49 | |
nakedt555 | 0:10f626cf3ec4 | 50 | while(1) { |
nakedt555 | 1:bdd17feaa4ce | 51 | //For debug |
nakedt555 | 1:bdd17feaa4ce | 52 | if(debug_ts <= GetTick.read_ms()){ |
nakedt555 | 3:a45557a0dcb8 | 53 | debug_ts = GetTick.read_ms() + 100; |
nakedt555 | 3:a45557a0dcb8 | 54 | led0_toggle(); |
nakedt555 | 1:bdd17feaa4ce | 55 | } |
nakedt555 | 1:bdd17feaa4ce | 56 | |
nakedt555 | 3:a45557a0dcb8 | 57 | //For odom loop |
nakedt555 | 1:bdd17feaa4ce | 58 | if(ros_loop_ts <= GetTick.read_ms()){ |
nakedt555 | 1:bdd17feaa4ce | 59 | ros_loop_ts = GetTick.read_ms() + 10; |
nakedt555 | 3:a45557a0dcb8 | 60 | for(std::vector<Odom_Abstract*>::iterator it = odom_container.begin(); it != odom_container.end(); ++it){ |
nakedt555 | 3:a45557a0dcb8 | 61 | (*it)->loop(); |
nakedt555 | 3:a45557a0dcb8 | 62 | } |
nakedt555 | 1:bdd17feaa4ce | 63 | } |
nakedt555 | 2:086272a2da1c | 64 | |
nakedt555 | 3:a45557a0dcb8 | 65 | if(mysw == 0){ |
nakedt555 | 3:a45557a0dcb8 | 66 | while(mysw == 0); |
nakedt555 | 3:a45557a0dcb8 | 67 | my_can.test(); |
nakedt555 | 3:a45557a0dcb8 | 68 | } |
nakedt555 | 2:086272a2da1c | 69 | |
nakedt555 | 2:086272a2da1c | 70 | //角度計算用:ポーリングに置いとく |
nakedt555 | 2:086272a2da1c | 71 | odom.update_angle(); |
nakedt555 | 0:10f626cf3ec4 | 72 | } |
nakedt555 | 2:086272a2da1c | 73 | } |