File content as of revision 15:02c68793a266:

/**
********************************************************************************
* @file    main.c
* @author  You
* @version V?.?.?
* @date    Today
* @brief   メインファイル
******************************************************************************
*/

/* Includes ------------------------------------------------------------------*/
#include <main.h>

/* 型定義 --------------------------------------------------------------------*/

/* ロボットの加速度 */
ROCATION NowAcc;
/* ロボットの座標 */
ROCATION NowLoc;
/* ロボットの目標座標 */
ROCATION Target;

/* 定数定義 ------------------------------------------------------------------*/

/* スラローム自動移動の座標最大数 */
const int SlalomPointMAX=10;
/* [0][1][2]…X,Y,Theta(deg) [3]許容残り距離 */
const int SlalomPoint[SlalomPointMAX][4] = {
    { 500,  500,  0,   0},
    {1950, 1250,  0, 100},
    {1950, 2750,  0, 100},
    { 510, 2750,  0, 100},
    { 510, 4250,  0, 100},
    {1950, 4250,  0, 100},
    {1950, 5750,  0, 100},
    {1210, 5750,  0, 100},
    {1210, 9000,  0, 100},
    {5515, 9000,  0,   0}
};
/* マクロ定義 ----------------------------------------------------------------*/
/* 関数プロトタイプ宣言 -------------------------------------------------------*/

/* タイマ呼び出し用 */
void IT_CallBack(void);

/* 自己位置推定処理 */
void LocEstimate(void);

/* オムニ関係 */
void SubOmuni(int X,int Y,int R);

/* ジャイロ補整 */
void CompYaw(void);

/* 台形制御値代入 */
int omuni_control(int X0,int Y0,int X1,int Y1,double tar_theta, int error);

/* 移動する点を探す */
int lncl(double *pt1, double *pt2, double *xyr, double *xy);

/* 変数定義 ------------------------------------------------------------------*/

/* 操作権 0…なし 1…手動 2…自動 */
int operate=0;

/* 自動シーケンス */
int auto_mode=0;
int auto_sequence=0;

/* ゾーン指定 0…青，1…赤 */
bool zone=0;

/* 直読みエンコーダ角度保存(degree) */
double EncoderDeg[EncoderMAX] = {0};

/* 足回り値保存変数 */
int MovMotor[4]= {0};

/* 自動yaw補整目標角度 */
double TarTheta=0;

/* クラス定義 ----------------------------------------------------------------*/

/* 割り込み用クラス */
Ticker flipper;

/* gyro用タイマ */
Timer yawCnt;

/* UART (Tx,Rx) */
Serial telemetry(USBTX, USBRX, 115200);

/* コントローラー */
SBDBT DS3(PA_0, PA_1, 9600);

/* オンボードLED */
DigitalOut led(LED2);

/* USERボタン */
DigitalIn UB(PC_13,PullDown);

/* エンコーダーピン CS */
DigitalOut CS[] = {PA_2,PA_3};
DigitalOut CL[] = {PA_4,PA_5};
DigitalIn  DO[] = {PA_6,PA_7};

/* 足回り動作クラス定義 */
Move omuni(MovMotor,NowLoc.theta);

/* I2C MDのクラス定義 */
YKNCT_MD_I2C MD(PB_9,PB_8);

/* ジャイロのピン設定 */
BNO055 bno(PB_9, PB_8);


