Kobayashi Akihiro / ActiveCaster

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   ActiveCaster_ ActiveCaster_2

PathTracking.cpp@2:f206311600ee, 2022-01-28 (annotated)

Committer:
e5119053f6
Date:
Fri Jan 28 15:43:18 2022 +0000
Revision:
2:f206311600ee
Parent:
0:5e4f1e288e2a 
DDSS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e5119053f6 0:5e4f1e288e2a 1 //-----------------------------------------
e5119053f6 0:5e4f1e288e2a 2 // 軌道追従や位置のPID制御を行うためのクラス
e5119053f6 0:5e4f1e288e2a 3 // 作成：2019/05/15 by Yuki Ueno
e5119053f6 0:5e4f1e288e2a 4 // 編集：Miki Nakaone
e5119053f6 0:5e4f1e288e2a 5 //-----------------------------------------
e5119053f6 0:5e4f1e288e2a 6
e5119053f6 0:5e4f1e288e2a 7 #include "PathTracking.h"
e5119053f6 0:5e4f1e288e2a 8
e5119053f6 0:5e4f1e288e2a 9 extern coords gPosi;
e5119053f6 0:5e4f1e288e2a 10
e5119053f6 0:5e4f1e288e2a 11 PID posiPIDx(POSI_X_KP, POSI_X_KI, POSI_X_KD, INT_TIME);
e5119053f6 0:5e4f1e288e2a 12 PID posiPIDy(POSI_Y_KP, POSI_Y_KI, POSI_Y_KD, INT_TIME);
e5119053f6 0:5e4f1e288e2a 13 PID posiPIDz(POSI_Z_KP, POSI_Z_KI, POSI_Z_KD, INT_TIME);
e5119053f6 0:5e4f1e288e2a 14
e5119053f6 0:5e4f1e288e2a 15 PID yokozurePID(YOKOZURE_KP, YOKOZURE_KI, YOKOZURE_KD, INT_TIME);//(3.0, 0.0, 0.0, INT_TIME);
e5119053f6 0:5e4f1e288e2a 16 PID kakudoPID(KAKUDO_KP, KAKUDO_KI, KAKUDO_KD, INT_TIME);
e5119053f6 0:5e4f1e288e2a 17
e5119053f6 0:5e4f1e288e2a 18 // 二次遅れ使えるようになる
e5119053f6 0:5e4f1e288e2a 19 Filter sokduo_filter(INT_TIME);
e5119053f6 0:5e4f1e288e2a 20 Filter kakudo_filter(INT_TIME);
e5119053f6 0:5e4f1e288e2a 21
e5119053f6 0:5e4f1e288e2a 22 // コンストラクタ
e5119053f6 0:5e4f1e288e2a 23 PathTracking::PathTracking(int xmode){
e5119053f6 0:5e4f1e288e2a 24 path_num = 0;
e5119053f6 0:5e4f1e288e2a 25 max_pathnum = 0;
e5119053f6 0:5e4f1e288e2a 26 t_be = 0.0;
e5119053f6 0:5e4f1e288e2a 27 pre_t_be = 0.1;
e5119053f6 0:5e4f1e288e2a 28 epsilon = 1.0;
e5119053f6 0:5e4f1e288e2a 29
e5119053f6 0:5e4f1e288e2a 30 refVx = 0;
e5119053f6 0:5e4f1e288e2a 31 refVy = 0;
e5119053f6 0:5e4f1e288e2a 32 refVz = 0;
e5119053f6 0:5e4f1e288e2a 33
e5119053f6 0:5e4f1e288e2a 34 count_acc = 0;
e5119053f6 0:5e4f1e288e2a 35
e5119053f6 0:5e4f1e288e2a 36 mode = xmode;
e5119053f6 0:5e4f1e288e2a 37
e5119053f6 0:5e4f1e288e2a 38 mode_changed = true;
e5119053f6 0:5e4f1e288e2a 39 init_done = false;
e5119053f6 0:5e4f1e288e2a 40 }
e5119053f6 0:5e4f1e288e2a 41
e5119053f6 0:5e4f1e288e2a 42 // tを求めるための方程式
e5119053f6 0:5e4f1e288e2a 43 double PathTracking::func(int p, double t)
e5119053f6 0:5e4f1e288e2a 44 {
e5119053f6 0:5e4f1e288e2a 45 return a_be[p] * pow(t, 5.0) + b_be[p] * pow(t,4.0) + c_be[p] * pow(t,3.0) + d_be[p] * pow(t,2.0) + e_be[p] * t + f_be[p];
e5119053f6 0:5e4f1e288e2a 46 }
e5119053f6 0:5e4f1e288e2a 47 // tを求めるための方程式の1階微分
e5119053f6 0:5e4f1e288e2a 48 double PathTracking::dfunc(int p, double t)
e5119053f6 0:5e4f1e288e2a 49 {
e5119053f6 0:5e4f1e288e2a 50 return 5.0 * a_be[p] * pow(t, 4.0) + 4.0 * b_be[p] * pow(t,3.0) + 3.0 * c_be[p] * pow(t,2.0) + 2.0 * d_be[p] * t + e_be[p];
e5119053f6 0:5e4f1e288e2a 51 }
e5119053f6 0:5e4f1e288e2a 52
e5119053f6 0:5e4f1e288e2a 53 // tにおけるベジエ曲線の座標を求める関数
e5119053f6 0:5e4f1e288e2a 54 double PathTracking::bezier_x(int p, double t)
e5119053f6 0:5e4f1e288e2a 55 {
e5119053f6 0:5e4f1e288e2a 56 return Ax[p]*pow(t,3.0) + 3.0*Bx[p]*pow(t,2.0) + 3.0*Cx[p]*t + Dx[p];
e5119053f6 0:5e4f1e288e2a 57 }
e5119053f6 0:5e4f1e288e2a 58 double PathTracking::bezier_y(int p, double t)
e5119053f6 0:5e4f1e288e2a 59 {
e5119053f6 0:5e4f1e288e2a 60 return Ay[p]*pow(t,3.0) + 3.0*By[p]*pow(t,2.0) + 3.0*Cy[p]*t + Dy[p];
e5119053f6 0:5e4f1e288e2a 61 }
e5119053f6 0:5e4f1e288e2a 62
e5119053f6 0:5e4f1e288e2a 63 // ベジエ曲線式の1階微分
e5119053f6 0:5e4f1e288e2a 64 double PathTracking::dbezier_x(int p, double t)
e5119053f6 0:5e4f1e288e2a 65 {
e5119053f6 0:5e4f1e288e2a 66 return 3.0*Ax[p]*pow(t,2.0) + 6.0*Bx[p]*t + 3.0*Cx[p];
e5119053f6 0:5e4f1e288e2a 67 }
e5119053f6 0:5e4f1e288e2a 68 double PathTracking::dbezier_y(int p, double t)
e5119053f6 0:5e4f1e288e2a 69 {
e5119053f6 0:5e4f1e288e2a 70 return 3.0*Ay[p]*pow(t,2.0) + 6.0*By[p]*t + 3.0*Cy[p];
e5119053f6 0:5e4f1e288e2a 71 }
e5119053f6 0:5e4f1e288e2a 72
e5119053f6 0:5e4f1e288e2a 73 // ニュートン法のための係数の初期化
e5119053f6 0:5e4f1e288e2a 74 void PathTracking::initSettings(){
e5119053f6 0:5e4f1e288e2a 75 // PID関連初期化
e5119053f6 0:5e4f1e288e2a 76 posiPIDx.PIDinit(0.0, 0.0);
e5119053f6 0:5e4f1e288e2a 77 posiPIDy.PIDinit(0.0, 0.0);
e5119053f6 0:5e4f1e288e2a 78 posiPIDz.PIDinit(0.0, 0.0);
e5119053f6 0:5e4f1e288e2a 79
e5119053f6 0:5e4f1e288e2a 80 yokozurePID.PIDinit(0.0, 0.0);
e5119053f6 0:5e4f1e288e2a 81 kakudoPID.PIDinit(0.0, 0.0);
e5119053f6 0:5e4f1e288e2a 82
e5119053f6 0:5e4f1e288e2a 83 sokduo_filter.setSecondOrderPara(FILT_SOKUDO_OMEGA, FILT_SOKUDO_DZETA, 0.0);//(15.0, 1.0, 0.0);
e5119053f6 0:5e4f1e288e2a 84 kakudo_filter.setSecondOrderPara(FILT_KAKUDO_OMEGA, FILT_KAKUDO_DZETA, 0.0);//(7.0, 1.0, 0.0);
e5119053f6 0:5e4f1e288e2a 85
e5119053f6 0:5e4f1e288e2a 86 for(int i = 0; i < PATHNUM; i++) {
e5119053f6 0:5e4f1e288e2a 87 Ax[i] = Px[3*i+3] -3*Px[3*i+2] + 3*Px[3*i+1] - Px[3*i+0];
e5119053f6 0:5e4f1e288e2a 88 Ay[i] = Py[3*i+3] -3*Py[3*i+2] + 3*Py[3*i+1] - Py[3*i+0];
e5119053f6 0:5e4f1e288e2a 89 Bx[i] = Px[3*i+2] -2*Px[3*i+1] + Px[3*i+0];
e5119053f6 0:5e4f1e288e2a 90 By[i] = Py[3*i+2] -2*Py[3*i+1] + Py[3*i+0];
e5119053f6 0:5e4f1e288e2a 91 Cx[i] = Px[3*i+1] - Px[3*i+0];
e5119053f6 0:5e4f1e288e2a 92 Cy[i] = Py[3*i+1] - Py[3*i+0];
e5119053f6 0:5e4f1e288e2a 93 Dx[i] = Px[3*i+0];
e5119053f6 0:5e4f1e288e2a 94 Dy[i] = Py[3*i+0];
e5119053f6 0:5e4f1e288e2a 95 }
e5119053f6 0:5e4f1e288e2a 96
e5119053f6 0:5e4f1e288e2a 97 for(int i = 0; i < PATHNUM; i++) {
e5119053f6 0:5e4f1e288e2a 98 a_be[i] = pow(Ax[i], 2.0) + pow(Ay[i], 2.0);
e5119053f6 0:5e4f1e288e2a 99 b_be[i] = 5*(Ax[i]*Bx[i] + Ay[i]*By[i]);
e5119053f6 0:5e4f1e288e2a 100 c_be[i] = 2*((3*pow(Bx[i],2.0)+2*Ax[i]*Cx[i]) + (3*pow(By[i],2.0)+2*Ay[i]*Cy[i]));
e5119053f6 0:5e4f1e288e2a 101 d_be_[i] = 9*Bx[i]*Cx[i] + 9*By[i]*Cy[i];
e5119053f6 0:5e4f1e288e2a 102 e_be_[i] = 3*pow(Cx[i],2.0) + 3*pow(Cy[i],2.0);
e5119053f6 0:5e4f1e288e2a 103 f_be_[i] = 0;
e5119053f6 0:5e4f1e288e2a 104 }
e5119053f6 0:5e4f1e288e2a 105 init_done = true;
e5119053f6 0:5e4f1e288e2a 106 }
e5119053f6 0:5e4f1e288e2a 107
e5119053f6 0:5e4f1e288e2a 108 // ベジエ曲線までの垂線距離をニュートン法で求めて，そこまでの距離と接線角度を計算する
e5119053f6 0:5e4f1e288e2a 109 void PathTracking::calcRefpoint(){
e5119053f6 0:5e4f1e288e2a 110 if(init_done){
e5119053f6 0:5e4f1e288e2a 111 double tmpx = Px[path_num * 3] - gPosi.x;
e5119053f6 0:5e4f1e288e2a 112 double tmpy = Py[path_num * 3] - gPosi.y;
e5119053f6 0:5e4f1e288e2a 113
e5119053f6 0:5e4f1e288e2a 114 d_be[path_num] = d_be_[path_num] + Ax[path_num] * tmpx + Ay[path_num] * tmpy;
e5119053f6 0:5e4f1e288e2a 115 e_be[path_num] = e_be_[path_num] + 2*Bx[path_num] * tmpx + 2*By[path_num] * tmpy;
e5119053f6 0:5e4f1e288e2a 116 f_be[path_num] = f_be_[path_num] + Cx[path_num] * tmpx + Cy[path_num] * tmpy;
e5119053f6 0:5e4f1e288e2a 117
e5119053f6 0:5e4f1e288e2a 118 int count_newton = 0;
e5119053f6 0:5e4f1e288e2a 119 do {
e5119053f6 0:5e4f1e288e2a 120 t_be = pre_t_be - func(path_num, pre_t_be)/dfunc(path_num, pre_t_be);
e5119053f6 0:5e4f1e288e2a 121 epsilon = fabs((t_be - pre_t_be)/pre_t_be);
e5119053f6 0:5e4f1e288e2a 122
e5119053f6 0:5e4f1e288e2a 123 pre_t_be = t_be;
e5119053f6 0:5e4f1e288e2a 124 count_newton++;
e5119053f6 0:5e4f1e288e2a 125 }while(epsilon >= 1e-4 && count_newton <= 50);
e5119053f6 0:5e4f1e288e2a 126
e5119053f6 0:5e4f1e288e2a 127 //double onx, ony; //ベジエ曲線上の点
e5119053f6 0:5e4f1e288e2a 128 onx = bezier_x(path_num, t_be);
e5119053f6 0:5e4f1e288e2a 129 ony = bezier_y(path_num, t_be);
e5119053f6 0:5e4f1e288e2a 130
e5119053f6 0:5e4f1e288e2a 131 // 外積による距離導出
e5119053f6 0:5e4f1e288e2a 132 //double angle;
e5119053f6 0:5e4f1e288e2a 133 angle = atan2(dbezier_y(path_num, t_be), dbezier_x(path_num, t_be)); // ベジエ曲線の接線方向
e5119053f6 0:5e4f1e288e2a 134 if(fabs(angle - preAngle) > M_PI){
e5119053f6 0:5e4f1e288e2a 135 angle += 2 * M_PI;
e5119053f6 0:5e4f1e288e2a 136 }
e5119053f6 0:5e4f1e288e2a 137 //double dist;
e5119053f6 0:5e4f1e288e2a 138 dist = (ony - gPosi.y)*cos(angle) - (onx - gPosi.x)*sin(angle);
e5119053f6 0:5e4f1e288e2a 139
e5119053f6 0:5e4f1e288e2a 140 epsilon = 1.0;
e5119053f6 0:5e4f1e288e2a 141 }
e5119053f6 0:5e4f1e288e2a 142 }
e5119053f6 0:5e4f1e288e2a 143
e5119053f6 0:5e4f1e288e2a 144 // モードによって，それぞれ指令速度を計算する
e5119053f6 0:5e4f1e288e2a 145 int PathTracking::calcRefvel(){
e5119053f6 0:5e4f1e288e2a 146 double refVxg, refVyg, refVzg; // グローバル座標系の指定速度
e5119053f6 0:5e4f1e288e2a 147 double tmpPx, tmpPy;
e5119053f6 0:5e4f1e288e2a 148 //static int counter = 0;
e5119053f6 0:5e4f1e288e2a 149
e5119053f6 0:5e4f1e288e2a 150 if(init_done){
e5119053f6 0:5e4f1e288e2a 151 if(path_num <= max_pathnum){ // パスが存在する場合は以下の処理を行う
e5119053f6 0:5e4f1e288e2a 152 if(mode == FOLLOW_TANGENT || mode == FOLLOW_COMMAND){ // ベジエ曲線追従モード
e5119053f6 0:5e4f1e288e2a 153 calcRefpoint();
e5119053f6 0:5e4f1e288e2a 154
e5119053f6 0:5e4f1e288e2a 155 double refVtan, refVper, refVrot;
e5119053f6 0:5e4f1e288e2a 156 if((acc_mode[path_num] == MODE_START || acc_mode[path_num] == MODE_START_STOP) && count_acc <= acc_count[path_num]){
e5119053f6 0:5e4f1e288e2a 157 count_acc++;
e5119053f6 0:5e4f1e288e2a 158 refVtan = refvel[path_num] * count_acc/(double)acc_count[path_num];
e5119053f6 0:5e4f1e288e2a 159 }else if(t_be < dec_tbe[path_num]){
e5119053f6 0:5e4f1e288e2a 160 refVtan = refvel[path_num];
e5119053f6 0:5e4f1e288e2a 161 }else if((acc_mode[path_num] == MODE_STOP || acc_mode[path_num] == MODE_START_STOP) && t_be >= dec_tbe[path_num]){
e5119053f6 0:5e4f1e288e2a 162 refVtan = refvel[path_num] - refvel[path_num] * (t_be - dec_tbe[path_num]) / (1.0 - dec_tbe[path_num]);
e5119053f6 0:5e4f1e288e2a 163 }else if(t_be >= dec_tbe[path_num]){
e5119053f6 0:5e4f1e288e2a 164 refVtan = refvel[path_num] - (refvel[path_num] - refvel[path_num + 1]) * (t_be - dec_tbe[path_num]) / (1.0 - dec_tbe[path_num]);
e5119053f6 0:5e4f1e288e2a 165 }
e5119053f6 0:5e4f1e288e2a 166
e5119053f6 0:5e4f1e288e2a 167 //refVtan = sokduo_filter.SecondOrderLag(refvel[path_num]); // 接線方向速度
e5119053f6 0:5e4f1e288e2a 168 refVper = yokozurePID.getCmd(dist, 0.0, refvel[path_num] * 2.0); // 横方向速度
e5119053f6 0:5e4f1e288e2a 169
e5119053f6 0:5e4f1e288e2a 170 // 旋回は以下の2種類を mode によって変える
e5119053f6 0:5e4f1e288e2a 171 if(mode == FOLLOW_TANGENT){
e5119053f6 0:5e4f1e288e2a 172 if(mode_changed){
e5119053f6 0:5e4f1e288e2a 173 kakudoPID.PIDinit(angle, gPosi.z);
e5119053f6 0:5e4f1e288e2a 174 mode_changed = false;
e5119053f6 0:5e4f1e288e2a 175 }
e5119053f6 0:5e4f1e288e2a 176 refVrot = kakudoPID.getCmd(angle, gPosi.z, 1.57);//(refKakudo, gPosiz, 1.57);
e5119053f6 0:5e4f1e288e2a 177 }else{
e5119053f6 0:5e4f1e288e2a 178 if(mode_changed){
e5119053f6 0:5e4f1e288e2a 179 kakudoPID.PIDinit(refKakudo, gPosi.z);
e5119053f6 0:5e4f1e288e2a 180 kakudo_filter.initPrevData(refKakudo);
e5119053f6 0:5e4f1e288e2a 181 mode_changed = false;
e5119053f6 0:5e4f1e288e2a 182 }
e5119053f6 0:5e4f1e288e2a 183 refKakudo = kakudo_filter.SecondOrderLag(refangle[path_num]);
e5119053f6 0:5e4f1e288e2a 184 refVrot = kakudoPID.getCmd(refKakudo, gPosi.z, 1.57);
e5119053f6 0:5e4f1e288e2a 185 }
e5119053f6 0:5e4f1e288e2a 186
e5119053f6 0:5e4f1e288e2a 187 per = refVper;
e5119053f6 0:5e4f1e288e2a 188 rot = refVrot;
e5119053f6 0:5e4f1e288e2a 189 // ローカル座標系の指令速度(グローバル座標系のも込み込み)
e5119053f6 0:5e4f1e288e2a 190 //refVxとrefVyをコメントアウトするとkakudoPIDのパラメータ調整が出来る(手で押してみて…)
e5119053f6 0:5e4f1e288e2a 191 //refVperだけにするとyokozurePIDのパラメータ調整できる
e5119053f6 0:5e4f1e288e2a 192 refVx = refVtan * cos( gPosi.z - angle ) + refVper * sin( gPosi.z - angle );
e5119053f6 0:5e4f1e288e2a 193 refVy = -refVtan * sin( gPosi.z - angle ) + refVper * cos( gPosi.z - angle );
e5119053f6 0:5e4f1e288e2a 194 refVz = refVrot;
e5119053f6 0:5e4f1e288e2a 195 }else{ // PID位置制御モード
e5119053f6 0:5e4f1e288e2a 196 if( mode_changed ){
e5119053f6 0:5e4f1e288e2a 197 Px[3 * path_num] = gPosi.x;
e5119053f6 0:5e4f1e288e2a 198 Py[3 * path_num] = gPosi.y;
e5119053f6 0:5e4f1e288e2a 199 posiPIDx.PIDinit(Px[3 * path_num], gPosi.x); // ref, act
e5119053f6 0:5e4f1e288e2a 200 posiPIDy.PIDinit(Py[3 * path_num], gPosi.y);
e5119053f6 0:5e4f1e288e2a 201 posiPIDz.PIDinit(refKakudo, gPosi.z);
e5119053f6 0:5e4f1e288e2a 202 kakudo_filter.initPrevData(refKakudo);
e5119053f6 0:5e4f1e288e2a 203 setRefKakudo();
e5119053f6 0:5e4f1e288e2a 204 mode_changed = false;
e5119053f6 0:5e4f1e288e2a 205 }
e5119053f6 0:5e4f1e288e2a 206
e5119053f6 0:5e4f1e288e2a 207 if(count_acc <= acc_count[path_num]){
e5119053f6 0:5e4f1e288e2a 208 count_acc++;
e5119053f6 0:5e4f1e288e2a 209 tmpPx = Px[3 * path_num] + (Px[3 * path_num + 3] - Px[3 * path_num]) * count_acc / (double)acc_count[path_num];
e5119053f6 0:5e4f1e288e2a 210 tmpPy = Py[3 * path_num] + (Py[3 * path_num + 3] - Py[3 * path_num]) * count_acc / (double)acc_count[path_num];
e5119053f6 0:5e4f1e288e2a 211 }else{
e5119053f6 0:5e4f1e288e2a 212 tmpPx = (Px[3 * path_num + 3]);
e5119053f6 0:5e4f1e288e2a 213 tmpPy = (Py[3 * path_num + 3]);
e5119053f6 0:5e4f1e288e2a 214 }
e5119053f6 0:5e4f1e288e2a 215
e5119053f6 0:5e4f1e288e2a 216 // PIDクラスを使って位置制御を行う(速度の指令値を得る)
e5119053f6 0:5e4f1e288e2a 217 refVxg = posiPIDx.getCmd(tmpPx, gPosi.x, refvel[path_num]);//(Px[30], gPosix, refvel[phase]);
e5119053f6 0:5e4f1e288e2a 218 refVyg = posiPIDy.getCmd(tmpPy, gPosi.y, refvel[path_num]);//(Py[30], gPosiy, refvel[phase]);
e5119053f6 0:5e4f1e288e2a 219 refKakudo = kakudo_filter.SecondOrderLag(refangle[path_num]);
e5119053f6 0:5e4f1e288e2a 220 refVzg = posiPIDz.getCmd(refKakudo, gPosi.z, 1.57);//角速度に対してrefvelは遅すぎるから　refvel[path_num]);//(0.0, gPosiz, refvel[phase]);
e5119053f6 0:5e4f1e288e2a 221
e5119053f6 0:5e4f1e288e2a 222 // 上記はグローバル座標系における速度のため，ローカルに変換
e5119053f6 0:5e4f1e288e2a 223 refVx = refVxg * cos(gPosi.z) + refVyg * sin(gPosi.z);
e5119053f6 0:5e4f1e288e2a 224 refVy = -refVxg * sin(gPosi.z) + refVyg * cos(gPosi.z);
e5119053f6 0:5e4f1e288e2a 225 refVz = refVzg;
e5119053f6 0:5e4f1e288e2a 226 }
e5119053f6 0:5e4f1e288e2a 227
e5119053f6 0:5e4f1e288e2a 228 // 収束判定して，収束していたら　1　を返す
e5119053f6 0:5e4f1e288e2a 229 dist2goal = sqrt(pow(gPosi.x - Px[3 * path_num + 3], 2.0) + pow(gPosi.y - Py[3 * path_num + 3], 2.0));
e5119053f6 0:5e4f1e288e2a 230 if(mode == FOLLOW_TANGENT || mode == FOLLOW_COMMAND){
e5119053f6 0:5e4f1e288e2a 231 // 軌道追従制御なら，到達位置からの距離とベジエ曲線の t のどちらかの条件
e5119053f6 0:5e4f1e288e2a 232 if(dist2goal <= conv_length || t_be >= conv_tnum){
e5119053f6 0:5e4f1e288e2a 233 //counter = 0;
e5119053f6 0:5e4f1e288e2a 234 return 1;
e5119053f6 0:5e4f1e288e2a 235 }
e5119053f6 0:5e4f1e288e2a 236 }if(mode == POSITION_PID){
e5119053f6 0:5e4f1e288e2a 237 // 位置制御なら，目標位置と角度両方を見る
e5119053f6 0:5e4f1e288e2a 238 if(dist2goal <= conv_length && fabs(refangle[path_num] - gPosi.z)){
e5119053f6 0:5e4f1e288e2a 239 //counter = 0;
e5119053f6 0:5e4f1e288e2a 240 return 1;
e5119053f6 0:5e4f1e288e2a 241 }
e5119053f6 0:5e4f1e288e2a 242 }
e5119053f6 0:5e4f1e288e2a 243
e5119053f6 0:5e4f1e288e2a 244 // 収束していなかったら　0　を返す
e5119053f6 0:5e4f1e288e2a 245 return 0;
e5119053f6 0:5e4f1e288e2a 246 }else{
e5119053f6 0:5e4f1e288e2a 247 // path_num が設定された max_pathnum を超えたら　-2　を返す
e5119053f6 0:5e4f1e288e2a 248 return -2;
e5119053f6 0:5e4f1e288e2a 249 }
e5119053f6 0:5e4f1e288e2a 250
e5119053f6 0:5e4f1e288e2a 251 }else{
e5119053f6 0:5e4f1e288e2a 252 // 初期化されていなかったら　-1　を返す
e5119053f6 0:5e4f1e288e2a 253 return -1;
e5119053f6 0:5e4f1e288e2a 254 }
e5119053f6 0:5e4f1e288e2a 255 }
e5119053f6 0:5e4f1e288e2a 256
e5119053f6 0:5e4f1e288e2a 257 // ベジエ曲線のパス番号をインクリメントする
e5119053f6 0:5e4f1e288e2a 258 void PathTracking::incrPathnum(double xconv_length, double xconv_tnum = 0.997){
e5119053f6 0:5e4f1e288e2a 259 path_num++;
e5119053f6 0:5e4f1e288e2a 260 pre_t_be = 0.1;
e5119053f6 0:5e4f1e288e2a 261 count_acc = 0;
e5119053f6 0:5e4f1e288e2a 262 setConvPara(xconv_length, xconv_tnum);
e5119053f6 0:5e4f1e288e2a 263 }
e5119053f6 0:5e4f1e288e2a 264
e5119053f6 0:5e4f1e288e2a 265 int PathTracking::getPathNum(){
e5119053f6 0:5e4f1e288e2a 266 return path_num;
e5119053f6 0:5e4f1e288e2a 267 }
e5119053f6 0:5e4f1e288e2a 268
e5119053f6 0:5e4f1e288e2a 269 void PathTracking::setPathNum(int num){
e5119053f6 0:5e4f1e288e2a 270 path_num = num;
e5119053f6 0:5e4f1e288e2a 271 }
e5119053f6 0:5e4f1e288e2a 272
e5119053f6 0:5e4f1e288e2a 273 // 収束判定に用いる距離などをセットする
e5119053f6 0:5e4f1e288e2a 274 void PathTracking::setConvPara(double xconv_length, double xconv_tnum = 0.997){
e5119053f6 0:5e4f1e288e2a 275 conv_length = xconv_length;
e5119053f6 0:5e4f1e288e2a 276 conv_tnum = xconv_tnum;
e5119053f6 0:5e4f1e288e2a 277 }
e5119053f6 0:5e4f1e288e2a 278
e5119053f6 0:5e4f1e288e2a 279 void PathTracking::setMode(int xmode){
e5119053f6 0:5e4f1e288e2a 280 mode = xmode;
e5119053f6 0:5e4f1e288e2a 281 mode_changed = true;
e5119053f6 0:5e4f1e288e2a 282 }
e5119053f6 0:5e4f1e288e2a 283
e5119053f6 0:5e4f1e288e2a 284 int PathTracking::getMode(){
e5119053f6 0:5e4f1e288e2a 285 return mode;
e5119053f6 0:5e4f1e288e2a 286 }
e5119053f6 0:5e4f1e288e2a 287
e5119053f6 0:5e4f1e288e2a 288 void PathTracking::setMaxPathnum(int num){
e5119053f6 0:5e4f1e288e2a 289 max_pathnum = num;
e5119053f6 0:5e4f1e288e2a 290 }
e5119053f6 0:5e4f1e288e2a 291
e5119053f6 0:5e4f1e288e2a 292 void PathTracking::setPosiPIDxPara(float xKp, float xKi, float xKd){
e5119053f6 0:5e4f1e288e2a 293 posiPIDx.setPara(xKp, xKi, xKd);
e5119053f6 0:5e4f1e288e2a 294 }
e5119053f6 0:5e4f1e288e2a 295
e5119053f6 0:5e4f1e288e2a 296 void PathTracking::setPosiPIDyPara(float xKp, float xKi, float xKd){
e5119053f6 0:5e4f1e288e2a 297 posiPIDy.setPara(xKp, xKi, xKd);
e5119053f6 0:5e4f1e288e2a 298 }
e5119053f6 0:5e4f1e288e2a 299
e5119053f6 0:5e4f1e288e2a 300 void PathTracking::setPosiPIDzPara(float xKp, float xKi, float xKd){
e5119053f6 0:5e4f1e288e2a 301 posiPIDz.setPara(xKp, xKi, xKd);
e5119053f6 0:5e4f1e288e2a 302 }
e5119053f6 0:5e4f1e288e2a 303
e5119053f6 0:5e4f1e288e2a 304 void PathTracking::setYokozurePIDPara(float xKp, float xKi, float xKd){
e5119053f6 0:5e4f1e288e2a 305 yokozurePID.setPara(xKp, xKi, xKd);
e5119053f6 0:5e4f1e288e2a 306 }
e5119053f6 0:5e4f1e288e2a 307
e5119053f6 0:5e4f1e288e2a 308 void PathTracking::setKakudoPIDPara(float xKp, float xKi, float xKd){
e5119053f6 0:5e4f1e288e2a 309 kakudoPID.setPara(xKp, xKi, xKd);
e5119053f6 0:5e4f1e288e2a 310 }
e5119053f6 0:5e4f1e288e2a 311
e5119053f6 0:5e4f1e288e2a 312
e5119053f6 0:5e4f1e288e2a 313 void PathTracking::kakudoPIDinit(){
e5119053f6 0:5e4f1e288e2a 314 kakudoPID.PIDinit(refKakudo, gPosi.z);
e5119053f6 0:5e4f1e288e2a 315 }
e5119053f6 0:5e4f1e288e2a 316
e5119053f6 0:5e4f1e288e2a 317 void PathTracking::setRefKakudo(){
e5119053f6 0:5e4f1e288e2a 318 refKakudo = refangle[path_num];
e5119053f6 0:5e4f1e288e2a 319 }
e5119053f6 0:5e4f1e288e2a 320
e5119053f6 0:5e4f1e288e2a 321 double PathTracking::getRefVper(){
e5119053f6 0:5e4f1e288e2a 322 return per;
e5119053f6 0:5e4f1e288e2a 323 }
e5119053f6 0:5e4f1e288e2a 324
e5119053f6 0:5e4f1e288e2a 325 double PathTracking::getRefVrot(){
e5119053f6 0:5e4f1e288e2a 326 return rot;
e5119053f6 0:5e4f1e288e2a 327 }