PathFollowing.cpp

00001 #include "PathFollowing.h"
00002 #include "mbed.h"
00003 #include "math.h"
00004 
00005 double p_out,r_out_max;
00006 double Kvq_p,Kvq_d,Kvr_p,Kvr_d;
00007 double diff_old,diffangle,diffangle_old;
00008 double out_dutyQ,out_dutyR;
00009 double now_angle,target_angle;
00010 double now_timeQ,old_timeQ,now_timeR,old_timeR;
00011 double now_x, now_y;
00012 double diff_st,diff_tgt,diff_st_tgt,p_param;
00013 double usw_data1,usw_data2,usw_data3,usw_data4;
00014 
00015 Timer timer;
00016 
00017 //初期座標：A, 目標座標：B、機体位置：C、点Cから直線ABに下ろした垂線の足：H
00018 void XYRmotorout(double plot_x1, double plot_y1, double plot_x2, double plot_y2, double *ad_x_out, double *ad_y_out, double *ad_r_out, double speed1, double speed2 )  //プログラム使用時、now_x,now_yはグローバル変数として定義する必要あり
00019 //plot_x1,plot_y1:出発地点の座標
00020 //plot_x2,plot_y2:目標地点の座標
00021 //speed1:初期速度
00022 //speed2:目標速度
00023 {
00024     double Vector_P[2] = {(plot_x2 - plot_x1), (plot_y2 - plot_y1)}; //ベクトルAB
00025     double A_Vector_P = hypot(Vector_P[0], Vector_P[1]); //ベクトルABの大きさ（hypot(a,b)で√（a^2+b^2）を計算できる <math.h>））
00026     double UnitVector_P[2] = {Vector_P[0]/A_Vector_P, Vector_P[1]/A_Vector_P}; //ベクトルABの単位ベクトル
00027     double UnitVector_Q[2] = {UnitVector_P[1], -UnitVector_P[0]}; //ベクトルCHの単位ベクトル
00028     double Vector_R[2] = {(now_x - plot_x1), (now_y - plot_y1)}; //ベクトルAC
00029     double diff = UnitVector_P[0]*Vector_R[1] - UnitVector_P[1]*Vector_R[0]; //機体位置と直線ABの距離（外積を用いて計算）
00030 
00031 
00032     //double VectorOut_P[2]= {0}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解*/
00033 
00034 ///////////////////＜XYRmotorout関数内＞以下、ベクトルABに垂直な方向の誤差を埋めるPD制御（ベクトルABに垂直方向の出力を求め、x軸方向、y軸方向の出力に分解）//////////////////////
00035 
00036     timer.start();
00037     now_timeQ=timer.read();
00038     out_dutyQ=Kvq_p*diff+Kvq_d*(diff-diff_old)/(now_timeQ-old_timeQ); //ベクトルABに垂直方向の出力を決定
00039     diff_old=diff;
00040 
00041     if(out_dutyQ>500)out_dutyQ=500;
00042     if(out_dutyQ<-500)out_dutyQ=-500;
00043 
00044     old_timeQ=now_timeQ;
00045 
00046     double VectorOut_Q[2] = {out_dutyQ*UnitVector_Q[0], out_dutyQ*UnitVector_Q[1]}; //ベクトルABに垂直方向の出力をx軸方向、y軸方向の出力に分解
00047 
00048 ///////////////////////////////<XYRmotorout関数内＞以下、機体角度と目標角度の誤差を埋めるPD制御（旋回のための出力値を決定）//////////////////////////////////
00049 
00050     now_timeR=timer.read();
00051     diffangle=target_angle-now_angle;
00052     out_dutyR=-(Kvr_p*diffangle+Kvr_d*(diffangle-diffangle_old)/(now_timeR-old_timeR));
00053     diffangle_old=diffangle;
00054 
00055     if(out_dutyR>r_out_max)out_dutyR=r_out_max;
00056     if(out_dutyR<-r_out_max)out_dutyR=-r_out_max;
00057 
00058     old_timeR=now_timeR;
00059 
00060 //////////////////////////<XYRmotorout関数内＞以下、x軸方向、y軸方向、旋回の出力をそれぞれad_x_out,ad_y_out,ad_r_outの指すアドレスに書き込む/////////////////////////////
00061 ////////////////////////////////////////////その際、x軸方向、y軸方向の出力はフィールドの座標系から機体の座標系に変換する。///////////////////////////////////////////////
00062 
00063     diff_st = hypot(now_x-plot_x1,now_y-plot_y1); //出発座標と機体の位置の距離
00064     diff_tgt = hypot(now_x - plot_x2, now_y - plot_y2); //機体の位置と目標座標の距離
00065     diff_st_tgt = hypot(plot_x1-plot_x2,plot_y1-plot_y2); //出発座標と目標座標の距離
00066 
00067     if(speed1 == speed2) { //等速移動
00068 
00069         double VectorOut_P[2] = {speed1*UnitVector_P[0], speed1*UnitVector_P[1]};
00070 
00071         *ad_x_out = (VectorOut_P[0]+VectorOut_Q[0])*cos(-now_angle*3.141592/180)-(VectorOut_P[1]+VectorOut_Q[1])*sin(-now_angle*3.141592/180);
00072         *ad_y_out = (VectorOut_P[0]+VectorOut_Q[0])*sin(-now_angle*3.141592/180)+(VectorOut_P[1]+VectorOut_Q[1])*cos(-now_angle*3.141592/180);
00073         *ad_r_out = out_dutyR;
00074 
00075     } else if(speed2 == 0) { //減速移動(目標速度が０)→ベクトルABに垂直な方向の出力にもP制御をかける。
00076 
00077         double VectorOut_P[2] = {speed1*UnitVector_P[0], speed1*UnitVector_P[1]};
00078 
00079         if(diff_tgt > diff_st_tgt) {
00080             diff_tgt = diff_st_tgt;
00081         }
00082 
00083         p_param=(diff_tgt/diff_st_tgt);
00084 
00085         *ad_x_out = p_param*((VectorOut_P[0]+VectorOut_Q[0])*cos(-now_angle*3.141592/180)-(VectorOut_P[1]+VectorOut_Q[1])*sin(-now_angle*3.141592/180));
00086         *ad_y_out = p_param*((VectorOut_P[0]+VectorOut_Q[0])*sin(-now_angle*3.141592/180)+(VectorOut_P[1]+VectorOut_Q[1])*cos(-now_angle*3.141592/180));
00087         *ad_r_out = out_dutyR;
00088 
00089     } else if(speed1 > speed2) { //減速移動（目標速度が０でない）
00090 
00091         if(diff_tgt > diff_st_tgt) {
00092             diff_tgt = diff_st_tgt;
00093         }
00094         
00095         p_param=(diff_tgt/diff_st_tgt);
00096 
00097         double speed3 = speed2 + (speed1-speed2)*p_param;
00098 
00099         double VectorOut_P[2] = {speed3*UnitVector_P[0], speed3*UnitVector_P[1]};
00100          
00101         *ad_x_out = (VectorOut_P[0]+VectorOut_Q[0])*cos(-now_angle*3.141592/180)-(VectorOut_P[1]+VectorOut_Q[1])*sin(-now_angle*3.141592/180);
00102         *ad_y_out = (VectorOut_P[0]+VectorOut_Q[0])*sin(-now_angle*3.141592/180)+(VectorOut_P[1]+VectorOut_Q[1])*cos(-now_angle*3.141592/180);
00103         *ad_r_out = out_dutyR;
00104 
00105     } else if(speed1 < speed2) { //加速移動(speed1)
00106 
00107         if(diff_st > diff_st_tgt) {
00108             diff_st = diff_st_tgt;
00109         }
00110              
00111         p_param=(diff_st/diff_st_tgt);
00112 
00113         double speed4 = speed1 + (speed2-speed1)*p_param;
00114 
00115         double VectorOut_P[2] = {speed4*UnitVector_P[0], speed4*UnitVector_P[1]};
00116          
00117         *ad_x_out = (VectorOut_P[0]+VectorOut_Q[0])*cos(-now_angle*3.141592/180)-(VectorOut_P[1]+VectorOut_Q[1])*sin(-now_angle*3.141592/180);
00118         *ad_y_out = (VectorOut_P[0]+VectorOut_Q[0])*sin(-now_angle*3.141592/180)+(VectorOut_P[1]+VectorOut_Q[1])*cos(-now_angle*3.141592/180);
00119         *ad_r_out = out_dutyR;
00120     }
00121 }
00122 
00123 ////////////////////////////////////////////////////////////<XYRmotorout関数は以上>////////////////////////////////////////////////////////////////
00124 
00125 
00126 /*void set_p_out(double p)  //ベクトルABに平行方向の出力値設定関数
00127 {
00128     p_out = p;
00129 }*/
00130 
00131 void q_setPDparam(double q_p,double q_d)  //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
00132 {
00133     Kvq_p=q_p;
00134     Kvq_d=q_d;
00135 }
00136 
00137 void r_setPDparam(double r_p,double r_d)  //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
00138 {
00139     Kvr_p=r_p;
00140     Kvr_d=r_d;
00141 }
00142 
00143 void set_r_out(double r)  //旋回時の最大出力値設定関数
00144 {
00145     r_out_max = r;
00146 }
00147 
00148 void set_target_angle(double t)  //機体の目標角度設定関数
00149 {
00150     target_angle = t;
00151 }