File content as of revision 19:0840e182f96d:

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

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

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

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

/* 定数定義 ------------------------------------------------------------------*/
/* マクロ定義 ----------------------------------------------------------------*/
/* 関数プロトタイプ宣言 -------------------------------------------------------*/

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

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

/* 壁あて */
void ToWall(void);

/* x座標移動 */
int XCooMove(int Tar,int Err);

/* ライン補正 */
int LineCor(int Line_Dire);

/*  */
void SubOmuni(int X,int Y,int R);

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

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

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

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

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

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

/* ライン補正移動方向 */
int Line_Dire=0;

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

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

/* 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};

/* ジャイロ用タイマー */
Timer yawCnt;

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

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

/* I2C Inputのクラス定義 */
YKNCT_I2C_IN In(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);

    /* 足リミット */
    In.Init(0,0);
    In.Init(0,1);

    /* ラインセンサ */
    In.Init(0,2);
    In.Init(0,3);

    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();


        /* オムニ分のMD代入 */
        OmuniExe;

        for(int i=0; i<4; i++) {
            MD.Set(i,MovMotor[i]);
            MovMotor[i]=0;
        }

        /* I2CMD実行 */
        MD.Exe();

        /* ジャイロ用タイマースタート */
        yawCnt.start();
        /* ジャイロ用タイマーリセット */
        yawCnt.reset();

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

        /* 操縦権：なし 停止動作 */
        if(operate==0) {
            /* 足回り停止 */
            SubOmuni(0,0,0);

        }
        /* 操縦権：手動 */
        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;

            /* タイマーをリセット */
            if(DS3.RX!=64) yawCnt.reset();
            /* 目標角度再設定 */
            if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
            /* r_val補正 */
            r_val+=(TarTheta-NowLoc.theta)*CONST_CORRECTION_YAW;

            SubOmuni(x_val,y_val,r_val);

            /* 壁あて実行 */
            if(DS3.R1) ToWall();
        }
        /* 操縦権：自動 */
        else if(operate==2) {
            switch(auto_mode) {
                /* スタート待機処理 */
                case 0:
                    /* オンボードSWで次のステップに */
                    if(UB) auto_mode++;
                    break;

                /* X座標移動処理 */
                case 1:
                    /* 動作終了時次のステップに */
                    if(XCooMove(1000,50)==0)
                        auto_mode++;

                    break;

                /* ライン補正処理 */
                case 2:
                    /* ライン補正完了後次のステップに */
                    if(LineCor(Line_Dire)==1)
                        auto_mode++;

                    break;

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

    }
}


/*******************************************************************************
* @概要   自己位置推定関数
* @引数   なし
* @返り値 なし
*******************************************************************************/
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];
    }
    /* Enc1つの差分求める */
    for(int i=0; i<1; i++) {
        EncDeg[i][1]=EncDeg[i][0];
        EncDeg[i][0]=EncoderDeg[i];
        disp[i]=EncDeg[i][1]-EncDeg[i][0];
    }
    /* 差分を加速度として保存 */
    NowAcc.theta = disp[2];
    NowAcc.X =  -disp[0] * cos(NowLoc.theta);

    /* 差分を累積して現在位置を保存 */
    NowLoc.X += NowAcc.X;
    NowLoc.theta += NowAcc.theta;
}




/* 割り込み(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 ToWall(void)
{
    /* 左側リミットに応じて左2輪の操作量を決める */
    if(In.Get(0)==0)
        for(int i=0; i<2; i++) MovMotor[i]-=10;

    /* 右側リミットに応じて左2輪の操作量を決める */
    if(In.Get(1)==0)
        for(int i=2; i<4; i++) MovMotor[i]+=10;
}



/*******************************************************************************
* @概要    x座標移動関数
* @引数    Tar:目標値
* @引数    Err  :許容誤差
* @返り値  val_p:残り距離
*******************************************************************************/
int XCooMove(int Tar,int Err)
{
    /* x座標移動初期化フラグ */
    static bool XCooMoveFlag=0;

    /* x座標移動タイマー */
    static Timer CooCnt;

    /* 速度 加速・減速・最終操作量 */
    int val_p=0,val_t=0,val=0;

    /* X座標移動最初だけ初期化 */
    if(XCooMoveFlag==0) {
        /* タイマースタート */
        CooCnt.start();
        /* X座標移動用タイマーリセット */
        CooCnt.reset();
        /* 初期化フラグ立てる */
        XCooMoveFlag=1;
    }

    /* 加速・減速それぞれ求める */
    val_p=(Tar-NowLoc.X)*CONST_ACC_X;
    val_t=CooCnt.read_ms()*CONST_DEC_X;

    /* 小さい方を操作量として採択 */
    val=MIN(val_p,val_t);

    /* -100~100に調整 */
    if(val>100) val=100;
    else if(val<-100) val=-100;

    /* X方向のみ操作量を与えてオムニ代入 */
    SubOmuni(val,0,0);

    /* 残り距離求める */
    int RemDis=Tar-NowLoc.X;

    /* 終了条件満たしているなら終了処理 */
    if(RemDis<=Err) {
        /* フラグ回収 */
        XCooMoveFlag=0;
        return 0;
    }
    /* 満たしてないなら残り距離返す */
    else
        return RemDis;
}




/*******************************************************************************
* @概要   オムニの値計算する(加算式)
* @引数   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];
}




/*******************************************************************************
* @概要    ライン補正関数
* @引数    なし
* @返り値  なし
*******************************************************************************/
int LineCor(int Line_Dire)
{
    /* xの操作量 */
    int val_x=0;
    /* 左側,右側の前回の値 */
    static int Last_val;
    /* ライン補正初期化フラグ */
    static bool LineFlag=0;

    if(LineFlag==0) {
        /* 初期値設定 0…左 1…右 */
        Last_val=Line_Dire;
        /* 初期化フラグ立てる */
        LineFlag=1;
    }

    /* どちらもラインに乗っていない状態 */
    if(In.Get(2)==0&&In.Get(3)==0) {
        if(Last_val==0) val_x-=10;
        if(Last_val==1) val_x+=10;
    }

    /* 左側のみ */
    if(In.Get(2)==1&&In.Get(3)==0) {
        val_x-=10;
        Last_val=0;
    }
    /* 右側のみ */
    else if(In.Get(2)==0&&In.Get(3)==1) {
        val_x+=10;
        Last_val=1;
    }
    /* どちらも乗っている */
    else
        /* フラグ回収 */
        LineFlag=0;


    SubOmuni(val_x,0,0);

    /* 到達 */
    if(LineFlag==0) return 1;
    /* 未到達 */
    else return 0;
}