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();
    }
}