aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
main.cpp@6:20a32baeff79, 2018-12-18 (annotated)
- Committer:
- nakedt555
- Date:
- Tue Dec 18 21:51:13 2018 +0000
- Revision:
- 6:20a32baeff79
- Parent:
- 4:cf1a4e503974
kita
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 | 4:cf1a4e503974 | 6 | #include "myOled.h" |
nakedt555 | 1:bdd17feaa4ce | 7 | #include <ros.h> |
nakedt555 | 1:bdd17feaa4ce | 8 | #include <ros/time.h> |
nakedt555 | 1:bdd17feaa4ce | 9 | #include <std_msgs/Empty.h> |
nakedt555 | 4:cf1a4e503974 | 10 | #include <geometry_msgs/Point.h> |
nakedt555 | 1:bdd17feaa4ce | 11 | #include "odom.h" |
nakedt555 | 1:bdd17feaa4ce | 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 | 4:cf1a4e503974 | 15 | ros::Subscriber<geometry_msgs::Point> drift_sub("nucleo/drift", &drift_sub_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 | 6:20a32baeff79 | 30 | My_Can my_can(&odom); |
nakedt555 | 6:20a32baeff79 | 31 | |
nakedt555 | 6:20a32baeff79 | 32 | void attach_can_cb(void){ my_can.receive_cb(); } |
nakedt555 | 6:20a32baeff79 | 33 | |
nakedt555 | 0:10f626cf3ec4 | 34 | int main() { |
nakedt555 | 1:bdd17feaa4ce | 35 | //Initialize TIM |
nakedt555 | 1:bdd17feaa4ce | 36 | Timer GetTick; |
nakedt555 | 1:bdd17feaa4ce | 37 | GetTick.start(); |
nakedt555 | 1:bdd17feaa4ce | 38 | uint32_t debug_ts = 0; |
nakedt555 | 1:bdd17feaa4ce | 39 | uint32_t ros_loop_ts = 0; |
nakedt555 | 2:086272a2da1c | 40 | |
nakedt555 | 1:bdd17feaa4ce | 41 | //Initialize ROS |
nakedt555 | 3:a45557a0dcb8 | 42 | My_Ros my_ros(&odom); |
nakedt555 | 1:bdd17feaa4ce | 43 | my_ros.set_subscliber(ack_sub); |
nakedt555 | 4:cf1a4e503974 | 44 | my_ros.set_subscliber(drift_sub); |
nakedt555 | 3:a45557a0dcb8 | 45 | |
nakedt555 | 4:cf1a4e503974 | 46 | //Initialize CAN |
nakedt555 | 3:a45557a0dcb8 | 47 | my_can.initialize_amcl_attach(&my_ros, &My_Ros::enable_initialize_amcl); |
nakedt555 | 3:a45557a0dcb8 | 48 | my_can.led_toggle_attach(&led1_toggle); |
nakedt555 | 6:20a32baeff79 | 49 | my_can.can_rx_it_cb(&attach_can_cb); |
nakedt555 | 3:a45557a0dcb8 | 50 | |
nakedt555 | 4:cf1a4e503974 | 51 | //Initialize OLED |
nakedt555 | 4:cf1a4e503974 | 52 | I2C i2c(PB_4, PA_8); |
nakedt555 | 4:cf1a4e503974 | 53 | My_Oled my_oled(&odom, i2c); |
nakedt555 | 4:cf1a4e503974 | 54 | |
nakedt555 | 3:a45557a0dcb8 | 55 | std::vector<Odom_Abstract*> odom_container; |
nakedt555 | 3:a45557a0dcb8 | 56 | odom_container.push_back(&my_ros); |
nakedt555 | 3:a45557a0dcb8 | 57 | odom_container.push_back(&my_can); |
nakedt555 | 4:cf1a4e503974 | 58 | odom_container.push_back(&my_oled); |
nakedt555 | 1:bdd17feaa4ce | 59 | |
nakedt555 | 0:10f626cf3ec4 | 60 | while(1) { |
nakedt555 | 1:bdd17feaa4ce | 61 | //For debug |
nakedt555 | 1:bdd17feaa4ce | 62 | if(debug_ts <= GetTick.read_ms()){ |
nakedt555 | 3:a45557a0dcb8 | 63 | debug_ts = GetTick.read_ms() + 100; |
nakedt555 | 3:a45557a0dcb8 | 64 | led0_toggle(); |
nakedt555 | 1:bdd17feaa4ce | 65 | } |
nakedt555 | 1:bdd17feaa4ce | 66 | |
nakedt555 | 3:a45557a0dcb8 | 67 | //For odom loop |
nakedt555 | 1:bdd17feaa4ce | 68 | if(ros_loop_ts <= GetTick.read_ms()){ |
nakedt555 | 1:bdd17feaa4ce | 69 | ros_loop_ts = GetTick.read_ms() + 10; |
nakedt555 | 3:a45557a0dcb8 | 70 | for(std::vector<Odom_Abstract*>::iterator it = odom_container.begin(); it != odom_container.end(); ++it){ |
nakedt555 | 3:a45557a0dcb8 | 71 | (*it)->loop(); |
nakedt555 | 3:a45557a0dcb8 | 72 | } |
nakedt555 | 1:bdd17feaa4ce | 73 | } |
nakedt555 | 2:086272a2da1c | 74 | |
nakedt555 | 4:cf1a4e503974 | 75 | //Debug |
nakedt555 | 3:a45557a0dcb8 | 76 | if(mysw == 0){ |
nakedt555 | 3:a45557a0dcb8 | 77 | while(mysw == 0); |
nakedt555 | 3:a45557a0dcb8 | 78 | my_can.test(); |
nakedt555 | 3:a45557a0dcb8 | 79 | } |
nakedt555 | 2:086272a2da1c | 80 | |
nakedt555 | 2:086272a2da1c | 81 | //角度計算用:ポーリングに置いとく |
nakedt555 | 2:086272a2da1c | 82 | odom.update_angle(); |
nakedt555 | 0:10f626cf3ec4 | 83 | } |
nakedt555 | 2:086272a2da1c | 84 | } |