/*----------------------------------- main ----------------------------------*/
int main()
{
    telemetry.printf("\n\rMainStart");

    /* 割り込みの設定
     * IT_CallBack関数を0.1msで割り込み */
    flipper.attach_us(&IT_CallBack, 100);

    /* ジャイロの設定 */
    bno.setmode(OPERATION_MODE_IMUPLUS);

    /* I2CMDの設定 */
    MD.Init(0,MD_SMB);
    MD.Init(1,MD_SMB);
    MD.Init(2,MD_SMB);
    MD.Init(3,MD_SMB);

    /* タイマ-スタート */
    yawCnt.start();
    /* タイマーリセット */
    yawCnt.reset();

    telemetry.printf("\n\rMainLoopStart");
    /* メインループ --------------------------------------------------------------*/
    while(1) {
        /* オンボードLED点滅 */
        led=!led;

        /* 表示改行 */
        telemetry.printf("\n\r");

        /* 自動処理関連テレメトリ */
        telemetry.printf("ope:%d ",operate);
        /* 座標テレメトリ */
        telemetry.printf("X:%4.0f Y:%4.0f T:%4.0f ",NowLoc.X,NowLoc.Y,NowLoc.theta);

        /* 自己位置推定更新 */
        LocEstimate();

        for(int i=0; i<4; i++) {
            MD.Set(i,MovMotor[i]);
            MovMotor[i]=0;
        }
        /* I2CMD実行 */
        MD.Exe();

        /* Startで各種初期化 */
        if(Start) {
            NowLoc.X=0;
            NowLoc.Y=0;
            NowLoc.theta=0;
            TarTheta=0;
            auto_sequence=0;
        }

        /* 操縦権変更 ×停止 △手動 〇自動 */
        if(DS3.CROSS)     operate=0;
        if(DS3.TRIANGLE)  operate=1;
        if(DS3.CIRCLE)    operate=2;

        /* 操縦権：なし 停止動作 */
        if(operate==0) {
            /* 足回り停止 */
            omuni.XmarkOmni_Move(0,0,0);
            for (int i = 0; i < 4; i++) {
                MD.Set(i,MovMotor[i]);
            }

        }
        /* 操縦権：手動 */
        else if(operate==1) {
            /* 足回り手動動作 */
            int x_val = (double)(DS3.LX-64)*100/64;
            int y_val = (double)(64-DS3.LY)*100/64;
            int r_val = (double)(DS3.RX-64)*100/64;

            SubOmuni(x_val, y_val, r_val);

            /* 目標角更新・ジャイロ補整 */
            if(DS3.RX!=64) yawCnt.reset();
            if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
            CompYaw();

        }
        /* 操縦権：自動 */
        else if(operate==2) {
            switch(auto_mode) {
                /* スタート待機処理 */
                case 0:
                    /* オンボードSWで終了処理，次のステップに */
                    if(UB){
                        auto_mode++;
                        auto_sequence=1;
                    }
                    break;

                /* 最初のスラローム */
                case 1:
                    /* 経路追従移動 */
                    if(omuni_control(
                        SlalomPoint[auto_sequence-1][0],SlalomPoint[auto_sequence-1][1],
                        SlalomPoint[auto_sequence][0],SlalomPoint[auto_sequence][1],
                        SlalomPoint[auto_sequence][2],SlalomPoint[auto_sequence][3])==0)
                        {
                            /* 次のポイントを目標に */
                            auto_sequence++;
                            /* 目標が最大数に達したら終了処理 */
                            if(auto_sequence==SlalomPointMAX){
                                auto_sequence=1;
                                auto_mode++;
                                }
                        }
                    break;

                /* 終了処理 */
                default:
                    auto_mode=0;
                    operate=0;
                    break;
            }
        }

    }
}

/* 割り込み(100us) *************************************************************/
void IT_CallBack(void)
{
    static int cnt = 0;
    static int data[EncoderMAX] = {0};
    static double EncDeg[EncoderMAX][2] = {0};

    for(int i=0; i<EncoderMAX; i++)
        switch(cnt) {
            /* 最初の処理 */
            case 0:
                data[i] = 0;
                CS[i] = 0;
                CL[i] = 1;
                break;
            /* 最後の処理 */
            case 25:
                CS[i]=1;
                /* 前回の値更新 今回の値更新(エンコーダの値(0~4096)を角度(0~360)に) */
                EncDeg[i][1] = EncDeg[i][0];
                EncDeg[i][0] = (double)data[i] * 360.0 / 4096;
                /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
                if ((270 <= EncDeg[i][1]) && (EncDeg[i][0] < 90))
                    EncDeg[i][1] -= 360;
                /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
                else if ((EncDeg[i][1] < 90) && (270 <= EncDeg[i][0]))
                    EncDeg[i][1] += 360;
                /* 差を求める*/
                EncoderDeg[i] += EncDeg[i][0] - EncDeg[i][1];
                break;
            /* 通常の処理 */
            default:
                CL[i]=!CL[i];
                /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
                if(cnt != 1 && cnt % 2) {
                    data[i] |= (DO[i]==1);
                    data[i] = data[i] << 1;
                }
                break;
        }
    cnt++;
    cnt%=26;
}


