aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
main.cpp
- Committer:
- nakedt555
- Date:
- 2019-08-30
- Revision:
- 12:f726eb78b54c
- Parent:
- 6:20a32baeff79
File content as of revision 12:f726eb78b54c:
#include <mbed.h> #include <vector> #include "myRos.h" #include "myCan.h" #include "myOled.h" #include <ros.h> #include <ros/time.h> #include <std_msgs/Empty.h> #include <geometry_msgs/Point.h> #include "odom.h" //Create subscliber ros::Subscriber<std_msgs::Empty> ack_sub("nucleo/ack_from_pc", &ack_from_pc_cb); ros::Subscriber<geometry_msgs::Point> drift_sub("nucleo/drift", &drift_sub_cb); //Initialize GPIO DigitalOut myled(LED1); DigitalOut led0(PC_0); DigitalOut led1(PC_1); DigitalOut led2(PB_0); DigitalIn mysw(PC_13); void led0_toggle(void) { led0 = !led0; } void led1_toggle(void) { led1 = !led1; } void led2_toggle(void) { led2 = !led2; } //Create Odom instance Odom odom; My_Can my_can(&odom); void attach_can_cb(void){ my_can.receive_cb(); } int main() { //Initialize TIM Timer GetTick; GetTick.start(); uint32_t debug_ts = 0; uint32_t ros_loop_ts = 0; //Initialize ROS My_Ros my_ros(&odom); my_ros.set_subscliber(ack_sub); my_ros.set_subscliber(drift_sub); //Initialize CAN my_can.initialize_amcl_attach(&my_ros, &My_Ros::enable_initialize_amcl); my_can.led_toggle_attach(&led1_toggle); my_can.can_rx_it_cb(&attach_can_cb); //Initialize OLED I2C i2c(PB_4, PA_8); My_Oled my_oled(&odom, i2c); std::vector<Odom_Abstract*> odom_container; odom_container.push_back(&my_ros); odom_container.push_back(&my_can); odom_container.push_back(&my_oled); while(1) { //For debug if(debug_ts <= GetTick.read_ms()){ debug_ts = GetTick.read_ms() + 100; led0_toggle(); } //For odom loop if(ros_loop_ts <= GetTick.read_ms()){ ros_loop_ts = GetTick.read_ms() + 10; for(std::vector<Odom_Abstract*>::iterator it = odom_container.begin(); it != odom_container.end(); ++it){ (*it)->loop(); } } //Debug if(mysw == 0){ while(mysw == 0); my_can.test(); } //角度計算用:ポーリングに置いとく odom.update_angle(); } }