ros
Dependencies: mbed QEI ros_lib_kinetic
main.cpp
00001 #include "mbed.h" 00002 #include <ros.h> 00003 #include <std_msgs/Float32MultiArray.h> 00004 #include "StdControlLibrary.h" 00005 #include <math.h> 00006 00007 #define getMPS(rpm, wheelDiameter) (rpm * wheelDiameter * PI / 60) 00008 const double WHEELDIAMETER = 0.060; 00009 00010 float last_velo[3] = {0}; // 前ループのロボットの速度 00011 float robot_odom[3] = {0}; // ロボットのオドメトリ 00012 float pose[3] = {0.0}; // ROSから返ってきた自己位置 00013 00014 void getGyro(float return_gyro[3]); 00015 void getVelo(float angle /*[rad]*/, float gyro[3], float return_velo[3]); 00016 void calOdom(float robot_velo[3], float return_odom[3]); 00017 void messageCb(const std_msgs::Float32MultiArray &msg); 00018 void sendToRos(); 00019 00020 Ticker flipper; 00021 00022 MPU6050 mpu(p9, p10); 00023 QEI qei_x(p5, p6, NC, 159, QEI::X4_ENCODING); 00024 QEI qei_y(p8, p7, NC, 159, QEI::X4_ENCODING); 00025 00026 DigitalOut leds[4] = { 00027 DigitalOut(LED1), 00028 DigitalOut(LED2), 00029 DigitalOut(LED3), 00030 DigitalOut(LED4)}; 00031 00032 // ROSへ送信するデータ 00033 // [0]リセット信号 00034 // [1]オドメトリx 00035 // [2]オドメトリy 00036 // [3]オドメトリ角度 00037 // [4]ラインセンサ1 00038 // [5]ラインセンサ2 00039 // 00040 // ROSから送られてくるデータ 00041 // [0]リセット信号の応答 00042 // [1]推定自己位置x 00043 // [2]推定自己位置y 00044 // [3]推定自己位置rad 00045 ros::NodeHandle nh; 00046 std_msgs::Float32MultiArray send_data; 00047 ros::Publisher pub_to_ros("mbed_to_ros", &send_data); 00048 ros::Subscriber<std_msgs::Float32MultiArray> sub("ros_to_mbed", &messageCb); 00049 00050 int main() 00051 { 00052 nh.initNode(); 00053 nh.advertise(pub_to_ros); 00054 nh.subscribe(sub); 00055 00056 // 送信パケットの初期化 00057 send_data.data_length = 6; 00058 send_data.data = (float *)malloc(sizeof(float) * send_data.data_length); 00059 for (int i = 0; i < send_data.data_length; i++) 00060 send_data.data[i] = 0.0; 00061 00062 // 送信関数を一定周期で呼び出す 00063 flipper.attach(&sendToRos, 0.025); 00064 00065 while (1) 00066 { 00067 // ジャイロ取得 00068 float gyro[3] = {0.0}; 00069 getGyro(gyro); 00070 00071 // 速度を出す 00072 float robot_velo[3] = {0}; 00073 getVelo(robot_odom[2], gyro, robot_velo); 00074 00075 // オドメトリを計算する 00076 calOdom(robot_velo, robot_odom); 00077 00078 // ループが動いてるか確認 00079 leds[0] = !leds[0]; 00080 00081 // Y座標が1m以上なら点灯 00082 leds[3] = (pose[1] > 1) ? 1 : 0; 00083 00084 nh.spinOnce(); 00085 } 00086 } 00087 00088 // ロボットの速度の取得 00089 void getVelo(float angle /*[rad]*/, float gyro[3], float return_velo[3]) 00090 { 00091 // 速度配列へ代入 00092 float buf_x = getMPS(qei_x.getRPM(), WHEELDIAMETER) / 0.64; 00093 float buf_y = getMPS(qei_y.getRPM(), WHEELDIAMETER) / 0.64; 00094 return_velo[0] = buf_x * cos(angle) - buf_y * sin(angle); 00095 return_velo[1] = buf_x * sin(angle) + buf_y * cos(angle); 00096 return_velo[2] = gyro[2] * 3.1415926 / 180; 00097 } 00098 00099 // オドメトリの計算 00100 void calOdom(float robot_velo[3], float return_odom[3]) 00101 { 00102 // 速度を積分 00103 static Timer deltaTimer; 00104 deltaTimer.start(); 00105 long double dt = deltaTimer.read(); 00106 for (int i = 0; i < 3; i++) 00107 { 00108 return_odom[i] += ((robot_velo[i] + last_velo[i]) * (dt / 2.0)); 00109 last_velo[i] = robot_velo[i]; 00110 } 00111 00112 deltaTimer.reset(); 00113 } 00114 00115 // ros_to_mbed コールバック関数 00116 void messageCb(const std_msgs::Float32MultiArray &msg) 00117 { 00118 leds[2] = !leds[2]; 00119 00120 pose[0] = msg.data[1]; 00121 pose[1] = msg.data[2]; 00122 pose[2] = msg.data[3]; 00123 } 00124 00125 // 送信用関数 00126 void sendToRos() 00127 { 00128 send_data.data[0] = 0; 00129 send_data.data[1] = robot_odom[0]; 00130 send_data.data[2] = robot_odom[1]; 00131 send_data.data[3] = robot_odom[2]; 00132 send_data.data[4] = 0b00011000; 00133 send_data.data[5] = 0b11000000; 00134 00135 // ROSへデータを送信 00136 pub_to_ros.publish(&send_data); 00137 00138 leds[1] = !leds[1]; 00139 } 00140 00141 void getGyro(float return_gyro[3]) 00142 { 00143 double tmp_gyro[3]; 00144 if (mpu.read()) 00145 { 00146 mpu.getGyro(MPU6050_CORRECT, tmp_gyro); 00147 } 00148 00149 // 値が小さい時は静止とみなし,大きい場合はそのまま代入 00150 for (int i = 0; i < 3; i++) 00151 return_gyro[i] = (abs(tmp_gyro[i]) < 0.8) ? 0 : (float)tmp_gyro[i]; 00152 }
Generated on Thu Jul 21 2022 01:14:33 by
1.7.2