Ryoga Sato / Mbed 2 deprecated robocon19_ros

ros

Dependencies:   mbed QEI ros_lib_kinetic

main.cpp@1:1ee4c3c2b6a3, 2019-06-20 (annotated)

Committer:
surpace0924
Date:
Thu Jun 20 23:51:09 2019 +0000
Revision:
1:1ee4c3c2b6a3
Parent:
0:20afeed366f0 
update;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garyservin 0:20afeed366f0 1 #include "mbed.h"
garyservin 0:20afeed366f0 2 #include <ros.h>
surpace0924 1:1ee4c3c2b6a3 3 #include <std_msgs/Float32MultiArray.h>
surpace0924 1:1ee4c3c2b6a3 4 #include "StdControlLibrary.h"
surpace0924 1:1ee4c3c2b6a3 5 #include <math.h>
surpace0924 1:1ee4c3c2b6a3 6
surpace0924 1:1ee4c3c2b6a3 7 #define getMPS(rpm, wheelDiameter) (rpm * wheelDiameter * PI / 60)
surpace0924 1:1ee4c3c2b6a3 8 const double WHEELDIAMETER = 0.060;
surpace0924 1:1ee4c3c2b6a3 9
surpace0924 1:1ee4c3c2b6a3 10 float last_velo[3] = {0}; // 前ループのロボットの速度
surpace0924 1:1ee4c3c2b6a3 11 float robot_odom[3] = {0}; // ロボットのオドメトリ
surpace0924 1:1ee4c3c2b6a3 12 float pose[3] = {0.0}; // ROSから返ってきた自己位置
surpace0924 1:1ee4c3c2b6a3 13
surpace0924 1:1ee4c3c2b6a3 14 void getGyro(float return_gyro[3]);
surpace0924 1:1ee4c3c2b6a3 15 void getVelo(float angle /*[rad]*/, float gyro[3], float return_velo[3]);
surpace0924 1:1ee4c3c2b6a3 16 void calOdom(float robot_velo[3], float return_odom[3]);
surpace0924 1:1ee4c3c2b6a3 17 void messageCb(const std_msgs::Float32MultiArray &msg);
surpace0924 1:1ee4c3c2b6a3 18 void sendToRos();
surpace0924 1:1ee4c3c2b6a3 19
surpace0924 1:1ee4c3c2b6a3 20 Ticker flipper;
garyservin 0:20afeed366f0 21
surpace0924 1:1ee4c3c2b6a3 22 MPU6050 mpu(p9, p10);
surpace0924 1:1ee4c3c2b6a3 23 QEI qei_x(p5, p6, NC, 159, QEI::X4_ENCODING);
surpace0924 1:1ee4c3c2b6a3 24 QEI qei_y(p8, p7, NC, 159, QEI::X4_ENCODING);
surpace0924 1:1ee4c3c2b6a3 25
surpace0924 1:1ee4c3c2b6a3 26 DigitalOut leds[4] = {
surpace0924 1:1ee4c3c2b6a3 27 DigitalOut(LED1),
surpace0924 1:1ee4c3c2b6a3 28 DigitalOut(LED2),
surpace0924 1:1ee4c3c2b6a3 29 DigitalOut(LED3),
surpace0924 1:1ee4c3c2b6a3 30 DigitalOut(LED4)};
surpace0924 1:1ee4c3c2b6a3 31
surpace0924 1:1ee4c3c2b6a3 32 // ROSへ送信するデータ
surpace0924 1:1ee4c3c2b6a3 33 // [0]リセット信号
surpace0924 1:1ee4c3c2b6a3 34 // [1]オドメトリx
surpace0924 1:1ee4c3c2b6a3 35 // [2]オドメトリy
surpace0924 1:1ee4c3c2b6a3 36 // [3]オドメトリ角度
surpace0924 1:1ee4c3c2b6a3 37 // [4]ラインセンサ1
surpace0924 1:1ee4c3c2b6a3 38 // [5]ラインセンサ2
surpace0924 1:1ee4c3c2b6a3 39 //
surpace0924 1:1ee4c3c2b6a3 40 // ROSから送られてくるデータ
surpace0924 1:1ee4c3c2b6a3 41 // [0]リセット信号の応答
surpace0924 1:1ee4c3c2b6a3 42 // [1]推定自己位置x
surpace0924 1:1ee4c3c2b6a3 43 // [2]推定自己位置y
surpace0924 1:1ee4c3c2b6a3 44 // [3]推定自己位置rad
garyservin 0:20afeed366f0 45 ros::NodeHandle nh;
surpace0924 1:1ee4c3c2b6a3 46 std_msgs::Float32MultiArray send_data;
surpace0924 1:1ee4c3c2b6a3 47 ros::Publisher pub_to_ros("mbed_to_ros", &send_data);
surpace0924 1:1ee4c3c2b6a3 48 ros::Subscriber<std_msgs::Float32MultiArray> sub("ros_to_mbed", &messageCb);
surpace0924 1:1ee4c3c2b6a3 49
surpace0924 1:1ee4c3c2b6a3 50 int main()
surpace0924 1:1ee4c3c2b6a3 51 {
surpace0924 1:1ee4c3c2b6a3 52 nh.initNode();
surpace0924 1:1ee4c3c2b6a3 53 nh.advertise(pub_to_ros);
surpace0924 1:1ee4c3c2b6a3 54 nh.subscribe(sub);
surpace0924 1:1ee4c3c2b6a3 55
surpace0924 1:1ee4c3c2b6a3 56 // 送信パケットの初期化
surpace0924 1:1ee4c3c2b6a3 57 send_data.data_length = 6;
surpace0924 1:1ee4c3c2b6a3 58 send_data.data = (float *)malloc(sizeof(float) * send_data.data_length);
surpace0924 1:1ee4c3c2b6a3 59 for (int i = 0; i < send_data.data_length; i++)
surpace0924 1:1ee4c3c2b6a3 60 send_data.data[i] = 0.0;
surpace0924 1:1ee4c3c2b6a3 61
surpace0924 1:1ee4c3c2b6a3 62 // 送信関数を一定周期で呼び出す
surpace0924 1:1ee4c3c2b6a3 63 flipper.attach(&sendToRos, 0.025);
garyservin 0:20afeed366f0 64
surpace0924 1:1ee4c3c2b6a3 65 while (1)
surpace0924 1:1ee4c3c2b6a3 66 {
surpace0924 1:1ee4c3c2b6a3 67 // ジャイロ取得
surpace0924 1:1ee4c3c2b6a3 68 float gyro[3] = {0.0};
surpace0924 1:1ee4c3c2b6a3 69 getGyro(gyro);
surpace0924 1:1ee4c3c2b6a3 70
surpace0924 1:1ee4c3c2b6a3 71 // 速度を出す
surpace0924 1:1ee4c3c2b6a3 72 float robot_velo[3] = {0};
surpace0924 1:1ee4c3c2b6a3 73 getVelo(robot_odom[2], gyro, robot_velo);
surpace0924 1:1ee4c3c2b6a3 74
surpace0924 1:1ee4c3c2b6a3 75 // オドメトリを計算する
surpace0924 1:1ee4c3c2b6a3 76 calOdom(robot_velo, robot_odom);
surpace0924 1:1ee4c3c2b6a3 77
surpace0924 1:1ee4c3c2b6a3 78 // ループが動いてるか確認
surpace0924 1:1ee4c3c2b6a3 79 leds[0] = !leds[0];
surpace0924 1:1ee4c3c2b6a3 80
surpace0924 1:1ee4c3c2b6a3 81 // Y座標が1m以上なら点灯
surpace0924 1:1ee4c3c2b6a3 82 leds[3] = (pose[1] > 1) ? 1 : 0;
surpace0924 1:1ee4c3c2b6a3 83
surpace0924 1:1ee4c3c2b6a3 84 nh.spinOnce();
surpace0924 1:1ee4c3c2b6a3 85 }
garyservin 0:20afeed366f0 86 }
garyservin 0:20afeed366f0 87
surpace0924 1:1ee4c3c2b6a3 88 // ロボットの速度の取得
surpace0924 1:1ee4c3c2b6a3 89 void getVelo(float angle /*[rad]*/, float gyro[3], float return_velo[3])
surpace0924 1:1ee4c3c2b6a3 90 {
surpace0924 1:1ee4c3c2b6a3 91 // 速度配列へ代入
surpace0924 1:1ee4c3c2b6a3 92 float buf_x = getMPS(qei_x.getRPM(), WHEELDIAMETER) / 0.64;
surpace0924 1:1ee4c3c2b6a3 93 float buf_y = getMPS(qei_y.getRPM(), WHEELDIAMETER) / 0.64;
surpace0924 1:1ee4c3c2b6a3 94 return_velo[0] = buf_x * cos(angle) - buf_y * sin(angle);
surpace0924 1:1ee4c3c2b6a3 95 return_velo[1] = buf_x * sin(angle) + buf_y * cos(angle);
surpace0924 1:1ee4c3c2b6a3 96 return_velo[2] = gyro[2] * 3.1415926 / 180;
surpace0924 1:1ee4c3c2b6a3 97 }
garyservin 0:20afeed366f0 98
surpace0924 1:1ee4c3c2b6a3 99 // オドメトリの計算
surpace0924 1:1ee4c3c2b6a3 100 void calOdom(float robot_velo[3], float return_odom[3])
surpace0924 1:1ee4c3c2b6a3 101 {
surpace0924 1:1ee4c3c2b6a3 102 // 速度を積分
surpace0924 1:1ee4c3c2b6a3 103 static Timer deltaTimer;
surpace0924 1:1ee4c3c2b6a3 104 deltaTimer.start();
surpace0924 1:1ee4c3c2b6a3 105 long double dt = deltaTimer.read();
surpace0924 1:1ee4c3c2b6a3 106 for (int i = 0; i < 3; i++)
surpace0924 1:1ee4c3c2b6a3 107 {
surpace0924 1:1ee4c3c2b6a3 108 return_odom[i] += ((robot_velo[i] + last_velo[i]) * (dt / 2.0));
surpace0924 1:1ee4c3c2b6a3 109 last_velo[i] = robot_velo[i];
surpace0924 1:1ee4c3c2b6a3 110 }
surpace0924 1:1ee4c3c2b6a3 111
surpace0924 1:1ee4c3c2b6a3 112 deltaTimer.reset();
surpace0924 1:1ee4c3c2b6a3 113 }
surpace0924 1:1ee4c3c2b6a3 114
surpace0924 1:1ee4c3c2b6a3 115 // ros_to_mbed コールバック関数
surpace0924 1:1ee4c3c2b6a3 116 void messageCb(const std_msgs::Float32MultiArray &msg)
surpace0924 1:1ee4c3c2b6a3 117 {
surpace0924 1:1ee4c3c2b6a3 118 leds[2] = !leds[2];
garyservin 0:20afeed366f0 119
surpace0924 1:1ee4c3c2b6a3 120 pose[0] = msg.data[1];
surpace0924 1:1ee4c3c2b6a3 121 pose[1] = msg.data[2];
surpace0924 1:1ee4c3c2b6a3 122 pose[2] = msg.data[3];
surpace0924 1:1ee4c3c2b6a3 123 }
surpace0924 1:1ee4c3c2b6a3 124
surpace0924 1:1ee4c3c2b6a3 125 // 送信用関数
surpace0924 1:1ee4c3c2b6a3 126 void sendToRos()
surpace0924 1:1ee4c3c2b6a3 127 {
surpace0924 1:1ee4c3c2b6a3 128 send_data.data[0] = 0;
surpace0924 1:1ee4c3c2b6a3 129 send_data.data[1] = robot_odom[0];
surpace0924 1:1ee4c3c2b6a3 130 send_data.data[2] = robot_odom[1];
surpace0924 1:1ee4c3c2b6a3 131 send_data.data[3] = robot_odom[2];
surpace0924 1:1ee4c3c2b6a3 132 send_data.data[4] = 0b00011000;
surpace0924 1:1ee4c3c2b6a3 133 send_data.data[5] = 0b11000000;
surpace0924 1:1ee4c3c2b6a3 134
surpace0924 1:1ee4c3c2b6a3 135 // ROSへデータを送信
surpace0924 1:1ee4c3c2b6a3 136 pub_to_ros.publish(&send_data);
surpace0924 1:1ee4c3c2b6a3 137
surpace0924 1:1ee4c3c2b6a3 138 leds[1] = !leds[1];
surpace0924 1:1ee4c3c2b6a3 139 }
surpace0924 1:1ee4c3c2b6a3 140
surpace0924 1:1ee4c3c2b6a3 141 void getGyro(float return_gyro[3])
surpace0924 1:1ee4c3c2b6a3 142 {
surpace0924 1:1ee4c3c2b6a3 143 double tmp_gyro[3];
surpace0924 1:1ee4c3c2b6a3 144 if (mpu.read())
surpace0924 1:1ee4c3c2b6a3 145 {
surpace0924 1:1ee4c3c2b6a3 146 mpu.getGyro(MPU6050_CORRECT, tmp_gyro);
garyservin 0:20afeed366f0 147 }
surpace0924 1:1ee4c3c2b6a3 148
surpace0924 1:1ee4c3c2b6a3 149 // 値が小さい時は静止とみなし，大きい場合はそのまま代入
surpace0924 1:1ee4c3c2b6a3 150 for (int i = 0; i < 3; i++)
surpace0924 1:1ee4c3c2b6a3 151 return_gyro[i] = (abs(tmp_gyro[i]) < 0.8) ? 0 : (float)tmp_gyro[i];
garyservin 0:20afeed366f0 152 }