File content as of revision 0:c527a14d991a:

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

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

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

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


/* 関数プロトタイプ宣言
 * --------------------------------------------------------*/
void uartRX(void);

void IT_CallBack(void);

/* PID */
double LocationPID(double, double);

/* 値制限 */
double Range(double, double);

/* 角速度算出関数 */
int RotationPerWhile(int, int, int);

/* 足回り */
void InverseTriOmni(int *, double, double, double);

/* 自己位置推定関数 */
void LocEstimate(void);

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

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

/* 周期保存用 */
int whileTime_us = 0;

/* コントローラ */
int operate = 0;

/* ジャグリング機構 動かすか否か */
int isMoveInjection = 0;

/* リボン回転機構 動かすか否か */
int isMoveRibbon = 0;

/* 足回りエンコーダー */
double EncoderDeg[EncoderMAX - 6] = {0};

/* 手動オムニ目標角 */
double TarTheta = 0;

/* 目標番号 */
int targetNum = 0;

/* タイマー関連 */
/* 自身の状況 1-warning 2-normal */
int statious = 2;

/* 時間[ms] */
double msTimer = 0;

/* メインタイマー制御 0-pause 1-start */
bool timerMode = 0;

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

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

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

/* XBee送受信 */
Serial UART(PC_10, PC_11, 9600);

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

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

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


DigitalOut led(LED2);
/* リボン射出ピン */
DigitalOut Rod(PC_2);

/* エンコーダーピン設定 */
DigitalOut CS(PB_4);
DigitalOut CL(PA_10);
DigitalIn Do[EncoderMAX] = {PB_12, PB_2,  PB_1, PB_15, PB_14, PB_13, PC_4, PB_5};
//DigitalIn Do[EncoderMAX] = {PB_15, PB_14, PB_13, PB_12, PB_2, PB_1, PC_4, PB_5};

/* カタパルト二台分の領域確保 */
INJECTION injection[2] = {
    INJECTION(PB_10, PA_8, PA_9),
    INJECTION(PA_4, PB_0, PC_1)
};  // pa_6,pa_7
//PA_8,PB_0

DigitalIn limitRibbonTop(PC_0);
DigitalIn limitRibbonBottom(PC_3);

Timer WaitNextShot;

