File content as of revision 0:a35213e1e14e:

#include "mbed.h"
#include "QEncoder.h"
#include "SpeedController.h"
#include "MPU9250_SPI.h"
#include "Motor.h"
#include "Servo.h"
#include <ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h> 
#define PI 3.141592653589793
MPU9250_SPI mpu(p5,p6,p7,p8,p12); //mosi miso sclk cs intr
Servo servo(p13);
QEncoder enco(p15, p16);
Motor motor (p23, p24, p25, p26);
DigitalOut led1 = LED1;
PIDController speedController(0.085, 0.245, 0.00025, 100.0, 0.0, -1.0, 100.0);//0.05514

Timer timer;
Timer tmr;

ros::NodeHandle nh;
std_msgs::Int32 msg_encoder;
std_msgs::Float32MultiArray msg_imu_raw;
std_msgs::Float32MultiArray msg_imu_mag;

unsigned int uiFlag_1ms = 0;
float PWM = 0;
float dt;

void getDt(){
  dt=tmr.read_us()/1000000.0;
  tmr.reset();
}

void applyCalbratedValue(){
  Vect3 gBias ={ 0.236 , 1.527  ,  -0.558};
  mpu.setGyroBias(gBias); 
  Vect3 mBias ={-393.590,  334.364,368.100};
  mpu.setMagBias(mBias);
  Vect3 mScale={ 1.058,    1.022 , 0.929}; 
  mpu.setMagScale(mScale); 
  Vect3 aBias = { -0.095, 0.03 , -0.10};
  mpu.setAccBias(aBias);
}

void servo_cb(const std_msgs::Int32& cmd_msg)
{
    servo.update(cmd_msg.data);
}
void motor_cb(const std_msgs::Float32& cmd_msg)
{
    PWM = cmd_msg.data;
}

ros::Subscriber<std_msgs::Int32> servo_sub("servo", &servo_cb);
ros::Subscriber<std_msgs::Float32> motor_sub("motor", &motor_cb);
ros::Publisher encoder_pub("encoder", &msg_encoder);
ros::Publisher imu_raw_pub("imu_raw", &msg_imu_raw);
ros::Publisher imu_mag_pub("imu_mag", &msg_imu_mag);

void counter_1ms ()
{
    uiFlag_1ms++;
}

void pub_sub_setup()
{
    nh.initNode();
    nh.advertise(encoder_pub);
    nh.advertise(imu_raw_pub);
    nh.advertise(imu_mag_pub);
    nh.subscribe(servo_sub);
    nh.subscribe(motor_sub);
}

int main() {
    enco.init();
    enco.setCount(0);
    
    mpu.setup();
    mpu.setMagneticDeclination(8.5);
    mpu.setSampleRate(SR_100HZ);
    mpu.setGyroRange(GYRO_RANGE_2000DPS);
    mpu.setAccelRange(ACCEL_RANGE_16G);
    mpu.setDlpfBandwidth(DLPF_BANDWIDTH_184HZ); 
    mpu.enableDataReadyInterrupt();
    applyCalbratedValue();
    tmr.start(); tmr.reset();

    pub_sub_setup();
    
    Ticker ticker_1ms;
    ticker_1ms.attach(&counter_1ms, 0.001);
    timer.start();
    
    float degree = 0.0;
    servo.update (degree);

    int count = 0;
    int RPM = 0;
        
    
    float curr_speed = 0.0;
    float u_percent= 0.0;

    float raw_parm[6];
    float mag_parm[3];
    while(1) {
        // Every 1 ms,
        if(uiFlag_1ms >= 1) {
            uiFlag_1ms = 0;
        
            // Speed control
            speedController.setTarget(PWM);
            curr_speed = RPM * 2 * PI * 0.045 / 60;
            u_percent = speedController.update (curr_speed, timer.read());
            motor.setSpeed_percent(u_percent, FORWARD);
        }
        if(mpu.isDataReady())
        {
            getDt();
            Vect3  a, g, m;  // acc/gyro/mag vectors
            mpu.update(a,g,m);
            msg_imu_raw.data = raw_parm;
            msg_imu_mag.data = mag_parm;
            msg_imu_raw.data_length = 6;
            msg_imu_mag.data_length = 3;
            msg_imu_raw.data[0] = a.x;
            msg_imu_raw.data[1] = a.y;
            msg_imu_raw.data[2] = a.z;
            msg_imu_raw.data[3] = g.x;
            msg_imu_raw.data[4] = g.y;
            msg_imu_raw.data[5] = g.z;
            msg_imu_mag.data[0] = m.x;
            msg_imu_mag.data[1] = m.y;
            msg_imu_mag.data[2] = m.z;
            imu_raw_pub.publish(&msg_imu_raw);
            imu_mag_pub.publish(&msg_imu_mag);
        }

        led1 = !led1; 
        
        count = enco.getCount();
        RPM = -(60 * count) / (timer.read() * 3 * 200 * 4);
        
        msg_encoder.data = RPM;
        encoder_pub.publish(&msg_encoder);
        
        timer.reset();
        enco.setCount(0);
        
        nh.spinOnce();
        wait(0.1);
    }
}