harurobo_mbed_undercarriage_sub
Diff: PathFollowing.cpp
- Revision:
- 0:591749f315ac
- Child:
- 1:d438093cb464
diff -r 000000000000 -r 591749f315ac PathFollowing.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PathFollowing.cpp Fri Nov 16 23:16:59 2018 +0000 @@ -0,0 +1,89 @@ +#include <PathFollowing.h> +#include <mbed.h> +#include <math.h> + +double p_out,r_out; +double Kvq_p,Kvq_d,Kvr_p,Kvr_d; +double diff_old,diffangle,diffangle_old; +double out_dutyQ,out_dutyR; +double now_angle,target_angle; +double now_timeQ,old_timeQ,now_timeR,old_timeR; +double now_x, now_y; + +Timer timer; + +//初期座標:A, 目標座標:B、機体位置:C、点Cから直線ABに下ろした垂線の足:H +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) //プログラム使用時、now_x,now_yはグローバル変数として定義する必要あり +{ + double Vector_P[2] = {(plot_x2 - plot_x1), (plot_y2 - plot_y1)}; //ベクトルAB + double A_Vector_P = hypot(Vector_P[0], Vector_P[1]); //ベクトルABの大きさ(hypot(a,b)で√(a^2+b^2)を計算できる <math.h>)) + double UnitVector_P[2] = {Vector_P[0]/A_Vector_P, Vector_P[1]/A_Vector_P}; //ベクトルABの単位ベクトル + double UnitVector_Q[2] = {UnitVector_P[1], -UnitVector_P[0]}; //ベクトルCHの単位ベクトル + double Vector_R[2] = {(now_x - plot_x1), (now_y - plot_y1)}; //ベクトルAC + double diff = UnitVector_P[0]*Vector_R[1] - UnitVector_P[1]*Vector_R[0]; //機体位置と直線ABの距離(外積を用いて計算) + + double VectorOut_P[2] = {p_out*UnitVector_P[0], p_out*UnitVector_P[1]}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解 + +///////////////////<XYRmotorout関数内>以下、ベクトルABに垂直な方向の誤差を埋めるPD制御(ベクトルABに垂直方向の出力を求め、x軸方向、y軸方向の出力に分解)////////////////////// + + timer.start(); + now_timeQ=timer.read(); + out_dutyQ=Kvq_p*diff+Kvq_d*(diff-diff_old)/(now_timeQ-old_timeQ); //ベクトルABに垂直方向の出力を決定 + diff_old=diff; + + if(out_dutyQ>0.1)out_dutyQ=0.1; + if(out_dutyQ<-0.1)out_dutyQ=-0.1; + + old_timeQ=now_timeQ; + + double VectorOut_Q[2] = {out_dutyQ*UnitVector_Q[0], out_dutyQ*UnitVector_Q[1]}; //ベクトルABに垂直方向の出力をx軸方向、y軸方向の出力に分解 + +///////////////////////////////<XYRmotorout関数内>以下、機体角度と目標角度の誤差を埋めるPD制御(旋回のための出力値を決定)////////////////////////////////// + + now_timeR=timer.read(); + diffangle=target_angle-now_angle; + out_dutyR=Kvr_p*diff+Kvr_d*(diffangle-diffangle_old)/(now_timeR-old_timeR); + diffangle_old=diffangle; + + if(out_dutyR>r_out)out_dutyR=r_out; + if(out_dutyR<-r_out)out_dutyR=-r_out; + + old_timeR=now_timeR; + +//////////////////////////<XYRmotorout関数内>以下、x軸方向、y軸方向、旋回の出力をそれぞれad_x_out,ad_y_out,ad_r_outの指すアドレスに書き込む///////////////////////////// +////////////////////////////////////////////その際、x軸方向、y軸方向の出力はフィールドの座標系から機体の座標系に変換する。/////////////////////////////////////////////// + + *ad_x_out = (VectorOut_P[0]+VectorOut_Q[0])*cos(now_angle)-(VectorOut_P[1]+VectorOut_Q[1])*sin(now_angle); + *ad_y_out = (VectorOut_P[0]+VectorOut_Q[0])*sin(now_angle)+(VectorOut_P[1]+VectorOut_Q[1])*cos(now_angle); + *ad_r_out = out_dutyR; +} + +////////////////////////////////////////////////////////////<XYRmotorout関数は以上>//////////////////////////////////////////////////////////////// + + +void set_p_out(double p) //ベクトルABに平行方向の出力値設定関数 +{ + p_out = p; +} + +void q_setPDparam(double q_p,double q_d) //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数 +{ + Kvq_p=q_p; + Kvq_d=q_d; +} + +void r_setPDparam(double r_p,double r_d) //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数 +{ + Kvr_p=r_p; + Kvr_d=r_d; +} + +void set_r_out(double r) //旋回時の最大出力値設定関数 +{ + r_out = r; +} + +void set_target_angle(double t) //機体の目標角度設定関数 +{ + target_angle = t; +}