/*----------------------------------- main ----------------------------------*/
int main()
{
    /* I2CMDの設定 */
    for (int i = 0; i < MotorMAX; i++) MD.Init(i, MD_SMB);
    for (int i = 0; i < EncoderMAX; i++) Do[i].mode(PullDown);

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

    /* uart受信割込み設定 */
    UART.attach(uartRX, Serial::RxIrq);

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

    /* 動作時間計測用 */
    Timer whileTimer;
    Timer yawCnt;

    /* 計測開始 */
    whileTimer.start();
    yawCnt.start();
    WaitNextShot.start();

    /* 小さくすると深くなる */
    injection[0].initalDeg = 29;
    injection[1].initalDeg = 52;

    injection[0].targetRps[0] = -28;
    injection[0].targetRps[1] = 28;
    injection[1].targetRps[0] = -25;
    injection[1].targetRps[1] = 25;


    /* メインループ ----------------------------------------------------------*/
    while (1) {

        led = !led;

        for (int i = 0; i < MotorMAX; i++) {
            MD.Set(i, MovMotor[i]);
            MovMotor[i] = 0;
        }
        for (int i = 0; i < 3; i++) {
            injection[0].moveMotor[i] = injection[1].moveMotor[i] = 0;
        }
        MD.Exe();

        /* 自己位置計算 */
        LocEstimate();

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

        /* STARTで座標系リセット */
        if (DS3.START) {
            NowLoc.reset();
            TarTheta=0.0;
        }

        /* CS送信ステータス更新 */
        statious=(operate==1)+1;

        /* 停止以外でジャグリングとリボンの処理 */
        if (operate != 0) {

            static int nextCnt = 0;

            if(isMoveInjection && WaitNextShot.read_ms() > STEP(targetNum) && !injection[(nextCnt+1)%2].isMove) {
                injection[nextCnt%2].isMove = 1;
                WaitNextShot.stop();
                nextCnt++;
            }


            for (int i = 0; i < 2; i++) {
                /* 動作状態を決める */
//                injection[i].isMove = isMoveInjection && injection[!i].WaitNextShot.read_ms() > step;
                injection[i].MainFunction();
            }

            /* 出力 */
            for (int i = 0; i < 6; i++) {
                MovMotor[i] = Range(injection[i / 3].moveMotor[i % 3], 100);
            }

            /* リボン */
            if(isMoveRibbon==1) MovMotor[9] = 70;//70
            else if(isMoveRibbon==2) {
                if(limitRibbonTop) MovMotor[10] = 50;
                else MovMotor[9] = 70;
            } else if(!limitRibbonBottom)MovMotor[9] = -60;

//            if(isMoveRibbon) {
//                if(limitRibbonTop) MovMotor[10] = 50;
//                else MovMotor[9] = 70;
//            } else {
//                if(!limitRibbonBottom) MovMotor[9] = -60;
//            }
        }

        /* 停止処理 */
        if (operate == 0) {
            for (int i = 0; i < MotorMAX; i++) MovMotor[i] = 0;
            for (int i = 0; i < 2; i++)
                injection[0].moveMotor[i] = injection[1].moveMotor[i] = 0;
            for (int i = 0; i < 2; i++)
                injection[0].updateVal[i][1] = injection[1].updateVal[i][1] = 0;
            Rod = 0;
            TarTheta=NowLoc.Theta;
        }

        /* 足回り自動 */
        else if (operate == 1) {
            targetNum = 1;
            while (Target[targetNum].Time < msTimer && targetNum < TargetMAX)
                targetNum++;

            if (targetNum == TargetMAX)
                operate = 0;
            else {
                double divTim = 0;
                divTim = (double)(msTimer - Target[targetNum - 1].Time) / (double)(Target[targetNum].Time - Target[targetNum - 1].Time);

                /* 時間毎の目標を算出する */
                LOCATION NowTar;
                NowTar.X = Target[targetNum - 1].X+(Target[targetNum].X - Target[targetNum - 1].X) * divTim;
                NowTar.Y = Target[targetNum - 1].Y+(Target[targetNum].Y - Target[targetNum - 1].Y) * divTim;
                NowTar.Theta = Target[targetNum - 1].Theta+(Target[targetNum].Theta - Target[targetNum - 1].Theta) * divTim;

                /* 目標までの必要諸量 */
                LOCATION moveGain;
                moveGain.X = LocationPID(NowTar.X, NowLoc.X);
                moveGain.Y = LocationPID(NowTar.Y, NowLoc.Y);
                moveGain.Theta = (NowTar.Theta - NowLoc.Theta)*-4;

                /* 三角オムニに出力する */
                InverseTriOmni(MovMotor, moveGain.X, moveGain.Y, moveGain.Theta);
                for(int i = 6; i < 9; i++)
                    MovMotor[i] = Range(MovMotor[i],100);

                isMoveInjection = Target[targetNum].optionInjection;
                isMoveRibbon = Target[targetNum].optionRibbon;
                Rod = Target[targetNum].optionRod;


//                pc.printf("num:%d ",targetNum);
//                pc.printf("rod:%d ",Target[targetNum].optionRod);
                // pc.printf("Tar:%4.0f %4.0f %1.2f ",Target[targetNum].X,Target[targetNum].Y,Target[targetNum].Theta);
                // pc.printf("NowTar:%4.0f %4.0f %1.2f ",NowTar.X,NowTar.Y,NowTar.Theta);
            }
        }

        /* 手動 */
        else if (operate == 2) {

            int x_val = (DS3.LX-64) * 100/64;
            int y_val = (64-DS3.LY) * 100/64;
            int r_val = (DS3.RX-64) * 100/64;

            /* 目標角更新 */
            if (DS3.RX != 64) yawCnt.reset();
            if (yawCnt.read_ms() < 1000) TarTheta = NowLoc.Theta;
            /* gyro値による補正 */
            r_val += (TarTheta - NowLoc.Theta) * -4.0;

            InverseTriOmni(MovMotor, x_val, y_val, r_val);

            isMoveInjection = DS3.R1;
            isMoveRibbon = DS3.R2+DS3.L2;
            Rod = DS3.L1;
        }


        else if(operate == 3) {
            isMoveInjection = DS3.R1;
            isMoveRibbon = DS3.R2+DS3.L2;
            Rod = DS3.L1;
        }

        /* 改行を入れる */
        pc.printf("\n\r");

//         pc.printf("Loc:%4.0f %4.0f %1.2f  ",NowLoc.X,NowLoc.Y,NowLoc.Theta);
//         pc.printf("isRibbon:%d ", isMoveRibbon);
//         pc.printf("isIn:%d ", isMoveInjection);
        /* データチェック */
//         pc.printf("ope:%d  ", operate);
//         pc.printf("tim:%6d ",msTimer);
//         pc.printf("tim:%6d ",whileTime_us);
        // pc.printf("act[0]:%4d %4d", injection[0].actNum,
        //           injection[0].WaitNextShot.read_ms());
        // pc.printf("act[1]:%4d %4d", injection[1].actNum,
        //           injection[1].WaitNextShot.read_ms());

        // pc.printf("BallShotTim:%d  ", injection[1].BallShotTim.read_ms());
        // pc.printf("lim:%4d  ",injection[1].limitMagazine!=0);
        // pc.printf("isBallLoading:%d  ", injection[1].isBallLoading);
        pc.printf("Motor %d %d %d ", MovMotor[6], MovMotor[7], MovMotor[8]);

//        pc.printf("Motor %d %d ", MovMotor[9], MovMotor[10]);

//         pc.printf("Enc %3.0f %3.0f ",EncoderDeg[0],EncoderDeg[1]);
        for (int i = 0; i < 2; i++) {
//             pc.printf("inj[%d]is:%d act:%d ", i, injection[i].isMove,
//                       injection[i].actNum);
            // pc.printf("BallLoad:%4d %d ", injection[i].BallLoadTim.read_ms(),
            //           injection[i].isBallLoading);
//            pc.printf("Enc %3.0f %3.0f %3.0f ", injection[i].encDeg[0],
//                      injection[i].encDeg[1], injection[i].encDeg[2]);
//            pc.printf("Enc %3.0f %3.0f ", injection[i].encDeg[0], injection[i].encDeg[0]);
//                 pc.printf("Lim %d %d ",
//                injection[i].limitTop!=0,injection[i].limitBottom!=0);
//            pc.printf("Mo %d %d %d ", injection[i].moveMotor[0], injection[i].moveMotor[1], injection[i].moveMotor[2]);
//            pc.printf("is %d ", injection[i].BallLoadTim.read_ms());
//            pc.printf("Ribbon %d ",isMoveRibbon);
        }

        /* 周期を保存 */
        whileTime_us = whileTimer.read_us();
        if(timerMode) msTimer+=(double)whileTimer.read()*1000.0;
        // if(timerMode) msTimer+=whileTimer.read_ms()+1;
        if(whileTime_us>50000) I2C hoge(PB_9, PB_8);
        /* 周期リセット */
        whileTimer.reset();
    }  // while
}  // main




