PathFollowing1.cpp

00001 #include <PathFollowing1.h>
00002 #include <mbed.h>
00003 #include <math.h>
00004 
00005 
00006 namespace{
00007     R1370P gyro(PC_6,PC_7);
00008     }
00009 
00010 //初期座標：A, 目標座標：B、機体位置：C、点Cから直線ABに下ろした垂線の足：H
00011 void pathfollowing1::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){ //プログラム使用時、now_x,now_yはグローバル変数として定義する必要あり
00012 
00013     double Vector_P[2] = {(plot_x2 - plot_x1), (plot_y2 - plot_y1)}; //ベクトルAB
00014     double A_Vector_P = hypot(Vector_P[0], Vector_P[1]); //ベクトルABの大きさ（hypot(a,b)で√（a^2+b^2）を計算できる <math.h>））
00015     double UnitVector_P[2] = {Vector_P[0]/A_Vector_P, Vector_P[1]/A_Vector_P}; //ベクトルABの単位ベクトル
00016     double UnitVector_Q[2] = {UnitVector_P[1], -UnitVector_P[0]}; //ベクトルCHの単位ベクトル
00017     double Vector_R[2] = {(now_x - plot_x1), (now_y - plot_y1)}; //ベクトルAC
00018     double diff = UnitVector_P[0]*Vector_R[1] - UnitVector_P[1]*Vector_R[0]; //機体位置と直線ABの距離（外積を用いて計算）
00019 
00020     double VectorOut_P[2] = {p_out*UnitVector_P[0], p_out*UnitVector_P[1]}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解
00021 
00022 
00023 ///////////////////＜XYRmotorout関数内＞以下、ベクトルABに垂直な方向の誤差を埋めるPD制御（ベクトルABに垂直方向の出力を求め、x軸方向、y軸方向の出力に分解）//////////////////////
00024 
00025 
00026     now_timeQ=timer.read();
00027     out_dutyQ=Kvq_p*diff+Kvq_d*(diff-diff_old)/(now_timeQ-old_timeQ); //ベクトルABに垂直方向の出力を決定
00028     diff_old=diff;
00029     
00030     if(out_dutyQ>0.1)out_dutyQ=0.1;
00031     if(out_dutyQ<-0.1)out_dutyQ=-0.1;
00032     
00033     old_timeQ=now_timeQ;
00034     
00035     double VectorOut_Q[2] = {out_dutyQ*UnitVector_Q[0], out_dutyQ*UnitVector_Q[1]}; //ベクトルABに垂直方向の出力をx軸方向、y軸方向の出力に分解
00036     
00037     
00038 ///////////////////////////////<XYRmotorout関数内＞以下、機体角度と目標角度の誤差を埋めるPD制御（旋回のための出力値を決定）//////////////////////////////////
00039     
00040     now_angle=gyro.getAngle();
00041     now_timeR=timer.read();
00042     diffangle=target_angle-now_angle;
00043     out_dutyR=Kvr_p*diff+Kvr_d*(diffangle-diffangle_old)/(now_timeR-old_timeR);
00044     diffangle_old=diffangle;
00045     
00046     if(out_dutyR>r_out)out_dutyR=r_out;
00047     if(out_dutyR<-r_out)out_dutyR=-r_out;
00048     
00049     old_timeR=now_timeR;
00050 
00051 //////////////////////////<XYRmotorout関数内＞以下、x軸方向、y軸方向、旋回の出力をそれぞれad_x_out,ad_y_out,ad_r_outの指すアドレスに書き込む/////////////////////////////
00052 ////////////////////////////////////////////その際、x軸方向、y軸方向の出力はフィールドの座標系から機体の座標系に変換する。///////////////////////////////////////////////
00053 
00054     *ad_x_out = (VectorOut_P[0]+VectorOut_Q[0])*cos(now_angle)-(VectorOut_P[1]+VectorOut_Q[1])*sin(now_angle);
00055     *ad_y_out = (VectorOut_P[0]+VectorOut_Q[0])*sin(now_angle)+(VectorOut_P[1]+VectorOut_Q[1])*cos(now_angle);
00056     *ad_r_out = out_dutyR;
00057    
00058 }
00059 
00060 ////////////////////////////////////////////////////////////<XYRmotorout関数は以上>////////////////////////////////////////////////////////////////
00061 
00062 
00063 
00064     
00065 void pathfollowing1::set_p_out(double p){ //ベクトルABに平行方向の出力値設定関数
00066     p_out = p;
00067     }
00068     
00069 void pathfollowing1::q_setPDparam(double q_p,double q_d){ //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
00070     Kvq_p=q_p;
00071     Kvq_d=q_d;
00072 }
00073    
00074 void pathfollowing1::r_setPDparam(double r_p,double r_d){ //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
00075     Kvr_p=r_p;
00076     Kvr_d=r_d;
00077 }
00078  
00079 void pathfollowing1::set_r_out(double r){ //旋回時の最大出力値設定関数
00080     r_out = r;
00081     }
00082     
00083 void pathfollowing1::set_target_angle(double t){ //機体の目標角度設定関数
00084     target_angle = t;
00085     }
00086