/*******************************************************************************
* @概要   自己位置推定関数
* @引数   なし
* @返り値 なし
*******************************************************************************/
void LocEstimate(void)
{
    static double GyroDeg[2]= {0};
    static double EncDeg[2][2]= {0};
    static double disp[3]= {0};

    /* ジャイロの値取得 */
    bno.get_angles();
    GyroDeg[1]=GyroDeg[0];
    GyroDeg[0]=bno.euler.yaw;
    if(GyroDeg[0]!=0) {
        /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
        if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
        /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
        else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
        /* 差を求める*/
        disp[2]=GyroDeg[1]-GyroDeg[0];
    }
    /* Enc2つの差分求める */
    for(int i=0; i<2; i++) {
        EncDeg[i][1]=EncDeg[i][0];
        EncDeg[i][0]=EncoderDeg[i];
        disp[i]=DEG_TO_DIS(EncDeg[i][1]-EncDeg[i][0]);
    }
    /* 差分を加速度として保存 */
    NowAcc.theta = disp[2];
    NowAcc.X = -disp[0] * cos(DEG_TO_RAD(NowLoc.theta)) - disp[1] * sin(DEG_TO_RAD(NowLoc.theta));
    NowAcc.Y = -disp[0] * sin(DEG_TO_RAD(NowLoc.theta)) + disp[1] * cos(DEG_TO_RAD(NowLoc.theta));
    /* 差分を累積して現在位置を保存 */
    NowLoc.X += NowAcc.X;
    NowLoc.Y += NowAcc.Y;
    NowLoc.theta += NowAcc.theta;
}


/*******************************************************************************
* @概要   オムニの値計算する(加算式)
* @引数   X,Y,Rotationそれぞれの操作量
* @返り値 なし
*******************************************************************************/
void SubOmuni(int X,int Y,int R)
{
    /* 入力を100%に制限 */
    X=Rest(X,100);
    Y=Rest(Y,100);
    R=Rest(R,100);

    /* オムニ計算結果をtmpに */
    int tmp[4]= {0};

    /* 一度データをtmpに保存 */
    for(int i=0; i<4; i++)
        tmp[i]=MovMotor[i];

    /* オムニ計算 */
    omuni.XmarkOmni_Move(X,Y,R);

    /* 計算結果を加算する */
    for(int i=0; i<4; i++)
        MovMotor[i]+=tmp[i];
}


/*******************************************************************************
* @概要   yaw角補整
* @引数   tar 目標角度
* @返り値 なし
*******************************************************************************/
void CompYaw(void)
{
    double r_val = (TarTheta-NowLoc.theta)*4;
    SubOmuni(0,0, r_val);
}


/*******************************************************************************
 * @概要   OEI式座標追従・P制御
 * @引数   X0,Y0      :直線始点のx,y座標
 * @引数   X1,Y1      :直線終点のx,y座標
 * @引数   tar_theta  :目標角
 * @引数   error      :次ステップに進む基準とする距離
 * @返り値 0(継続) 1(移動完了)
*******************************************************************************/
int omuni_control(int X0,int Y0,int X1,int Y1,double tar_theta, int error)
{

    /* 最初の角度・移動距離を記録しておく */
    static int log_distance = 0, log_theta = 0;
    static bool dis_flag = 0;

    if(dis_flag==0) {
        /* 現在の角度・移動距離を記録しておく */
        log_theta = NowLoc.theta;
        log_distance = sqrt(pow((double)X1 - NowLoc.X, 2.0) + pow((double)Y1 - NowLoc.Y, 2.0))-error;
        /* フラグ回収 */
        dis_flag=1;
    }

    /* 線の始点代入 */
    double start[2] = {X0,Y0};
    /* 線の終点代入 */
    double end[2] = {X1,Y1};
    /* 探査円の中心と半径代入 */
    double circle[3] = {
        NowLoc.X,NowLoc.Y,
        sqrt(pow(NowAcc.X,2)+pow(NowAcc.Y,2)) * 1.5
    };

    /* 残り距離計算 */
    double remain = (int)sqrt(pow((double)X1 - NowLoc.X, 2.0) + pow((double)Y1 - NowLoc.Y, 2.0));

    /* 探査円半径を残り距離以下に */
    if(circle[2]>remain)
        circle[2]=remain;

    /* 探査円と経路直線との交点 */
    double P_tar[2] = {0};

    /* 目標点の探索、代入 */
    lncl(start, end, circle, P_tar);


    /* X,Y,Rそれぞれの操作量 */
    double x_val = 0,y_val = 0;

    /* 減速処理の値代入  */
    x_val = ABS(P_tar[0] - NowLoc.X) * 0.06;
    y_val = ABS(P_tar[1] - NowLoc.Y) * 0.06;

    /* 最低・最高を決める */
    x_val = M_MAX(x_val, 20);
    x_val = M_MIN(x_val, 100);
    y_val = M_MAX(y_val, 20);
    y_val = M_MIN(y_val, 100);

    /* 正負を決める */
    if ((P_tar[0] - NowLoc.X) < 0)
        x_val *= -1;
    if ((P_tar[1] - NowLoc.Y) < 0)
        y_val *= -1;

    /* オムニ計算 */
    SubOmuni(x_val, y_val,0);


    /* 移動量に応じたその瞬間の合わせるthetaを求める */
    TarTheta = (((double)log_distance - (remain - error)) / log_distance) * (tar_theta - log_theta) + log_theta;
    /* thetaに合わせる */
    CompYaw();

    /* 残り距離がほぼないなら終了 */
    if(ABS(error - remain)<3) {
        /* Theta基準値を最終に */
        TarTheta=tar_theta;
        return 0;
    }
    return remain;
}