/*******************************************************************************
 * @概要　　射出機構のメイン関数
 * @引数　　なし
 * @返り値　なし
 *******************************************************************************/
void INJECTION::MainFunction(void)
{
    /* 上のリミットと接触したらEncリセット */
    if (limitTop) encDeg[0] = encDeg[1] = 0;

    /* 下のリミットと接触したらエンコーダー角度を合わせる */
    if(limitBottom) {
        if(abs(encDeg[0]) > abs(encDeg[1])) encDeg[1] = -encDeg[0];
        else if(abs(encDeg[0]) < abs(encDeg[1])) encDeg[0] = -encDeg[1];
    }

    /* 装填機構のリミットと接触したら装填処理*/
    if (limitMagazine != 0) BallLoadTim.start();

    /* 装填機構動作 */
    moveMotor[2] = LoadingPID((double)initalDeg,encDeg[2]);

    switch (actNum) {
        case 0:

            /* 上昇したら終了 */
            if (encDeg[1] > 800) {
                for (int i = 0; i < 2; i++) {
                    velDev[i][0] = velDev[i][1] = velDev[i][2] = 0;
                    moveMotor[i] = 0;
                    updateVal[i][0] = updateVal[i][1] = 0;
                    EncDegDiff[i][0]=EncDegDiff[i][1]=0;
                }
                actNum++;

            } else if (BallShotTim.read_ms() >  250) {//350 //250
                /* 左右それぞれ一秒あたりの回転数を計算する */
                double rps[2]= {0};
                for (int i = 0; i < 2; i++) {
                    double angularVel = RotationPerWhile(encDeg[i], i);  //差分
                    rps[i] = (angularVel * 1000000.0 / whileTime_us) / 360.0;
                }

                /* 操作量を更新 */
                for (int i = 0; i < 2; i++) {
                    updateVal[i][0] = updateVal[i][1];
                    updateVal[i][1] =
                        updateVal[i][0] + VelocityPID(targetRps[i], rps[i], i);
                    /* 値を制限する */
                    moveMotor[i] = Range(updateVal[i][1], 100);

                }
            }
            break;

        case 1:
            /* 目標更新，装填機構動作 */
            if (BallLoadTim.read_ms() >400 && isMove) {//450 200
                isBallLoading = 1;
                initalDeg -= 120;
                isMove = 0;
                WaitNextShot.reset();
                WaitNextShot.start();
            }

            /* 装填機構を動作させる */
            if (isBallLoading) {
                BallLoadTim.stop();
                BallLoadTim.reset();

                if (limitTop) {
                    /* 誤差範囲なら終了フラグ立て */
                    /* 装填機構の目標角度更新 */
                    if (abs(initalDeg - encDeg[2]) < 5.0) {
                        isBallLoading = 0;
                        BallShotTim.reset();
                        actNum++;
                    }
                }
                /* 射出機構戻ってなかったら途中までで上書き */
                else {
                    /* 下降中に途中まで回す */
                    moveMotor[2] = LoadingPID(initalDeg+40, encDeg[2]);
                }
            }

            /* リミットに触れるまで下降 */
            if (limitTop) {
                for (int i = 0; i < 2; i++) moveMotor[i] = 0;
            } else if (encDeg[1] > 500) {
                for (int i = 0; i < 2; i++) {
                    moveMotor[i] = -400 - encDeg[i];
                    moveMotor[i] = Range(moveMotor[i], 70);
                }
            } else {
                /* 固定値で一番下まで下降 */
                for (int i = 0; i < 2; i++) moveMotor[i] = -10 * (i * 2 - 1);
            }
            break;

        default:
            actNum = 0;
            break;
    }  // switch
}

