movement.cpp

00001 #include "EC.h"
00002 #include "R1370P.h"
00003 #include "move4wheel.h"
00004 #include "mbed.h"
00005 #include "math.h"
00006 #include "PathFollowing.h"
00007 #include "movement.h"
00008 #include "manual.h"
00009 #include "can.h"
00010 #include "R6093U.h"
00011 #include "uw.h"
00012 
00013 #define PI 3.141592
00014 
00015 char can_ashileddata[2]= {0};
00016 char can_ashileddata2[8]= {0};
00017 //char can_ashileddata3[2]= {0};
00018 //char can_ashileddata4[2]= {0};
00019 //char can_ashileddata5[2]= {0};
00020 
00021 int can_ashileddata0_0,can_ashileddata0_1,can_ashileddata0_2,can_ashileddata0_3;
00022 
00023 Ec EC2(p16,p15,NC,2048,0.05);
00024 Ec EC1(p18,p17,NC,2048,0.05);
00025 
00026 
00027 Uw uw1(p28);
00028 Uw uw4(p27);
00029 
00030 
00031 Ticker ec_ticker;  //ec角速度計算用ticker
00032 Ticker uw_ticker;  //uw値取得用ticker
00033 
00034 //R1370P gyro(p9,p10);
00035 
00036 R6093U gyro(p9,p10);
00037 
00038 double new_dist1=0,new_dist2=0;
00039 double old_dist1=0,old_dist2=0;
00040 double d_dist1=0,d_dist2=0;  //座標計算用関数
00041 double d_x,d_y;
00042 //現在地X,y座標、現在角度については、PathFollowingでnow_x,now_y,now_angleを定義済
00043 double start_x=0,start_y=0;  //スタート位置
00044 double x_out,y_out,r_out;  //出力値
00045 
00046 int16_t m1=0, m2=0, m3=0, m4=0;  //int16bit = int2byte
00047 
00048 double xy_type,pm_typeX,pm_typeY,x_base,y_base;
00049 
00050 int flag;
00051 
00052 int RL_mode;
00053 
00054 int uw_flag1 = 0,uw_flag2 = 0,uw_flag3 = 0,uw_flag4 = 0;
00055 
00056 ///////////////////機体情報をメンバとする構造体"robo_data"と構造体型変数info(←この変数に各センサーにより求めた機体情報(機体位置/機体角度)を格納する)の宣言/////////////////
00057 
00058 /*「info.(機体情報の種類).(使用センサーの種類)」に各情報を格納する
00059  *状況に応じて、どのセンサーにより算出した情報を信用するかを選択し、その都度now_angle,now_x,now_yに代入する。（何種類かのセンサーの情報を混ぜて使用することも可能）
00060  *(ex)
00061  *info.nowX.enc → エンコーダにより算出した機体位置のx座標
00062  *info.nowY.usw → 超音波センサーにより求めた機体位置のy座標
00063 */
00064 
00065 typedef struct { //使用センサーの種類
00066     double usw;  //超音波センサー
00067     double enc;  //エンコーダ
00068     double gyro; //ジャイロ
00069     //double line;//ラインセンサー
00070 } robo_sensor;
00071 
00072 typedef struct { //機体情報の種類
00073     robo_sensor angle; //←機体角度は超音波センサーやラインセンサーからも算出可能なので一応格納先を用意したが、ジャイロの値を完全に信用してもいいかも
00074     robo_sensor nowX;
00075     robo_sensor nowY;
00076 } robo_data;
00077 
00078 robo_data info= {{0,0,0},{0,0,0},{0,0,0}}; //全てのデータを0に初期化
00079 
00080 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00081 
00082 void UserLoopSetting_sensor()
00083 {
00084 
00085     gyro.initialize();
00086     ec_ticker.attach(&calOmega,0.05);  //0.05秒間隔で角速度を計算
00087     uw_ticker.attach(&cal_uw,0.05);
00088     EC1.setDiameter_mm(70);
00089     EC2.setDiameter_mm(70);  //測定輪半径//後で測定
00090     info.nowX.enc = 457; //初期位置の設定
00091     info.nowY.enc = 457;
00092 }
00093 
00094 void UserLoopSetting_enc_right()
00095 {
00096     info.nowX.enc = 3112; //エンコーダの初期位置の設定(右側フィールド)
00097     info.nowY.enc = 3500;
00098     RL_mode = 0;
00099 }
00100 
00101 void UserLoopSetting_enc_left()
00102 {
00103     info.nowX.enc = -3112; //エンコーダの初期位置の設定(左側フィールド)
00104     info.nowY.enc = 3500;
00105     RL_mode = 1;
00106 }
00107 
00108 void calOmega()  //角速度計算関数
00109 {
00110     EC1.CalOmega();
00111     EC2.CalOmega();
00112 
00113     //usw_data1 = 10 * uw1.get_dist();
00114     ////usw_data2 = 10 * uw2.get_dist();
00115     //usw_data3 = 10 * uw3.get_dist();
00116     ////usw_data4 = 10 * uw4.get_dist();
00117 }
00118 
00119 void cal_uw()   //uw値取得用
00120 {
00121     if(uw_flag1 == 1) {
00122         usw_data1 = 10 * uw1.get_dist();
00123         //printf("uw1 = %f\n\r",usw_data1);
00124     }
00125     if(uw_flag2 == 1) {
00126         //usw_data2 = 10 * uw2.get_dist();
00127     }
00128     if(uw_flag3 == 1) {
00129         //usw_data3 = 10 * uw3.get_dist();
00130     }
00131     if(uw_flag4 == 1) {
00132         usw_data4 = 10 * uw4.get_dist();
00133         //printf("uw4 = %f\n\r",usw_data4);
00134     }
00135 }
00136 
00137 void output(double FL,double BL,double BR,double FR)
00138 {
00139     m1=FL;
00140     m2=BL;
00141     m3=BR;
00142     m4=FR;
00143 }
00144 
00145 void base(double FL,double BL,double BR,double FR,double Max)
00146 //いろんな加算をしても最大OR最小がMaxになるような補正//絶対値が一番でかいやつで除算
00147 //DCモーターならMax=1、マクソンは-4095～4095だからMax=4095にする
00148 {
00149     if(fabs(FL)>=Max||fabs(BL)>=Max||fabs(BR)>=Max||fabs(FR)>=Max) {
00150 
00151         if     (fabs(FL)>=fabs(BL)&&fabs(FL)>=fabs(BR)&&fabs(FL)>=fabs(FR))output(Max*FL/fabs(FL),Max*BL/fabs(FL),Max*BR/fabs(FL),Max*FR/fabs(FL));
00152         else if(fabs(BL)>=fabs(FL)&&fabs(BL)>=fabs(BR)&&fabs(BL)>=fabs(FR))output(Max*FL/fabs(BL),Max*BL/fabs(BL),Max*BR/fabs(BL),Max*FR/fabs(BL));
00153         else if(fabs(BR)>=fabs(FL)&&fabs(BR)>=fabs(BL)&&fabs(BR)>=fabs(FR))output(Max*FL/fabs(BR),Max*BL/fabs(BR),Max*BR/fabs(BR),Max*FR/fabs(BR));
00154         else                                                               output(Max*FL/fabs(FR),Max*BL/fabs(FR),Max*BR/fabs(FR),Max*FR/fabs(FR));
00155     } else {
00156         output(FL,BL,BR,FR);
00157     }
00158 }
00159 
00160 void ashi_led()
00161 {
00162     if(now_angle > -1 && now_angle < 1) {
00163         can_ashileddata0_0 = 1;
00164     } else {
00165         can_ashileddata0_0 = 0;
00166     }
00167 
00168     if(now_angle > 350) {
00169         can_ashileddata0_1 = 1;
00170     } else {
00171         can_ashileddata0_1 = 0;
00172     }
00173 
00174     if(RL_mode == 0) {
00175         if(now_x > 3110 && now_x < 3114) {
00176             can_ashileddata0_2 = 1;
00177         } else {
00178             can_ashileddata0_2 = 0;
00179         }
00180 
00181         if(now_y > 3498 && now_y < 3502) {
00182             can_ashileddata0_3 = 1;
00183         } else {
00184             can_ashileddata0_3 = 0;
00185         }
00186     } else if(RL_mode == 1) {
00187         if(now_x > -3114 && now_x < -3110) {
00188             can_ashileddata0_2 = 1;
00189         } else {
00190             can_ashileddata0_2 = 0;
00191         }
00192 
00193         if(now_y > 3498 && now_y < 3502) {
00194             can_ashileddata0_3 = 1;
00195         } else {
00196             can_ashileddata0_3 = 0;
00197         }
00198     }
00199 
00200     can_ashileddata[0] = (can_ashileddata0_0<<7 | can_ashileddata0_1<<6 | can_ashileddata0_2<<5 | can_ashileddata0_3<<4);
00201 }
00202 
00203 void calc_gyro()
00204 {
00205     //now_angle=gyro.getAngle();  //ジャイロの値読み込み
00206     now_angle = -gyro.getZ_Angle();
00207 }
00208 
00209 void print_gyro()
00210 {
00211     while(1) {
00212         //printf("now_gyro = %f\n\r",-gyro.getAngle());
00213     }
00214 
00215 }
00216 
00217 void calc_xy_enc()  //エンコーダ&ジャイロによる座標計算
00218 {
00219     //now_angle=gyro.getAngle();  //ジャイロの値読み込み
00220     now_angle = -gyro.getZ_Angle();
00221 
00222     new_dist1=EC1.getDistance_mm();
00223     new_dist2=EC2.getDistance_mm();
00224     d_dist1=new_dist1-old_dist1;
00225     d_dist2=new_dist2-old_dist2;
00226     old_dist1=new_dist1;
00227     old_dist2=new_dist2;  //微小時間当たりのエンコーダ読み込み
00228 
00229     d_x=d_dist2*sin(now_angle*PI/180)-d_dist1*cos(now_angle*PI/180);
00230     d_y=d_dist2*cos(now_angle*PI/180)+d_dist1*sin(now_angle*PI/180);  //微小時間毎の座標変化
00231     info.nowX.enc = info.nowX.enc + d_x;
00232     info.nowY.enc = info.nowY.enc - d_y;  //微小時間毎に座標に加算
00233 }
00234 
00235 void set_cond(int t, int px, double bx, int py, double by)   //超音波センサーを使用するときの条件設定関数
00236 {
00237 //引数の詳細は関数"calc_xy_usw"参照
00238 
00239     xy_type = t;
00240 
00241     pm_typeX = px;
00242     x_base = bx;
00243 
00244     pm_typeY = py;
00245     y_base = by;
00246 }
00247 
00248 void calc_xy_usw(double tgt_angle)   //超音波センサーによる座標計算（機体が旋回する場合はこの方法による座標計算は出来ない）
00249 {
00250 //tgt_angle:機体の目標角度（運動初期角度と同じ/今大会では0,90,180のみ）
00251 //xy_type:（0:Y軸平行の壁を読む/1:X軸平行の壁を読む/2:X,Y軸平行の壁を共に読む）
00252 //pm_typeX,pm_typeY:（0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む）
00253 //x_base,y_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標/x軸平行の壁のy座標)
00254 
00255     double R1=414.5,R2=414.5,R3=414.5,R4=414.5; //機体の中心から各超音波センサーが付いている面までの距離
00256     double D1=237.5,D2=237.5,D3=237.5,D4=237.5; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離（時計回りを正とする）
00257 
00258 //    now_angle=gyro.getAngle();  //ジャイロの値読み込み
00259     now_angle = -gyro.getZ_Angle();
00260 
00261     if(tgt_angle==0) {
00262         if((xy_type==0 || xy_type==2) && pm_typeX==0) {
00263 
00264             info.nowX.usw = x_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
00265             uw_flag4 = 1;
00266 
00267         } else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
00268 
00269             info.nowX.usw = x_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
00270             uw_flag3 = 1;
00271 
00272         }
00273         if((xy_type==1 || xy_type==2) && pm_typeY==0) {
00274 
00275             info.nowY.usw = y_base - (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
00276             uw_flag2 = 1;
00277 
00278         } else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
00279 
00280             info.nowY.usw = y_base + (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
00281             uw_flag1 = 1;
00282 
00283         }
00284 
00285     } else if(tgt_angle==90) {
00286         if((xy_type==0 || xy_type==2) && pm_typeX==0) {
00287 
00288             info.nowX.usw = x_base - (usw_data1 + R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180));
00289             uw_flag1 = 1;
00290 
00291         } else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
00292 
00293             info.nowX.usw = x_base + (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180));
00294             uw_flag2 = 1;
00295 
00296         }
00297         if((xy_type==1 || xy_type==2) && pm_typeY==0) {
00298 
00299             info.nowY.usw = y_base - (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180));
00300             uw_flag4 = 1;
00301 
00302         } else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
00303 
00304             info.nowY.usw = y_base + (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180));
00305             uw_flag3 = 1;
00306 
00307         }
00308 
00309     } else if(tgt_angle==180 || tgt_angle==-180) {
00310         if((xy_type==0 || xy_type==2) && pm_typeX==0) {
00311 
00312             info.nowX.usw = x_base - (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180));
00313             uw_flag3 = 1;
00314 
00315         } else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
00316 
00317             info.nowX.usw = x_base + (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180));
00318             uw_flag4 = 1;
00319 
00320         }
00321         if((xy_type==1 || xy_type==2) && pm_typeY==0) {
00322 
00323             info.nowY.usw = y_base - (usw_data1+ R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180));
00324             uw_flag1 = 1;
00325 
00326         } else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
00327 
00328             info.nowY.usw = y_base + (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180));
00329             uw_flag2 = 1;
00330 
00331         }
00332     } else if(tgt_angle==-90) {
00333         if((xy_type==0 || xy_type==2) && pm_typeX==0) {
00334 
00335             info.nowX.usw = x_base - (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180));
00336             uw_flag2 = 1;
00337 
00338         } else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
00339 
00340             info.nowX.usw = x_base + (usw_data1 + R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180));
00341             uw_flag1 = 1;
00342 
00343         }
00344         if((xy_type==1 || xy_type==2) && pm_typeY==0) {
00345 
00346             info.nowY.usw = y_base - (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180));
00347             uw_flag3 = 1;
00348 
00349         } else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
00350 
00351             info.nowY.usw = y_base + (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180));
00352             uw_flag4 = 1;
00353 
00354         }
00355     }
00356 }
00357 
00358 void uwflag_reset()
00359 {
00360     uw_flag1 = 0;
00361     uw_flag2 = 0;
00362     uw_flag3 = 0;
00363     uw_flag4 = 0;
00364 }
00365 
00366 void uwflag_change(int u1,int u2, int u3, int u4)
00367 {
00368     uw_flag1 = u1;
00369     uw_flag2 = u2;
00370     uw_flag3 = u3;
00371     uw_flag4 = u4;
00372 }
00373 
00374 
00375 void calc_xy(double target_angle, double u,double v)
00376 {
00377 //エンコーダにより求めた機体の座標と超音波センサーにより求めた機体の座標を(エンコーダ : 超音波 = u : 1-u / v : 1-v)の割合で混ぜて now_x,now_y に代入する
00378 
00379     calc_xy_enc();
00380     //usw_data1 = 10 * uw1.get_dist();
00381     ///usw_data2 = 10 * uw2.get_dist();
00382     //usw_data3 = 10 * uw3.get_dist();
00383     ///usw_data4 = 10 * uw4.get_dist();
00384 
00385     //printf("uw2 = %f,  uw4 = %f\n\r",usw_data2,usw_data4);
00386 
00387     if(u != 1 || v != 1) {
00388         calc_xy_usw(target_angle);  //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。
00389     }
00390 
00391     now_x = u * info.nowX.enc + (1-u) * info.nowX.usw;
00392     now_y = v * info.nowY.enc + (1-v) * info.nowY.usw;
00393 
00394     /*if(now_x >-1 && now_x <1 && now_y >-1 && now_y <1){  //スタート時の0合わせ用
00395         ec_led = 1;
00396     }else{
00397         ec_led = 0;
00398     }
00399 
00400     if(now_angle >-0.5 && now_angle <0.5){
00401         gyro_led = 1;
00402     }else{
00403         gyro_led = 0;
00404     }*/
00405 }
00406 
00407 void enc_correction(int x_select,int y_select)  //エンコーダの座標を超音波センサの座標で上書き
00408 {
00409 //x_select,y_select → (0:上書きしない/1:上書きする)
00410 
00411     if(x_select == 1) {
00412         info.nowX.enc = info.nowX.usw;
00413     }
00414     if(y_select == 1) {
00415         info.nowY.enc = info.nowY.usw;
00416     }
00417 
00418 }
00419 
00420 void enc_correction2(int x_plot1, int y_plot2) //引数の座標でエンコーダの座標を修正
00421 {
00422     info.nowX.enc = x_plot1;
00423     info.nowY.enc = y_plot2;
00424 }
00425 
00426 //ここからそれぞれのプログラム/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00427 //now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正)
00428 //ジャイロの出力は角度だが三角関数はラジアンとして計算する
00429 //通常の移動＋座標のずれ補正＋機体の角度補正(＋必要に応じさらに別補正)
00430 //ジャイロの仕様上、角度補正をするときに計算式内で角度はそのままよりsinをとったほうがいいかもね
00431 
00432 void purecurve(int type,double u,double v,       //正面を変えずに円弧or楕円を描いて曲がる
00433                double point_x1,double point_y1,
00434                double point_x2,double point_y2,
00435                int theta,
00436                double speed,
00437                double q_p,double q_d,
00438                double r_p,double r_d,
00439                double r_out_max,
00440                double target_angle, double v_base, double q_out_max)
00441 //type:動きの種類（8パターン) point_x1,point_y1=出発地点の座標 point_x2,point_x2=目標地点の座標 theta=plotの間隔(0～90°) speed=速度
00442 {
00443     //-----PathFollowingのパラメーター設定-----//
00444     q_setPDparam(q_p,q_d);  //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
00445     r_setPDparam(r_p,r_d);  //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
00446     set_r_out(r_out_max);  //旋回時の最大出力値設定関数
00447     set_q_out(q_out_max);
00448     set_target_angle(target_angle);  //機体目標角度設定関数
00449 
00450     int s;
00451     int t = 0;
00452     double X,Y;//X=楕円の中心座標、Y=楕円の中心座標
00453     double a,b; //a=楕円のx軸方向の幅の半分,b=楕円のy軸方向の幅の半分
00454     double plotx[(90/theta)+1];  //楕円にとるplotのx座標
00455     double ploty[(90/theta)+1];
00456 
00457     double x_out,y_out,r_out;
00458 
00459     a=fabs(point_x1-point_x2);
00460     b=fabs(point_y1-point_y2);
00461 
00462     switch(type) {
00463 
00464         case 1://→↑移動
00465             X=point_x1;
00466             Y=point_y2;
00467 
00468             for(s=0; s<((90/theta)+1); s++) {
00469                 plotx[s] = X + a * cos(-PI/2 + s * (PI*theta/180));
00470                 ploty[s] = Y + b * sin(-PI/2 + s * (PI*theta/180));
00471                 //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
00472             }
00473             break;
00474 
00475         case 2://↑→移動
00476             X=point_x2;
00477             Y=point_y1;
00478 
00479             for(s=0; s<((90/theta)+1); s++) {
00480                 plotx[s] = X + a * cos(PI - s * (PI*theta/180));
00481                 ploty[s] = Y + b * sin(PI - s * (PI*theta/180));
00482                 //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
00483             }
00484             break;
00485 
00486         case 3://↑←移動
00487             X=point_x2;
00488             Y=point_y1;
00489 
00490             for(s=0; s<((90/theta)+1); s++) {
00491                 plotx[s] = X + a * cos(s * (PI*theta/180));
00492                 ploty[s] = Y + b * sin(s * (PI*theta/180));
00493                 //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
00494             }
00495             break;
00496 
00497         case 4://←↑移動
00498             X=point_x1;
00499             Y=point_y2;
00500 
00501             for(s=0; s<((90/theta)+1); s++) {
00502                 plotx[s] = X + a * cos(-PI/2 - s * (PI*theta/180));
00503                 ploty[s] = Y + b * sin(-PI/2 - s * (PI*theta/180));
00504                 //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
00505             }
00506             break;
00507 
00508         case 5://←↓移動
00509             X=point_x1;
00510             Y=point_y2;
00511 
00512             for(s=0; s<((90/theta)+1); s++) {
00513                 plotx[s] = X + a * cos(PI/2 + s * (PI*theta/180));
00514                 ploty[s] = Y + b * sin(PI/2 + s * (PI*theta/180));
00515                 //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
00516             }
00517             break;
00518 
00519         case 6://↓←移動
00520             X=point_x2;
00521             Y=point_y1;
00522 
00523             for(s=0; s<((90/theta)+1); s++) {
00524                 plotx[s] = X + a * cos(-s * (PI*theta/180));
00525                 ploty[s] = Y + b * sin(-s * (PI*theta/180));
00526                 //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
00527             }
00528             break;
00529 
00530         case 7://↓→移動
00531             X=point_x2;
00532             Y=point_y1;
00533 
00534             for(s=0; s<((90/theta)+1); s++) {
00535                 plotx[s] = X + a * cos(PI + s * (PI*theta/180));
00536                 ploty[s] = Y + b * sin(PI + s * (PI*theta/180));
00537                 //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
00538             }
00539             break;
00540 
00541         case 8://→↓移動
00542             X=point_x1;
00543             Y=point_y2;
00544 
00545             for(s=0; s<((90/theta)+1); s++) {
00546                 plotx[s] = X + a * cos(PI/2 - s * (PI*theta/180));
00547                 ploty[s] = Y + b * sin(PI/2 - s * (PI*theta/180));
00548                 //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
00549             }
00550             break;
00551     }
00552 
00553     while(1) {
00554 
00555         if(id1_value[0] != 1)break;
00556         if(id1_value[6] != flag)break;
00557 
00558         calc_xy(target_angle,u,v);
00559 
00560         XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out,speed,speed);
00561         CalMotorOut(x_out,y_out,r_out);
00562         //debug_printf("t=%d now_x=%f now_y=%f x_out=%f y_out=%f\n\r",t,now_x,now_y,x_out,y_out);
00563 
00564         base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),v_base);  //m1～m4に代入
00565         //debug_printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
00566 
00567         if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
00568 
00569 //        MaxonControl(m1,m2,m3,m4);  //出力
00570 //        debug_printf("t=%d m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",t,m1,m2,m3,m4,now_x,now_y,now_angle);
00571 
00572         if(t == (90/theta))break;
00573     }
00574 }
00575 
00576 void gogo_straight(double u,double v,                //直線運動プログラム
00577                    double x1_point,double y1_point,
00578                    double x2_point,double y2_point,
00579                    double speed1,double speed2,
00580                    double q_p,double q_d,
00581                    double r_p,double r_d,
00582                    double r_out_max,
00583                    double target_angle,double v_base, double q_out_max)
00584 //引数:出発地点の座標（x,y）、目標地点の座標(x,y)、初速度（speed1）、目標速度（speed2）//speed1=speed2 のとき等速運動
00585 {
00586     //-----PathFollowingのパラメーター設定-----//
00587     q_setPDparam(q_p,q_d);  //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
00588     r_setPDparam(r_p,r_d);  //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
00589     set_r_out(r_out_max);  //旋回時の最大出力値設定関数
00590     set_q_out(q_out_max);
00591     set_target_angle(target_angle);  //機体目標角度設定関数
00592 
00593     while (1) {
00594 
00595         if(id1_value[0] != 1)break;
00596         if(id1_value[6] != flag)break;
00597 
00598         calc_xy(target_angle,u,v);
00599 
00600         XYRmotorout(x1_point,y1_point,x2_point,y2_point,&x_out,&y_out,&r_out,speed1,speed2);
00601         //printf("n_x = %f, n_y = %f,n_angle = %f, t_x = %f, t_y = %f, t_angle = %f, x_out=%lf, y_out=%lf, r_out=%lf\n\r",now_x,now_y,now_angle,x2_point,y2_point,target_angle,x_out, y_out,r_out);
00602 
00603         CalMotorOut(x_out,y_out,r_out);
00604         //printf("out1=%lf, out2=%lf, out3=%lf, out4=%lf\n",GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
00605 
00606         base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),v_base);
00607         //printf("m1=%d, m2=%d, m3=%d, m4=%d\r\n",m_1,m_2,m_3,m_4);
00608 
00609 //        MaxonControl(m1,m2,m3,m4);
00610 //        debug_printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle);
00611         printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle);
00612         //printf("usw2 = %f  usw4 = %f x=%f y=%f angle=%f\n\r",usw_data2,usw_data4,now_x,now_y,now_angle);
00613 
00614         if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0)break;
00615     }
00616 }
00617 
00618 
00619 
00620 double spline_base(int i, int k, double t, int nv[]) //スプライン基底関数を求める関数
00621 {
00622     // i:0~(制御点の個数-1)
00623     // k:スプライト曲線の次元
00624     // t:0~(ノットベクトルの最大値)
00625     // nv[]:ノットベクトル
00626     double w1 = 0.0, w2 = 0.0;
00627     if (k == 1) {
00628         if (t > nv[i] && t <= nv[i + 1])
00629             return 1.0;
00630         else
00631             return 0.0;
00632     } else {
00633         if ((nv[i + k] - nv[i + 1]) != 0) {
00634             w1 = ((nv[i + k] - t) / (nv[i + k] - nv[i + 1])) * spline_base(i + 1, k - 1, t, nv);
00635             //printf("%f\n\r",w1);
00636         }
00637         if ((nv[i + k - 1] - nv[i]) != 0) {
00638             w2 = ((t - nv[i]) / (nv[i + k - 1] - nv[i])) * spline_base(i, k - 1, t, nv);
00639             //printf("%f\n\r",w2);
00640         }
00641         return (w1 + w2);
00642     }
00643 }
00644 
00645 
00646 void spline_move(double u, double v,
00647                  double st_x,double st_y,double end_x,double end_y,
00648                  double cont1_x,double cont1_y,double cont2_x,double cont2_y,
00649                  double st_speed, double end_speed,
00650                  double q_p,double q_d,
00651                  double r_p,double r_d,
00652                  double r_out_max,
00653                  double target_angle, double v_base, double q_out_max, int num)
00654 {
00655     double dx, dy, dr;
00656     int nt[] = {0, 0, 0, 1, 2, 2, 2}; //ノットベクトル
00657     //dr = (end_angle - st_angle) / num;
00658     int ds = (end_speed - st_speed) / num;
00659 
00660     //-----PathFollowingのパラメーター設定-----//
00661     q_setPDparam(q_p,q_d);  //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
00662     r_setPDparam(r_p,r_d);  //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
00663     set_r_out(r_out_max);  //旋回時の最大出力値設定関数
00664     set_q_out(q_out_max);
00665     set_target_angle(target_angle);  //機体目標角度設定関数
00666 
00667     double plotx[num + 1]; //楕円にとるplotのx座標
00668     double ploty[num + 1];
00669     double value_t;
00670     int i, j;
00671     int t = 0;
00672     // for(i = 0; i < 7; i++){
00673     //  printf("not_V = %d\n\r",nt[i]);
00674     // }
00675     for (i = 0; i < num + 1; i++) {
00676         plotx[i] = 0.0;
00677         ploty[i] = 0.0;
00678     }
00679     printf("{\n");
00680     for (i = 0; i < num + 1; i++) {
00681         value_t = (double)2 * i / num;
00682         for (j = 0; j < 4; j++) {
00683             if (j == 0) {
00684                 plotx[i] += st_x * spline_base(j, 3, value_t, nt);
00685                 ploty[i] += st_y * spline_base(j, 3, value_t, nt);
00686             } else if (j == 1) {
00687                 plotx[i] += cont1_x * spline_base(j, 3, value_t, nt);
00688                 ploty[i] += cont1_y * spline_base(j, 3, value_t, nt);
00689             } else if (j == 2) {
00690                 plotx[i] += cont2_x * spline_base(j, 3, value_t, nt);
00691                 ploty[i] += cont2_y * spline_base(j, 3, value_t, nt);
00692             } else if (j == 3) {
00693                 plotx[i] += end_x * spline_base(j, 3, value_t, nt);
00694                 ploty[i] += end_y * spline_base(j, 3, value_t, nt);
00695             }
00696         }
00697         //printf("plot_x = %f, plot_y = %f\n\r", plotx[i], ploty[i]);
00698     }
00699     while(1) {
00700 
00701         if(id1_value[0] != 1)break;
00702         if(id1_value[6] != flag)break;
00703 
00704         calc_xy(target_angle,u,v);
00705 
00706         XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out,st_speed+ds*t,st_speed+ds*(t+1));
00707         CalMotorOut(x_out,y_out,r_out);
00708         //debug_printf("t=%d now_x=%f now_y=%f x_out=%f y_out=%f\n\r",t,now_x,now_y,x_out,y_out);
00709 
00710         base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),v_base);  //m1～m4に代入
00711         //debug_printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
00712 
00713         if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
00714 
00715 //        MaxonControl(m1,m2,m3,m4);  //出力
00716 //        debug_printf("t=%d m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",t,m1,m2,m3,m4,now_x,now_y,now_angle);
00717         printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle);
00718 
00719         if(t == num)break;
00720     }
00721 
00722 }
00723 
00724 
00725 
00726 
00727 /*void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v)   //位置補正(使用前にMaxonControl(0,0,0,0)を入れる)
00728 {
00729 
00730     double r, R=10;  // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
00731     double out;
00732 
00733     calc_xy(tgt_angle, u, v);
00734 
00735     while(1) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
00736         gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,200,50,5,0.1,10,0.1,500,tgt_angle);
00737         MaxonControl(0,0,0,0);
00738 
00739         calc_xy(tgt_angle, u, v);
00740 
00741         r=hypot(now_x - tgt_x, now_y - tgt_y);
00742 
00743         if(r < R) break;
00744         if(id1_value[0] != 1)break;
00745     }
00746 
00747     while(1) {
00748 
00749         calc_gyro();
00750 
00751         out = 10 * (tgt_angle - now_angle);
00752 
00753         if(out > 300) {  //0～179°のときは時計回りに回転
00754             MaxonControl(300,300,300,300);
00755         } else if(out < -300) {
00756             MaxonControl(-300,-300,-300,-300);
00757         } else if(out <= 300 && out > -300) {
00758             MaxonControl(out,out,out,out);
00759         }
00760 
00761         if(tgt_angle - 0.5 < now_angle && now_angle < tgt_angle + 0.5) break;  //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
00762         if(id1_value[0] != 1)break;
00763     }
00764     MaxonControl(0,0,0,0);
00765 }*/
00766 
00767 void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v, double v_base)   //改良版 位置補正(使用前にMaxonControl(0,0,0,0)を入れる)
00768 {
00769 //距離に比例させて補正初速度を増加させる。（最大速度を設定しそれ以上は出ないようにする）
00770 
00771     double first_speed, first_speed50 = 10, last_speed = 10, Max_speed = 500, speed5 = 20;
00772     double r, R=50;  // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
00773     double out;
00774 
00775     calc_xy(tgt_angle, u, v);
00776 
00777     //r = hypot(now_x - tgt_x, now_y - tgt_y);
00778 
00779     while(1) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
00780         //printf("col\n\n\n");
00781         if(id1_value[0] != 1)break;
00782         if(id1_value[6] != flag)break;
00783 
00784         //first_speed = first_speed50 * r / 50;
00785 
00786         /*if(first_speed > Max_speed){
00787             gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,Max_speed,Max_speed,5,0.1,10,0.1,500,tgt_angle, v_base);
00788         }else{
00789             gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,first_speed,last_speed,5,0.1,10,0.1,500,tgt_angle);
00790         }*/
00791 
00792         //gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,first_speed50,last_speed,5,0.1,10,0.1,500,tgt_angle);
00793 
00794         int diff_sm = hypot(now_x-tgt_x,now_y-tgt_y);
00795 
00796         int f_speed = diff_sm / 5 * (speed5 - last_speed);
00797         gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,f_speed,last_speed,0.5,0.05,5,0.05,20,tgt_angle, v_base, 30);
00798         //gogo_straight(1,1,0,0,200,0,50,500,5,0.1,10,0.1,50,0);
00799         //gogo_straight(u,v,now_x,now_y,0,100,first_speed50,last_speed,5,0.1,10,0.1,500,tgt_angle);
00800 
00801 //      MaxonControl(0,0,0,0);
00802         m1 = 0;
00803         m2 = 0;
00804         m3 = 0;
00805         m4 = 0;
00806 
00807         calc_xy(tgt_angle, u, v);
00808 
00809         r=hypot(now_x - tgt_x, now_y - tgt_y);
00810 
00811         if(r < R) break;
00812     }
00813 
00814     while(1) {
00815 
00816         if(id1_value[0] != 1)break;
00817         if(id1_value[6] != flag)break;
00818 
00819         //calc_gyro();
00820 //        now_angle=gyro.getAngle();
00821         now_angle = -gyro.getZ_Angle();
00822         if(tgt_angle - 1 < now_angle && now_angle < tgt_angle + 1) break;  //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
00823         else if(now_angle > tgt_angle + 1)out = 5 * (tgt_angle - now_angle + 1);
00824         else if(tgt_angle - 1 > now_angle)out = 5 * (tgt_angle - now_angle - 1);
00825 
00826         printf("angle = %f  out = %f\n\r",now_angle,out);
00827 
00828         if(out > 100) {  //0～179°のときは時計回りに回転
00829 //            MaxonControl(-300,-300,-300,-300);
00830             m1 = -100;
00831             m2 = -100;
00832             m3 = -100;
00833             m4 = -100;
00834 
00835         } else if(out < -100) {
00836 //            MaxonControl(300,300,300,300);
00837             m1 = 100;
00838             m2 = 100;
00839             m3 = 100;
00840             m4 = 100;
00841         } else if(out <= 100 && out > -100) {
00842 //            MaxonControl(-out,-out,-out,-out);
00843             m1 = -out;
00844             m2 = -out;
00845             m3 = -out;
00846             m4 = -out;
00847         }
00848 
00849 
00850     }
00851 //    MaxonControl(0,0,0,0);
00852     m1 = 0;
00853     m2 = 0;
00854     m3 = 0;
00855     m4 = 0;
00856 }
00857 
00858 void mt_stop()
00859 {
00860     m1 = 0;
00861     m2 = 0;
00862     m3 = 0;
00863     m4 = 0;
00864     printf("motor stop\n\r");
00865 }