Diff: main.cpp

Revision:

1:1ee4c3c2b6a3

Parent:

0:20afeed366f0

--- a/main.cpp	Sat Dec 31 01:08:57 2016 +0000
+++ b/main.cpp	Thu Jun 20 23:51:09 2019 +0000
@@ -1,26 +1,152 @@
-/*
- * rosserial Subscriber Example
- * Blinks an LED on callback
- */
 #include "mbed.h"
 #include <ros.h>
-#include <std_msgs/Empty.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;
-DigitalOut myled(LED1);
+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);
 
-void messageCb(const std_msgs::Empty& toggle_msg){
-    myled = !myled;   // blink the led
+    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();
+    }
 }
 
-ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb);
+// ロボットの速度の取得
+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;
+}
 
-int main() {
-    nh.initNode();
-    nh.subscribe(sub);
+// オドメトリの計算
+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];
 
-    while (1) {
-        nh.spinOnce();
-        wait_ms(1);
+    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];
 }