aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: main.cpp
- Revision:
- 3:a45557a0dcb8
- Parent:
- 2:086272a2da1c
- Child:
- 4:cf1a4e503974
diff -r 086272a2da1c -r a45557a0dcb8 main.cpp --- a/main.cpp Fri Dec 07 22:34:31 2018 +0000 +++ b/main.cpp Tue Dec 11 17:51:47 2018 +0000 @@ -1,10 +1,11 @@ -#include "mbed.h" +#include <mbed.h> +#include <vector> #include "myRos.h" +#include "myCan.h" #include <ros.h> #include <ros/time.h> #include <std_msgs/Empty.h> - #include "odom.h" //prototype @@ -20,7 +21,11 @@ DigitalOut led2(PB_0); DigitalIn mysw(PC_13); -//Initialize ODOM +void led0_toggle(void) { led0 = !led0; } +void led1_toggle(void) { led1 = !led1; } +void led2_toggle(void) { led2 = !led2; } + +//Create Odom instance Odom odom; int main() { @@ -30,33 +35,39 @@ uint32_t debug_ts = 0; uint32_t ros_loop_ts = 0; - //Set TIM IT Callback - Ticker odom_tick; - odom_tick.attach(&odom_update_callback, 0.02); - //Initialize ROS - My_Ros my_ros; + My_Ros my_ros(&odom); my_ros.set_subscliber(ack_sub); + + My_Can my_can(&odom); + my_can.initialize_amcl_attach(&my_ros, &My_Ros::enable_initialize_amcl); + my_can.led_toggle_attach(&led1_toggle); + + std::vector<Odom_Abstract*> odom_container; + odom_container.push_back(&my_ros); + odom_container.push_back(&my_can); while(1) { //For debug if(debug_ts <= GetTick.read_ms()){ - debug_ts = GetTick.read_ms() + 20; - led0 = !led0; + debug_ts = GetTick.read_ms() + 100; + led0_toggle(); } - //For myRos loop + //For odom loop if(ros_loop_ts <= GetTick.read_ms()){ ros_loop_ts = GetTick.read_ms() + 10; - my_ros.loop(); + for(std::vector<Odom_Abstract*>::iterator it = odom_container.begin(); it != odom_container.end(); ++it){ + (*it)->loop(); + } } + if(mysw == 0){ + while(mysw == 0); + my_can.test(); + } //角度計算用:ポーリングに置いとく odom.update_angle(); } -} - -void odom_update_callback(){ - odom.update_it_cb(); } \ No newline at end of file