_a i_ / Mbed 2 deprecated 2020_lab_No2reactor_contorl_ver2

abeの研究に利用：エンドエフェクタ2のモータを動作させるために必要となるプログラム

Dependencies:   ros_lib_indigo mbed ServoMotor

main.cpp@0:9735e9771ad4, 2021-02-24 (annotated)

Committer:
_ai_
Date:
Wed Feb 24 03:04:50 2021 +0000
Revision:
0:9735e9771ad4
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
_ai_ 0:9735e9771ad4 1 #include "mbed.h"
_ai_ 0:9735e9771ad4 2 #include <ros.h>
_ai_ 0:9735e9771ad4 3 #include <std_msgs/String.h>
_ai_ 0:9735e9771ad4 4 #include <std_msgs/Float32MultiArray.h>
_ai_ 0:9735e9771ad4 5 #include <std_msgs/Float32.h>
_ai_ 0:9735e9771ad4 6 #include "ServoMotor.h"
_ai_ 0:9735e9771ad4 7
_ai_ 0:9735e9771ad4 8 //システム関連
_ai_ 0:9735e9771ad4 9 Timer timer;
_ai_ 0:9735e9771ad4 10 float CTRL_PERIOD = 0.02; //制御周期[s]
_ai_ 0:9735e9771ad4 11 float READ_TIME = 0.0 ; //時間読み取り
_ai_ 0:9735e9771ad4 12
_ai_ 0:9735e9771ad4 13 //サーボモータをPWMでうごかすための設定(futaba RS204MDはPWMで動かす時は0~288[deg]が動作範囲)
_ai_ 0:9735e9771ad4 14 ServoMotor SM_R( D3, 0.0, 288.0, 0.02, 0.00056, 0.00248); //右指ブラシモータ
_ai_ 0:9735e9771ad4 15 ServoMotor SM_L( D7, 0.0, 288.0, 0.02, 0.00248, 0.00056); //左指ブラシモータ
_ai_ 0:9735e9771ad4 16 ServoMotor SM_S(D10, 0.0, 288.0, 0.02, 0.00056, 0.00248); //左指部
_ai_ 0:9735e9771ad4 17
_ai_ 0:9735e9771ad4 18 ros::NodeHandle n;
_ai_ 0:9735e9771ad4 19
_ai_ 0:9735e9771ad4 20 std_msgs::String str_msg;
_ai_ 0:9735e9771ad4 21 std_msgs::Float32 debug_deg;
_ai_ 0:9735e9771ad4 22 std_msgs::Float32MultiArray lrs_now_deg;
_ai_ 0:9735e9771ad4 23
_ai_ 0:9735e9771ad4 24 ros::Publisher motion_judg("/micon_LRS_motion_judg", &str_msg);
_ai_ 0:9735e9771ad4 25 ros::Publisher LRS_now_deg_pub("/micon_LRS_now_deg", &lrs_now_deg);
_ai_ 0:9735e9771ad4 26 ros::Publisher debug_deg_pub("/debug_deg",&debug_deg);
_ai_ 0:9735e9771ad4 27
_ai_ 0:9735e9771ad4 28 //モータ計算関連
_ai_ 0:9735e9771ad4 29 void synchoroBrushAndSlider(float deg_L_, float deg_S_, int num_); //モータ微小目標角度生成関数 引数1:目標角度 引数2:分割数
_ai_ 0:9735e9771ad4 30 void allSMZero(float deg); //モータの角度を全てdeg度にする
_ai_ 0:9735e9771ad4 31
_ai_ 0:9735e9771ad4 32 float now_deg[3]= {0.0,0.0,0.0}; //0:S 1:L 2:R
_ai_ 0:9735e9771ad4 33
_ai_ 0:9735e9771ad4 34
_ai_ 0:9735e9771ad4 35 /*--- 左指ブラシとスライダを同期して動かす関数 ---*/
_ai_ 0:9735e9771ad4 36 bool motion_triger = 0;
_ai_ 0:9735e9771ad4 37 void synchoroBrushAndSlider(float deg_L_, float deg_S_, float target_time_)
_ai_ 0:9735e9771ad4 38 {
_ai_ 0:9735e9771ad4 39 if(motion_triger == 0) {
_ai_ 0:9735e9771ad4 40 motion_triger = 1;
_ai_ 0:9735e9771ad4 41
_ai_ 0:9735e9771ad4 42 float target[2]; //目標角度 0:S 1:L
_ai_ 0:9735e9771ad4 43 float diff_deg[2]; //目標と現在位置の差分
_ai_ 0:9735e9771ad4 44 float min_deg[2]; //最小移動角度
_ai_ 0:9735e9771ad4 45 float ini_deg[2];
_ai_ 0:9735e9771ad4 46 int motion_num = target_time_/CTRL_PERIOD; //移動回数
_ai_ 0:9735e9771ad4 47 int cnt = 0;
_ai_ 0:9735e9771ad4 48
_ai_ 0:9735e9771ad4 49 target[0]=deg_S_;
_ai_ 0:9735e9771ad4 50 target[1]=deg_L_;
_ai_ 0:9735e9771ad4 51 for(int i=0; i<2; i++) {
_ai_ 0:9735e9771ad4 52 ini_deg[i] = now_deg[i];
_ai_ 0:9735e9771ad4 53 diff_deg[i] = target[i]-ini_deg[i];
_ai_ 0:9735e9771ad4 54 min_deg[i] = diff_deg[i]*CTRL_PERIOD/target_time_;
_ai_ 0:9735e9771ad4 55 }
_ai_ 0:9735e9771ad4 56
_ai_ 0:9735e9771ad4 57 timer.start();
_ai_ 0:9735e9771ad4 58 timer.reset();
_ai_ 0:9735e9771ad4 59 while(1) {
_ai_ 0:9735e9771ad4 60 if(timer.read() >= CTRL_PERIOD) {
_ai_ 0:9735e9771ad4 61 cnt++;
_ai_ 0:9735e9771ad4 62
_ai_ 0:9735e9771ad4 63 for(int i=0; i<2; i++)
_ai_ 0:9735e9771ad4 64 now_deg[i] = min_deg[i]* cnt + ini_deg[i];
_ai_ 0:9735e9771ad4 65
_ai_ 0:9735e9771ad4 66 SM_S.rot(now_deg[0]);
_ai_ 0:9735e9771ad4 67 SM_L.rot(now_deg[1]);
_ai_ 0:9735e9771ad4 68
_ai_ 0:9735e9771ad4 69 timer.reset();
_ai_ 0:9735e9771ad4 70
_ai_ 0:9735e9771ad4 71 }
_ai_ 0:9735e9771ad4 72 if(cnt > motion_num)
_ai_ 0:9735e9771ad4 73 break;
_ai_ 0:9735e9771ad4 74 }
_ai_ 0:9735e9771ad4 75 motion_triger = 0;
_ai_ 0:9735e9771ad4 76 timer.stop();
_ai_ 0:9735e9771ad4 77 }
_ai_ 0:9735e9771ad4 78 }
_ai_ 0:9735e9771ad4 79
_ai_ 0:9735e9771ad4 80 /*--- 全サーボをゼロ度にする ---*/
_ai_ 0:9735e9771ad4 81 void allSMZero(float deg_data_) {
_ai_ 0:9735e9771ad4 82 for(int i=0; i<3; i++)
_ai_ 0:9735e9771ad4 83 now_deg[i] = deg_data_;
_ai_ 0:9735e9771ad4 84
_ai_ 0:9735e9771ad4 85 SM_L.rot(deg_data_);
_ai_ 0:9735e9771ad4 86 SM_R.rot(deg_data_);
_ai_ 0:9735e9771ad4 87 SM_S.rot(deg_data_);
_ai_ 0:9735e9771ad4 88 }
_ai_ 0:9735e9771ad4 89
_ai_ 0:9735e9771ad4 90
_ai_ 0:9735e9771ad4 91 /*--- 全サーボに同じ角度を与える ---*/
_ai_ 0:9735e9771ad4 92 void init_pose_cb(const std_msgs::Float32& data_) {
_ai_ 0:9735e9771ad4 93 allSMZero(data_.data);
_ai_ 0:9735e9771ad4 94
_ai_ 0:9735e9771ad4 95 }
_ai_ 0:9735e9771ad4 96
_ai_ 0:9735e9771ad4 97
_ai_ 0:9735e9771ad4 98 /*--- LRSにそれぞれの角度を与え”非同期”で動作する ---*/ //aa
_ai_ 0:9735e9771ad4 99 void add_rotate_cb(const std_msgs::Float32MultiArray& data_) {
_ai_ 0:9735e9771ad4 100 if(motion_triger == 0) {
_ai_ 0:9735e9771ad4 101 motion_triger = 1;
_ai_ 0:9735e9771ad4 102
_ai_ 0:9735e9771ad4 103
_ai_ 0:9735e9771ad4 104 float target[3]; //目標角度
_ai_ 0:9735e9771ad4 105 float diff_deg[3]; //目標と現在位置の差分
_ai_ 0:9735e9771ad4 106 float min_deg[3]; //最小移動角度
_ai_ 0:9735e9771ad4 107 float ini_deg[3];
_ai_ 0:9735e9771ad4 108 int motion_num = data_.data[3]/CTRL_PERIOD; //移動回数
_ai_ 0:9735e9771ad4 109 int cnt = 0;
_ai_ 0:9735e9771ad4 110
_ai_ 0:9735e9771ad4 111 float target_time = data_.data[3];
_ai_ 0:9735e9771ad4 112
_ai_ 0:9735e9771ad4 113 for(int i=0; i<3; i++) {
_ai_ 0:9735e9771ad4 114 ini_deg[i] = now_deg[i];
_ai_ 0:9735e9771ad4 115 target[i] = data_.data[i] ; //0:S 1:L 2:R
_ai_ 0:9735e9771ad4 116 diff_deg[i] = target[i]-ini_deg[i];
_ai_ 0:9735e9771ad4 117 min_deg[i] = diff_deg[i]*CTRL_PERIOD/target_time;
_ai_ 0:9735e9771ad4 118 }
_ai_ 0:9735e9771ad4 119
_ai_ 0:9735e9771ad4 120 timer.start();
_ai_ 0:9735e9771ad4 121 timer.reset();
_ai_ 0:9735e9771ad4 122 while(1) {
_ai_ 0:9735e9771ad4 123 if(timer.read() >= CTRL_PERIOD) {
_ai_ 0:9735e9771ad4 124 cnt++;
_ai_ 0:9735e9771ad4 125
_ai_ 0:9735e9771ad4 126 for(int i=0; i<3; i++)
_ai_ 0:9735e9771ad4 127 now_deg[i] = min_deg[i] * cnt + ini_deg[i];
_ai_ 0:9735e9771ad4 128
_ai_ 0:9735e9771ad4 129 SM_S.rot(now_deg[0]);
_ai_ 0:9735e9771ad4 130 SM_L.rot(now_deg[1]);
_ai_ 0:9735e9771ad4 131 SM_R.rot(now_deg[2]);
_ai_ 0:9735e9771ad4 132 debug_deg.data = now_deg[0];
_ai_ 0:9735e9771ad4 133 debug_deg_pub.publish(&debug_deg);
_ai_ 0:9735e9771ad4 134
_ai_ 0:9735e9771ad4 135 timer.reset();
_ai_ 0:9735e9771ad4 136 }
_ai_ 0:9735e9771ad4 137
_ai_ 0:9735e9771ad4 138 if(cnt >= motion_num)
_ai_ 0:9735e9771ad4 139 break;
_ai_ 0:9735e9771ad4 140 }
_ai_ 0:9735e9771ad4 141 timer.stop();
_ai_ 0:9735e9771ad4 142 motion_triger = 0;
_ai_ 0:9735e9771ad4 143 }
_ai_ 0:9735e9771ad4 144 }
_ai_ 0:9735e9771ad4 145
_ai_ 0:9735e9771ad4 146 //aaaaa
_ai_ 0:9735e9771ad4 147
_ai_ 0:9735e9771ad4 148 void tar_rotate_cb(const std_msgs::Float32MultiArray& data_) {
_ai_ 0:9735e9771ad4 149 if(motion_triger == 0) {
_ai_ 0:9735e9771ad4 150 motion_triger = 1;
_ai_ 0:9735e9771ad4 151
_ai_ 0:9735e9771ad4 152
_ai_ 0:9735e9771ad4 153 float target[3]; //目標角度
_ai_ 0:9735e9771ad4 154 float diff_deg[3]; //目標と現在位置の差分
_ai_ 0:9735e9771ad4 155 float min_deg[3]; //最小移動角度
_ai_ 0:9735e9771ad4 156 float ini_deg[3];
_ai_ 0:9735e9771ad4 157 int motion_num = data_.data[3]/CTRL_PERIOD; //移動回数
_ai_ 0:9735e9771ad4 158 int cnt = 0;
_ai_ 0:9735e9771ad4 159
_ai_ 0:9735e9771ad4 160 float target_time = data_.data[3];
_ai_ 0:9735e9771ad4 161
_ai_ 0:9735e9771ad4 162 for(int i=0; i<3; i++) {
_ai_ 0:9735e9771ad4 163 ini_deg[i] = now_deg[i];
_ai_ 0:9735e9771ad4 164 target[i] = data_.data[i] ; //0:S 1:L 2:R
_ai_ 0:9735e9771ad4 165 diff_deg[i] = target[i]-ini_deg[i];
_ai_ 0:9735e9771ad4 166 min_deg[i] = diff_deg[i]*CTRL_PERIOD/target_time;
_ai_ 0:9735e9771ad4 167 }
_ai_ 0:9735e9771ad4 168
_ai_ 0:9735e9771ad4 169 timer.start();
_ai_ 0:9735e9771ad4 170 timer.reset();
_ai_ 0:9735e9771ad4 171 while(1) {
_ai_ 0:9735e9771ad4 172 if(timer.read() >= CTRL_PERIOD) {
_ai_ 0:9735e9771ad4 173 cnt++;
_ai_ 0:9735e9771ad4 174
_ai_ 0:9735e9771ad4 175 for(int i=0; i<3; i++)
_ai_ 0:9735e9771ad4 176 now_deg[i] = min_deg[i] * cnt + ini_deg[i];
_ai_ 0:9735e9771ad4 177
_ai_ 0:9735e9771ad4 178 SM_S.rot(now_deg[0]);
_ai_ 0:9735e9771ad4 179 SM_L.rot(now_deg[1]);
_ai_ 0:9735e9771ad4 180 SM_R.rot(now_deg[2]);
_ai_ 0:9735e9771ad4 181
_ai_ 0:9735e9771ad4 182 timer.reset();
_ai_ 0:9735e9771ad4 183 }
_ai_ 0:9735e9771ad4 184
_ai_ 0:9735e9771ad4 185 if(cnt >= motion_num)
_ai_ 0:9735e9771ad4 186 break;
_ai_ 0:9735e9771ad4 187 }
_ai_ 0:9735e9771ad4 188 timer.stop();
_ai_ 0:9735e9771ad4 189 motion_triger = 0;
_ai_ 0:9735e9771ad4 190 }
_ai_ 0:9735e9771ad4 191 }
_ai_ 0:9735e9771ad4 192
_ai_ 0:9735e9771ad4 193 /*--- LSを同期させて動かす ---*/
_ai_ 0:9735e9771ad4 194 void synchro_cb(const std_msgs::Float32MultiArray& data_) {
_ai_ 0:9735e9771ad4 195 synchoroBrushAndSlider(data_.data[0], data_.data[1],data_.data[2]);
_ai_ 0:9735e9771ad4 196 }
_ai_ 0:9735e9771ad4 197
_ai_ 0:9735e9771ad4 198
_ai_ 0:9735e9771ad4 199 ros::Subscriber<std_msgs::Float32> init_pose_rot("/micon_init_pose_rot",&init_pose_cb);
_ai_ 0:9735e9771ad4 200 ros::Subscriber<std_msgs::Float32MultiArray> LRS_add_rot("/micon_LRS_add_rot",add_rotate_cb);
_ai_ 0:9735e9771ad4 201 ros::Subscriber<std_msgs::Float32MultiArray> LRS_tar_rot("/micon_LRS_tar_rot",tar_rotate_cb);
_ai_ 0:9735e9771ad4 202 ros::Subscriber<std_msgs::Float32MultiArray> LS_synchro_rot("/micon_LS_synchro_rot",synchro_cb);
_ai_ 0:9735e9771ad4 203
_ai_ 0:9735e9771ad4 204
_ai_ 0:9735e9771ad4 205 int main() {
_ai_ 0:9735e9771ad4 206 n.getHardware()->setBaud(115200);
_ai_ 0:9735e9771ad4 207 n.initNode();
_ai_ 0:9735e9771ad4 208
_ai_ 0:9735e9771ad4 209 n.advertise(motion_judg);
_ai_ 0:9735e9771ad4 210 n.advertise(LRS_now_deg_pub);
_ai_ 0:9735e9771ad4 211 n.advertise(debug_deg_pub);
_ai_ 0:9735e9771ad4 212 n.subscribe(init_pose_rot);
_ai_ 0:9735e9771ad4 213 n.subscribe(LRS_add_rot);
_ai_ 0:9735e9771ad4 214 n.subscribe(LRS_tar_rot);
_ai_ 0:9735e9771ad4 215 n.subscribe(LS_synchro_rot);
_ai_ 0:9735e9771ad4 216
_ai_ 0:9735e9771ad4 217 allSMZero(0.000001);
_ai_ 0:9735e9771ad4 218
_ai_ 0:9735e9771ad4 219 str_msg.data = "move now";
_ai_ 0:9735e9771ad4 220
_ai_ 0:9735e9771ad4 221 lrs_now_deg.data_length = 3;
_ai_ 0:9735e9771ad4 222 lrs_now_deg.data = (float *)malloc(sizeof(float)*8);
_ai_ 0:9735e9771ad4 223
_ai_ 0:9735e9771ad4 224 while(1) {
_ai_ 0:9735e9771ad4 225 n.spinOnce();
_ai_ 0:9735e9771ad4 226 lrs_now_deg.data[0]=(now_deg[1]);
_ai_ 0:9735e9771ad4 227 lrs_now_deg.data[1]=(now_deg[2]);
_ai_ 0:9735e9771ad4 228 lrs_now_deg.data[2]=(now_deg[0]);
_ai_ 0:9735e9771ad4 229 // motion_judg.publish(&str_msg);
_ai_ 0:9735e9771ad4 230 LRS_now_deg_pub.publish(&lrs_now_deg);
_ai_ 0:9735e9771ad4 231 wait_ms(10);
_ai_ 0:9735e9771ad4 232 }
_ai_ 0:9735e9771ad4 233 }