/*******************************************************************************
 * @概要   円と線の交点を求める
 *         なんかもうわかんない．あんまコメントアウトは参考にしないように
 * @引数   *pt1     :直線始まりの座標配列の先頭アドレス
 * @引数   *pt2     :直線終わりの座標配列の先頭アドレス
 * @引数   *xyr     :円のXY半径の座標配列の先頭アドレス
 * @引数   xy       :直線と円の交点の座標配列の先頭アドレス
 * @返り値 交点の個数(-1…エラー)
*******************************************************************************/
int lncl(double *pt1, double *pt2, double *xyr, double *xy)
{
    double  root, factor, xo, yo, f, g, fsq, gsq, fgsq, xjo, yjo, a, b, c;
    double  fygx, fxgy, t, fginv, t1, t2, x1, y1, x2, y2, sqdst1, sqdst2;
    double  accy = 1.0E-15;

    /* X,Yそれぞれの変化量 */
    double xlk = pt2[0] - pt1[0];
    double ylk = pt2[1] - pt1[1];
    /* 偏差 */
    double rsq = sqrt(pow(xlk, 2.0) + pow(ylk, 2.0));

    /* 0の除算時異常終了 */
    if (rsq < accy) return (-1);
    double rinv = 1.0 / rsq;

    /* ax+by+cの式の係数をそれぞれ */
    a = -ylk * rinv;
    b = xlk * rinv;
    c = (pt1[0] * pt2[1] - pt2[0] * pt1[1]) * rinv;

    /* rootは1になる */
    root = 1.0 / (a * a + b * b);
    /* 直線の法線ベクトルと直線上の点の位置ベクトルとの内積 */
    factor = -c * root;
    xo = a * factor;
    yo = b * factor;
    root = sqrt(root);
    /* xについての関数fとyについてg */
    f = b * root;
    g = -a * root;
    /* 2乗バージョン */
    fsq = f * f;
    gsq = g * g;
    /* 2つの合成 */
    fgsq = fsq + gsq;

    xjo = xyr[0] - xo;
    yjo = xyr[1] - yo;
    fygx = f * yjo - g * xjo;
    root = xyr[2] * xyr[2] * fgsq - fygx * fygx;

    /* 2つの関数が近い 3点で接する時異常終了 */
    if (fgsq < accy) return (3);
    /* 円と線が接さない 0点で接する時異常終了 */
    else if (root < -accy)  return (-1);
    /* 円と線が正接 1点で接する */
    else if (root < accy) {
        fxgy = f * xjo + g * yjo;
        t = fxgy / fgsq;
        xy[0] = xo + f * t;
        xy[1] = yo + g * t;
        return (1);
    }
    /* 円と線が交差する 2点で接する */
    else {
        fxgy = f * xjo + g * yjo;
        root = sqrt(root);
        fginv = 1.0 / fgsq;
        /* 根号の+-の2通り求める */
        t1 = (fxgy - root)*fginv;
        t2 = (fxgy + root)*fginv;
        x1 = xo + f * t1;
        y1 = yo + g * t1;
        x2 = xo + f * t2;
        y2 = yo + g * t2;
        /*終点と距離をとる √は省略 */
        sqdst1 = pow((pt2[0] - x1), 2.0) + pow((pt2[1] - y1), 2.0);
        sqdst2 = pow((pt2[0] - x2), 2.0) + pow((pt2[1] - y2), 2.0);
        /* 終点までの距離が近い方を採用 */
        if (sqdst1 < sqdst2) {
            xy[0] = x1;
            xy[1] = y1;
        } else {
            xy[0] = x2;
            xy[1] = y2;
        }
        return (2);
    }
}