/*******************************************************************************
 * @概要　　速度型PID制御
 * @引数　　目標値, 現在値, モーター番号
 * @返り値　操作量
 *******************************************************************************/
double INJECTION::VelocityPID(double Target_value, double Current_value, int motorNum)
{
    /* 比例定数,積分定数,微分定数 */
    const double kp = 1.0;
    const double ki = 0.3;
    const double kd = 0.05;
    /* 比例項,積分項,微分項の計算結果を保存する変数*/

    /* 偏差を更新 */
    velDev[motorNum][2] = velDev[motorNum][1];
    velDev[motorNum][1] = velDev[motorNum][0];
    velDev[motorNum][0] = Target_value - Current_value;

    /* 比例項,積分項,微分項をそれぞれ求める */
    double P = (velDev[motorNum][0] - velDev[motorNum][1]) * kp;
    double I = velDev[motorNum][0] * ki;
    double D = ((velDev[motorNum][0] - velDev[motorNum][1]) -
                (velDev[motorNum][1] - velDev[motorNum][2])) *
               kd;

    /* 合計値を操作量として返す */
    return P + I + D;
}

/*******************************************************************************
 * @概要　　位置型PID制御
 * @引数　　目標値, 現在値
 * @返り値　操作量
 *******************************************************************************/
