春ロボ1班(元F3RC4班+) / PathFollowing1-ver4

春ロボ2019 経路追従プログラム（ver.4）

Dependencies:   CruizCore_R1370P

Dependents:   harurobo-XYRmotorout-ver2

Fork of PathFollowing1-ver3 by 春ロボ1班(元F3RC4班+)

PathFollowing1.cpp@3:c15d0537ff7e, 2018-11-01 (annotated)

Committer:
yuki0701
Date:
Thu Nov 01 03:40:05 2018 +0000
Revision:
3:c15d0537ff7e
Parent:
1:9858517eef98 
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0701 0:3d9101dd0061 1 #include <PathFollowing1.h>
yuki0701 0:3d9101dd0061 2 #include <mbed.h>
yuki0701 0:3d9101dd0061 3 #include <math.h>
yuki0701 0:3d9101dd0061 4
yuki0701 0:3d9101dd0061 5
yuki0701 3:c15d0537ff7e 6
yuki0701 0:3d9101dd0061 7 R1370P gyro(PC_6,PC_7);
yuki0701 3:c15d0537ff7e 8 Timer timer;
yuki0701 3:c15d0537ff7e 9
yuki0701 3:c15d0537ff7e 10 double p_out,r_out;
yuki0701 3:c15d0537ff7e 11 double Kvq_p,Kvq_d,Kvr_p,Kvr_d;
yuki0701 3:c15d0537ff7e 12 double diff,diff_old,diffangle,diffangle_old;
yuki0701 3:c15d0537ff7e 13 double out_dutyQ,out_dutyR;
yuki0701 3:c15d0537ff7e 14 double now_angle,target_angle;
yuki0701 3:c15d0537ff7e 15 double now_timeQ,old_timeQ,now_timeR,old_timeR;
yuki0701 3:c15d0537ff7e 16 double now_x, now_y;
yuki0701 3:c15d0537ff7e 17
yuki0701 0:3d9101dd0061 18
yuki0701 0:3d9101dd0061 19 //初期座標：A, 目標座標：B、機体位置：C、点Cから直線ABに下ろした垂線の足：H
yuki0701 3:c15d0537ff7e 20 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はグローバル変数として定義する必要あり
yuki0701 0:3d9101dd0061 21
yuki0701 0:3d9101dd0061 22 double Vector_P[2] = {(plot_x2 - plot_x1), (plot_y2 - plot_y1)}; //ベクトルAB
yuki0701 0:3d9101dd0061 23 double A_Vector_P = hypot(Vector_P[0], Vector_P[1]); //ベクトルABの大きさ（hypot(a,b)で√（a^2+b^2）を計算できる <math.h>））
yuki0701 0:3d9101dd0061 24 double UnitVector_P[2] = {Vector_P[0]/A_Vector_P, Vector_P[1]/A_Vector_P}; //ベクトルABの単位ベクトル
yuki0701 0:3d9101dd0061 25 double UnitVector_Q[2] = {UnitVector_P[1], -UnitVector_P[0]}; //ベクトルCHの単位ベクトル
yuki0701 0:3d9101dd0061 26 double Vector_R[2] = {(now_x - plot_x1), (now_y - plot_y1)}; //ベクトルAC
yuki0701 0:3d9101dd0061 27 double diff = UnitVector_P[0]*Vector_R[1] - UnitVector_P[1]*Vector_R[0]; //機体位置と直線ABの距離（外積を用いて計算）
yuki0701 0:3d9101dd0061 28
yuki0701 0:3d9101dd0061 29 double VectorOut_P[2] = {p_out*UnitVector_P[0], p_out*UnitVector_P[1]}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解
yuki0701 0:3d9101dd0061 30
yuki0701 0:3d9101dd0061 31
yuki0701 0:3d9101dd0061 32 ///////////////////＜XYRmotorout関数内＞以下、ベクトルABに垂直な方向の誤差を埋めるPD制御（ベクトルABに垂直方向の出力を求め、x軸方向、y軸方向の出力に分解）//////////////////////
yuki0701 0:3d9101dd0061 33
yuki0701 0:3d9101dd0061 34
yuki0701 0:3d9101dd0061 35 now_timeQ=timer.read();
yuki0701 0:3d9101dd0061 36 out_dutyQ=Kvq_p*diff+Kvq_d*(diff-diff_old)/(now_timeQ-old_timeQ); //ベクトルABに垂直方向の出力を決定
yuki0701 0:3d9101dd0061 37 diff_old=diff;
yuki0701 0:3d9101dd0061 38
yuki0701 0:3d9101dd0061 39 if(out_dutyQ>0.1)out_dutyQ=0.1;
yuki0701 0:3d9101dd0061 40 if(out_dutyQ<-0.1)out_dutyQ=-0.1;
yuki0701 0:3d9101dd0061 41
yuki0701 0:3d9101dd0061 42 old_timeQ=now_timeQ;
yuki0701 0:3d9101dd0061 43
yuki0701 0:3d9101dd0061 44 double VectorOut_Q[2] = {out_dutyQ*UnitVector_Q[0], out_dutyQ*UnitVector_Q[1]}; //ベクトルABに垂直方向の出力をx軸方向、y軸方向の出力に分解
yuki0701 0:3d9101dd0061 45
yuki0701 0:3d9101dd0061 46
yuki0701 0:3d9101dd0061 47 ///////////////////////////////<XYRmotorout関数内＞以下、機体角度と目標角度の誤差を埋めるPD制御（旋回のための出力値を決定）//////////////////////////////////
yuki0701 0:3d9101dd0061 48
yuki0701 0:3d9101dd0061 49 now_angle=gyro.getAngle();
yuki0701 0:3d9101dd0061 50 now_timeR=timer.read();
yuki0701 0:3d9101dd0061 51 diffangle=target_angle-now_angle;
yuki0701 0:3d9101dd0061 52 out_dutyR=Kvr_p*diff+Kvr_d*(diffangle-diffangle_old)/(now_timeR-old_timeR);
yuki0701 0:3d9101dd0061 53 diffangle_old=diffangle;
yuki0701 0:3d9101dd0061 54
yuki0701 0:3d9101dd0061 55 if(out_dutyR>r_out)out_dutyR=r_out;
yuki0701 0:3d9101dd0061 56 if(out_dutyR<-r_out)out_dutyR=-r_out;
yuki0701 0:3d9101dd0061 57
yuki0701 0:3d9101dd0061 58 old_timeR=now_timeR;
yuki0701 0:3d9101dd0061 59
yuki0701 0:3d9101dd0061 60 //////////////////////////<XYRmotorout関数内＞以下、x軸方向、y軸方向、旋回の出力をそれぞれad_x_out,ad_y_out,ad_r_outの指すアドレスに書き込む/////////////////////////////
yuki0701 1:9858517eef98 61 ////////////////////////////////////////////その際、x軸方向、y軸方向の出力はフィールドの座標系から機体の座標系に変換する。///////////////////////////////////////////////
yuki0701 0:3d9101dd0061 62
yuki0701 1:9858517eef98 63 *ad_x_out = (VectorOut_P[0]+VectorOut_Q[0])*cos(now_angle)-(VectorOut_P[1]+VectorOut_Q[1])*sin(now_angle);
yuki0701 1:9858517eef98 64 *ad_y_out = (VectorOut_P[0]+VectorOut_Q[0])*sin(now_angle)+(VectorOut_P[1]+VectorOut_Q[1])*cos(now_angle);
yuki0701 0:3d9101dd0061 65 *ad_r_out = out_dutyR;
yuki0701 0:3d9101dd0061 66
yuki0701 0:3d9101dd0061 67 }
yuki0701 0:3d9101dd0061 68
yuki0701 0:3d9101dd0061 69 ////////////////////////////////////////////////////////////<XYRmotorout関数は以上>////////////////////////////////////////////////////////////////
yuki0701 0:3d9101dd0061 70
yuki0701 0:3d9101dd0061 71
yuki0701 0:3d9101dd0061 72
yuki0701 0:3d9101dd0061 73
yuki0701 3:c15d0537ff7e 74 void set_p_out(double p){ //ベクトルABに平行方向の出力値設定関数
yuki0701 0:3d9101dd0061 75 p_out = p;
yuki0701 0:3d9101dd0061 76 }
yuki0701 0:3d9101dd0061 77
yuki0701 3:c15d0537ff7e 78 void q_setPDparam(double q_p,double q_d){ //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
yuki0701 0:3d9101dd0061 79 Kvq_p=q_p;
yuki0701 0:3d9101dd0061 80 Kvq_d=q_d;
yuki0701 0:3d9101dd0061 81 }
yuki0701 0:3d9101dd0061 82
yuki0701 3:c15d0537ff7e 83 void r_setPDparam(double r_p,double r_d){ //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
yuki0701 0:3d9101dd0061 84 Kvr_p=r_p;
yuki0701 0:3d9101dd0061 85 Kvr_d=r_d;
yuki0701 0:3d9101dd0061 86 }
yuki0701 0:3d9101dd0061 87
yuki0701 3:c15d0537ff7e 88 void set_r_out(double r){ //旋回時の最大出力値設定関数
yuki0701 0:3d9101dd0061 89 r_out = r;
yuki0701 0:3d9101dd0061 90 }
yuki0701 0:3d9101dd0061 91
yuki0701 3:c15d0537ff7e 92 void set_target_angle(double t){ //機体の目標角度設定関数
yuki0701 0:3d9101dd0061 93 target_angle = t;
yuki0701 0:3d9101dd0061 94 }
yuki0701 0:3d9101dd0061 95