File content as of revision 3:5da150ef209c:

#include <PathFollowing.h>
#include <mbed.h>
#include <math.h>

double p_out,r_out_max;
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;
double diff_tgt, p_param;


Timer timer;

//初期座標：A, 目標座標：B、機体位置：C、点Cから直線ABに下ろした垂線の足：H
void XYRmotorout(int type, 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>500)out_dutyQ=500;
    if(out_dutyQ<-500)out_dutyQ=-500;

    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*diffangle+Kvr_d*(diffangle-diffangle_old)/(now_timeR-old_timeR));
    diffangle_old=diffangle;

    if(out_dutyR>r_out_max)out_dutyR=r_out_max;
    if(out_dutyR<-r_out_max)out_dutyR=-r_out_max;

    old_timeR=now_timeR;

//////////////////////////<XYRmotorout関数内＞以下、x軸方向、y軸方向、旋回の出力をそれぞれad_x_out,ad_y_out,ad_r_outの指すアドレスに書き込む/////////////////////////////
////////////////////////////////////////////その際、x軸方向、y軸方向の出力はフィールドの座標系から機体の座標系に変換する。///////////////////////////////////////////////
    switch(type) {

        case 1://減速しない運動
            *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);
            *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);
            *ad_r_out = out_dutyR;
            break;

        case 2://減速する運動
            diff_tgt = hypot(now_x - plot_x2, now_y - plot_y2);

            if(diff_tgt > 300) {
                diff_tgt = 300;
            }

            p_param=(diff_tgt/300);

            *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));
            *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));
            *ad_r_out = out_dutyR;
            break;
    }
}

////////////////////////////////////////////////////////////<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_max = r;
}

void set_target_angle(double t)  //機体の目標角度設定関数
{
    target_angle = t;
}