double LocationPID(double Target_value, double Current_value)
{
    /* 比例定数,積分定数,微分定数 */
    const double kp = 0.5;
    const double ki = 0;
    const double kd = 0;
    /* 比例項,積分項,微分項の計算結果を保存する変数*/
    static double P = 0;
    static double I = 0;
    static double D = 0;
    /* 偏差の一時保存領域 */
    static double dev[2] = {0};

    /* 偏差を更新 */
    dev[1] = dev[0];
    dev[0] = Target_value - Current_value;

    /* 比例項,積分項,微分項をそれぞれ求める */
    P = dev[0] * kp;
    I += dev[0] * ki;
    D = (dev[0] - dev[1]) * kd;

    /* ワインドアップ */
    if ((dev[0] < 0 && dev[1] > 0) || (dev[0] > 0 && dev[1] < 0)) I = 0;

    /* 合計値を操作量として返す */
    return P + I + D;
}

/*******************************************************************************
 * @概要　　位置型PID制御(装填機構)
 * @引数　　目標値, 現在値
 * @返り値　操作量
 *******************************************************************************/
double INJECTION::LoadingPID(double Target_value, double Current_value)
{
    /* 比例定数,積分定数,微分定数 */ //0.2 0.02 0.25 | 0.2 0.04 0.25
    const double kp = 1.0;
    const double ki = 0.06;
    const double kd = 0.9;
    /* 比例項,積分項,微分項の計算結果を保存する変数*/
    static double P = 0;
    static double I = 0;
    static double D = 0;

    /* 偏差を更新 */
    loadDev[1] = loadDev[0];
    loadDev[0] = Target_value - Current_value;

    /* 比例項,積分項,微分項をそれぞれ求める */
    P = loadDev[0] * kp;
    I += loadDev[0] * ki;
    D = (loadDev[0] - loadDev[1]) * kd;


    /* ワインドアップ */
    if ((loadDev[0] < 0 && loadDev[1] > 0) || (loadDev[0] > 0 && loadDev[1] < 0)) I = 0;

    /* 合計値を操作量として返す */
    return Range(P + Range(I,6) + D, 80);
}

/*******************************************************************************
 * @概要　　制限する
 * @引数　　対象,限界値
 * @返り値　制限後の値
 *******************************************************************************/
double Range(double value, double limit)
{
    if (value > limit)
        return limit;
    else if (value < -limit)
        return -limit;
    else
        return value;
}

/*******************************************************************************
 * @概要　　回転角度を求める関数
 * @引数　　角度, エンコーダー番号, 機体番号
 * @返り値　周期あたりの角速度
 *******************************************************************************/
double INJECTION::RotationPerWhile(double degree, int num)
{
    /* 前回の値更新 今回の値更新 */
    EncDegDiff[num][1] = EncDegDiff[num][0];
    EncDegDiff[num][0] = degree;

    /* 前回と現在の値を比べる */
    return EncDegDiff[num][0] - EncDegDiff[num][1];
}

