File content as of revision 1:1ee4c3c2b6a3:

#include "mbed.h"
#include <ros.h>
#include <std_msgs/Float32MultiArray.h>
#include "StdControlLibrary.h"
#include <math.h>

#define getMPS(rpm, wheelDiameter) (rpm * wheelDiameter * PI / 60)
const double WHEELDIAMETER = 0.060;

float last_velo[3] = {0};  // 前ループのロボットの速度
float robot_odom[3] = {0}; // ロボットのオドメトリ
float pose[3] = {0.0};     // ROSから返ってきた自己位置

void getGyro(float return_gyro[3]);
void getVelo(float angle /*[rad]*/, float gyro[3], float return_velo[3]);
void calOdom(float robot_velo[3], float return_odom[3]);
void messageCb(const std_msgs::Float32MultiArray &msg);
void sendToRos();

Ticker flipper;

MPU6050 mpu(p9, p10);
QEI qei_x(p5, p6, NC, 159, QEI::X4_ENCODING);
QEI qei_y(p8, p7, NC, 159, QEI::X4_ENCODING);

DigitalOut leds[4] = {
    DigitalOut(LED1),
    DigitalOut(LED2),
    DigitalOut(LED3),
    DigitalOut(LED4)};

// ROSへ送信するデータ
// [0]リセット信号
// [1]オドメトリx
// [2]オドメトリy
// [3]オドメトリ角度
// [4]ラインセンサ1
// [5]ラインセンサ2
//
// ROSから送られてくるデータ
// [0]リセット信号の応答
// [1]推定自己位置x
// [2]推定自己位置y
// [3]推定自己位置rad
ros::NodeHandle nh;
std_msgs::Float32MultiArray send_data;
ros::Publisher pub_to_ros("mbed_to_ros", &send_data);
ros::Subscriber<std_msgs::Float32MultiArray> sub("ros_to_mbed", &messageCb);

int main()
{
    nh.initNode();
    nh.advertise(pub_to_ros);
    nh.subscribe(sub);

    // 送信パケットの初期化
    send_data.data_length = 6;
    send_data.data = (float *)malloc(sizeof(float) * send_data.data_length);
    for (int i = 0; i < send_data.data_length; i++)
        send_data.data[i] = 0.0;

    // 送信関数を一定周期で呼び出す
    flipper.attach(&sendToRos, 0.025);

    while (1)
    {
        // ジャイロ取得
        float gyro[3] = {0.0};
        getGyro(gyro);

        // 速度を出す
        float robot_velo[3] = {0};
        getVelo(robot_odom[2], gyro, robot_velo);

        // オドメトリを計算する
        calOdom(robot_velo, robot_odom);

        // ループが動いてるか確認
        leds[0] = !leds[0];

        // Y座標が1m以上なら点灯
        leds[3] = (pose[1] > 1) ? 1 : 0;

        nh.spinOnce();
    }
}

// ロボットの速度の取得
void getVelo(float angle /*[rad]*/, float gyro[3], float return_velo[3])
{
    // 速度配列へ代入
    float buf_x = getMPS(qei_x.getRPM(), WHEELDIAMETER) / 0.64;
    float buf_y = getMPS(qei_y.getRPM(), WHEELDIAMETER) / 0.64;
    return_velo[0] = buf_x * cos(angle) - buf_y * sin(angle);
    return_velo[1] = buf_x * sin(angle) + buf_y * cos(angle);
    return_velo[2] = gyro[2] * 3.1415926 / 180;
}

// オドメトリの計算
void calOdom(float robot_velo[3], float return_odom[3])
{
    // 速度を積分
    static Timer deltaTimer;
    deltaTimer.start();
    long double dt = deltaTimer.read();
    for (int i = 0; i < 3; i++)
    {
        return_odom[i] += ((robot_velo[i] + last_velo[i]) * (dt / 2.0));
        last_velo[i] = robot_velo[i];
    }

    deltaTimer.reset();
}

// ros_to_mbed コールバック関数
void messageCb(const std_msgs::Float32MultiArray &msg)
{
    leds[2] = !leds[2];

    pose[0] = msg.data[1];
    pose[1] = msg.data[2];
    pose[2] = msg.data[3];
}

// 送信用関数
void sendToRos()
{
    send_data.data[0] = 0;
    send_data.data[1] = robot_odom[0];
    send_data.data[2] = robot_odom[1];
    send_data.data[3] = robot_odom[2];
    send_data.data[4] = 0b00011000;
    send_data.data[5] = 0b11000000;

    // ROSへデータを送信
    pub_to_ros.publish(&send_data);

    leds[1] = !leds[1];
}

void getGyro(float return_gyro[3])
{
    double tmp_gyro[3];
    if (mpu.read())
    {
        mpu.getGyro(MPU6050_CORRECT, tmp_gyro);
    }

    // 値が小さい時は静止とみなし，大きい場合はそのまま代入
    for (int i = 0; i < 3; i++)
        return_gyro[i] = (abs(tmp_gyro[i]) < 0.8) ? 0 : (float)tmp_gyro[i];
}