main.cpp

00001 #include "mbed.h"
00002 #include <ros.h>
00003 #include <std_msgs/String.h>
00004 #include <std_msgs/Float32MultiArray.h>
00005 #include <std_msgs/Float32.h>
00006 #include "ServoMotor.h"
00007 
00008 //システム関連
00009 Timer   timer;
00010 float CTRL_PERIOD   = 0.02;     //制御周期[s]
00011 float READ_TIME     = 0.0 ;     //時間読み取り
00012 
00013 //サーボモータをPWMでうごかすための設定(futaba RS204MDはPWMで動かす時は0~288[deg]が動作範囲)
00014 ServoMotor      SM_R( D3, 0.0, 288.0, 0.02, 0.00056, 0.00248); //右指ブラシモータ
00015 ServoMotor      SM_L( D7, 0.0, 288.0, 0.02, 0.00248, 0.00056); //左指ブラシモータ
00016 ServoMotor      SM_S(D10, 0.0, 288.0, 0.02, 0.00056, 0.00248); //左指部
00017 
00018 ros::NodeHandle n;
00019 
00020 std_msgs::String                str_msg;
00021 std_msgs::Float32               debug_deg;
00022 std_msgs::Float32MultiArray     lrs_now_deg;
00023 
00024 ros::Publisher motion_judg("/micon_LRS_motion_judg", &str_msg);
00025 ros::Publisher LRS_now_deg_pub("/micon_LRS_now_deg", &lrs_now_deg);
00026 ros::Publisher debug_deg_pub("/debug_deg",&debug_deg);
00027 
00028 //モータ計算関連
00029 void synchoroBrushAndSlider(float deg_L_, float deg_S_, int num_);   //モータ微小目標角度生成関数 引数1:目標角度 引数2:分割数
00030 void allSMZero(float deg);                                          //モータの角度を全てdeg度にする
00031 
00032 float now_deg[3]= {0.0,0.0,0.0};   //0:S  1:L  2:R
00033 
00034 
00035 /*--- 左指ブラシとスライダを同期して動かす関数 ---*/
00036 bool motion_triger = 0;
00037 void synchoroBrushAndSlider(float deg_L_, float deg_S_, float target_time_)
00038 {
00039     if(motion_triger == 0) {
00040         motion_triger = 1;
00041 
00042         float target[2];        //目標角度  0:S  1:L
00043         float diff_deg[2];      //目標と現在位置の差分
00044         float min_deg[2];       //最小移動角度
00045         float ini_deg[2];
00046         int motion_num = target_time_/CTRL_PERIOD;  //移動回数
00047         int cnt = 0;
00048 
00049         target[0]=deg_S_;
00050         target[1]=deg_L_;
00051         for(int i=0; i<2; i++) {
00052             ini_deg[i] = now_deg[i];
00053             diff_deg[i] = target[i]-ini_deg[i];
00054             min_deg[i] = diff_deg[i]*CTRL_PERIOD/target_time_;
00055         }
00056 
00057         timer.start();
00058         timer.reset();
00059         while(1) {
00060             if(timer.read() >= CTRL_PERIOD) {
00061                 cnt++;
00062 
00063                 for(int i=0; i<2; i++) 
00064                     now_deg[i] = min_deg[i]* cnt + ini_deg[i];
00065 
00066                 SM_S.rot(now_deg[0]);
00067                 SM_L.rot(now_deg[1]);
00068 
00069                 timer.reset();
00070                     
00071             }
00072             if(cnt > motion_num)
00073                 break;
00074         }
00075         motion_triger = 0;
00076         timer.stop();
00077     }
00078 }
00079 
00080 /*--- 全サーボをゼロ度にする ---*/
00081 void allSMZero(float deg_data_) {
00082     for(int i=0; i<3; i++)
00083         now_deg[i] = deg_data_;
00084 
00085     SM_L.rot(deg_data_);
00086     SM_R.rot(deg_data_);
00087     SM_S.rot(deg_data_);
00088 }
00089 
00090 
00091 /*--- 全サーボに同じ角度を与える ---*/
00092 void init_pose_cb(const std_msgs::Float32& data_) {
00093     allSMZero(data_.data);
00094 
00095 }
00096 
00097 
00098 /*--- LRSにそれぞれの角度を与え”非同期”で動作する ---*/  //aa
00099 void add_rotate_cb(const std_msgs::Float32MultiArray& data_) {
00100     if(motion_triger == 0) {
00101         motion_triger = 1;
00102 
00103 
00104         float target[3];        //目標角度
00105         float diff_deg[3];      //目標と現在位置の差分
00106         float min_deg[3];       //最小移動角度
00107         float ini_deg[3];
00108         int motion_num = data_.data[3]/CTRL_PERIOD;  //移動回数
00109         int cnt = 0;
00110 
00111         float target_time  = data_.data[3];
00112 
00113         for(int i=0; i<3; i++) {
00114             ini_deg[i] = now_deg[i];
00115             target[i]   = data_.data[i] ;               //0:S  1:L  2:R
00116             diff_deg[i] = target[i]-ini_deg[i];
00117             min_deg[i]  = diff_deg[i]*CTRL_PERIOD/target_time;
00118         }
00119 
00120         timer.start();
00121         timer.reset();
00122         while(1) {
00123             if(timer.read() >= CTRL_PERIOD) {
00124                 cnt++;
00125                 
00126                 for(int i=0; i<3; i++)
00127                     now_deg[i] = min_deg[i] * cnt + ini_deg[i];
00128 
00129                 SM_S.rot(now_deg[0]);
00130                 SM_L.rot(now_deg[1]);
00131                 SM_R.rot(now_deg[2]);
00132                 debug_deg.data = now_deg[0];
00133                 debug_deg_pub.publish(&debug_deg);
00134             
00135                 timer.reset();
00136             }
00137 
00138             if(cnt >= motion_num)
00139                 break;
00140         }
00141         timer.stop();
00142         motion_triger = 0;
00143     }
00144 }
00145 
00146 //aaaaa
00147 
00148 void tar_rotate_cb(const std_msgs::Float32MultiArray& data_) {
00149     if(motion_triger == 0) {
00150         motion_triger = 1;
00151 
00152 
00153         float target[3];        //目標角度
00154         float diff_deg[3];      //目標と現在位置の差分
00155         float min_deg[3];       //最小移動角度
00156         float ini_deg[3];
00157         int motion_num = data_.data[3]/CTRL_PERIOD;  //移動回数
00158         int cnt = 0;
00159 
00160         float target_time  = data_.data[3];
00161 
00162         for(int i=0; i<3; i++) {
00163             ini_deg[i] = now_deg[i];
00164             target[i]   = data_.data[i] ;               //0:S  1:L  2:R
00165             diff_deg[i] = target[i]-ini_deg[i];
00166             min_deg[i]  = diff_deg[i]*CTRL_PERIOD/target_time;
00167         }
00168 
00169         timer.start();
00170         timer.reset();
00171         while(1) {
00172             if(timer.read() >= CTRL_PERIOD) {
00173                 cnt++;
00174                 
00175                 for(int i=0; i<3; i++)
00176                     now_deg[i] = min_deg[i] * cnt + ini_deg[i];
00177 
00178                 SM_S.rot(now_deg[0]);
00179                 SM_L.rot(now_deg[1]);
00180                 SM_R.rot(now_deg[2]);
00181 
00182                 timer.reset();
00183             }
00184 
00185             if(cnt >= motion_num)
00186                 break;
00187         }
00188         timer.stop();
00189         motion_triger = 0;
00190     }
00191 }
00192 
00193 /*--- LSを同期させて動かす ---*/
00194 void synchro_cb(const std_msgs::Float32MultiArray& data_) {
00195     synchoroBrushAndSlider(data_.data[0], data_.data[1],data_.data[2]);
00196 }
00197 
00198 
00199 ros::Subscriber<std_msgs::Float32> init_pose_rot("/micon_init_pose_rot",&init_pose_cb);
00200 ros::Subscriber<std_msgs::Float32MultiArray> LRS_add_rot("/micon_LRS_add_rot",add_rotate_cb);
00201 ros::Subscriber<std_msgs::Float32MultiArray> LRS_tar_rot("/micon_LRS_tar_rot",tar_rotate_cb);
00202 ros::Subscriber<std_msgs::Float32MultiArray> LS_synchro_rot("/micon_LS_synchro_rot",synchro_cb);
00203 
00204 
00205 int main() {
00206     n.getHardware()->setBaud(115200);
00207     n.initNode();
00208 
00209     n.advertise(motion_judg);
00210     n.advertise(LRS_now_deg_pub);
00211     n.advertise(debug_deg_pub);
00212     n.subscribe(init_pose_rot);
00213     n.subscribe(LRS_add_rot);
00214     n.subscribe(LRS_tar_rot);
00215     n.subscribe(LS_synchro_rot);
00216 
00217     allSMZero(0.000001);
00218 
00219     str_msg.data = "move now";
00220 
00221     lrs_now_deg.data_length = 3;
00222     lrs_now_deg.data = (float *)malloc(sizeof(float)*8);
00223 
00224     while(1) {
00225         n.spinOnce();
00226         lrs_now_deg.data[0]=(now_deg[1]);
00227         lrs_now_deg.data[1]=(now_deg[2]);
00228         lrs_now_deg.data[2]=(now_deg[0]);
00229         //   motion_judg.publish(&str_msg);
00230         LRS_now_deg_pub.publish(&lrs_now_deg);
00231         wait_ms(10);
00232     }
00233 }