/*******************************************************************************
 * @概要   自己位置推定関数
 * @引数   なし
 * @返り値 なし
 *******************************************************************************/
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 / 2.0;
    NowLoc.Y += NowAcc.Y / 2.0;
    NowLoc.Theta += NowAcc.Theta;
}

/*******************************************************************************
 * @概要　　回転角度を求める関数
 * @引数　　角度, エンコーダー番号, 機体番号
 * @返り値　周期あたりの角速度
 *******************************************************************************/
int Rotation(int degree, int num, int robotNum)
{
    static int EncDeg[2][2][2] = {0};
    static int NowRotate[2][2] = {0};

    /* 前回の値更新 今回の値更新 */
    EncDeg[robotNum][num][1] = EncDeg[robotNum][num][0];
    EncDeg[robotNum][num][0] = degree;

    /* 前回と現在の値を比べる */
    NowRotate[robotNum][num] =
        EncDeg[robotNum][num][0] - EncDeg[robotNum][num][1];
    return NowRotate[robotNum][num];
}

/*******************************************************************************
 * @概要　　逆三角オムニ関数
 * @引数　　モータの配列・x,y,r ベクトル
 * @返り値　なし
 ******************************************************************************/
void InverseTriOmni(int *duty, double x, double y, double r)
{
    int duty_max = 0;

    double rad = DEG_TO_RAD(NowLoc.Theta);
    duty[6] = x * cos(rad) + y * sin(rad);
    duty[7] = x * cos(rad + PI / 3.0 * 2) + y * sin(rad + PI / 3.0 * 2);
    duty[8] = -x * cos(rad + PI / 3.0) - y * sin(rad + PI / 3.0);

    duty_max = 0;
    for (int i = 6; i < 9; i++) {
        duty[i] += r;
        duty_max = max(duty_max, abs(duty[i]));
    }
    for (int i = 6; i < 9; i++)
        if (duty_max > 99) duty[i] = duty[i] * 99 / duty_max;
}


/* UART受信割り込み *************************************************************/
void uartRX()
{
    static int num = 0;
    static uint8_t buf[3] = {0};

    /* 受信データ */
    uint8_t data = UART.getc();
    if((data&0x80) != 0) {
        timerMode = (data & 0x40)!=0;

        //ビット結合
        uint32_t tmp = 0;
        for(int i=0; i<3; i++)
            tmp |= buf[i]<<(14-i*7);

        //チェックサム計算
        uint8_t checkSum = 0;
        for (int i = 0; i < 7; i++)
            checkSum += tmp / (int)pow(10.0, (double)i) % 10;

        //誤り検知陰性
        if(checkSum == (data&0x3F))
            msTimer = tmp;

        num = 0;
    } else {
        buf[num] = data;
        num++;
    }


    /* 送信データ */
    char sendData = 2<<2;
    sendData|=(statious&0b11);
    UART.putc(sendData);
}

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

    switch (cnt) {
        /* 最初の処理 */
        case 0:
            for (int i = 0; i < EncoderMAX; i++) data[i] = 0;
            CS = 0;
            CL = 1;
            break;
        /* 最後の処理 */
        case 25:
            CS = 1;

            for (int i = 0; i < EncoderMAX; i++) {
                /* 前回の値更新
                 * 今回の値更新(エンコーダの値(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;
                /* 差を求め，適当な変数に代入 */
                if (i < 6)
                    injection[i / 3].encDeg[i % 3] +=
                        EncDeg[i][0] - EncDeg[i][1];
                else
                    EncoderDeg[i - 6] -= EncDeg[i][0] - EncDeg[i][1];
            }
            break;
        /* 通常の処理 */
        default:
            CL = !CL;
            /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */

            for (int i = 0; i < EncoderMAX; i++)
                if (cnt != 1 && cnt % 2) {
                    data[i] |= (Do[i] == 1);
                    data[i] = data[i] << 1;
                }
            break;
    }
    cnt++;
    cnt %= 26;
}