Revision 0:ad3dd7fb650a, committed 2021-09-13

Comitter:

TakushimaYukimasa

Date:

Mon Sep 13 02:40:49 2021 +0000

Commit message:

makeTakushima

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BNO055.lib	Mon Sep 13 02:40:49 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/YKNCT/code/BNO055/#fe0b400ec8f5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SBDBT.lib	Mon Sep 13 02:40:49 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/YKNCT/code/SBDBT/#8c49a263e344

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/YKNCT_I2C.lib	Mon Sep 13 02:40:49 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/YKNCT/code/YKNCT_I2C/#77df03c9efce

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/YKNCT_MD.lib	Mon Sep 13 02:40:49 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/YKNCT/code/YKNCT_MD/#a0857a6544e4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Sep 13 02:40:49 2021 +0000
@@ -0,0 +1,726 @@
+/* Includes -----------------------------------------------------------------*/
+#include <main.h>
+#include <target.h>
+
+/* 型定義
+ * ---------------------------------------------------------------------*/
+
+/* ロボットの加速度 */
+LOCATION NowAcc;
+/* ロボットの座標 */
+LOCATION NowLoc;
+
+/* 出発地点 */
+COORDINATE Origin;
+/* 定数定義
+ * -------------------------------------------------------------------*/
+
+
+/* 関数プロトタイプ宣言
+ * --------------------------------------------------------*/
+void uartRX(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 Map(int, int, int, int, int);
+
+void IT_CallBack(void);
+
+/* 変数定義
+ * -------------------------------------------------------------------*/
+
+/* 足回り値保存変数 */
+int MovMotor[MotorMAX] = {0};
+
+/* 周期保存用 */
+int whileTime_us = 0;
+
+/* コントローラ */
+int operate = 0;
+
+/* ジャグリング機構 動かすか否か */
+int isMoveInjection = 0;
+
+double EncoderDeg[EncoderMAX - 6] = {0};
+
+/* 手動オムニ目標角 */
+double TarTheta = 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};
+
+/* カタパルト二台分の領域確保 */
+INJECTION injection[2] = {INJECTION(PB_10, PA_8, PA_9),
+                          INJECTION(PA_4, PB_0, PC_1)};  // pa_6,pa_7
+
+/*----------------------------------- 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);
+
+    /* ジャイロの設定 */
+    bno.setmode(OPERATION_MODE_IMUPLUS);
+
+    /* 動作時間計測用 */
+    Timer whileTimer;
+    Timer ActTimer;
+    Timer yawCnt;
+
+    /* 計測開始 */
+    whileTimer.start();
+    ActTimer.start();
+    yawCnt.start();
+
+    /* 機体ごとに初期目標保存 */
+    // for (int i = 0; i < 2; i++)
+    //     injection[i].initalDeg = injection[i].encDeg[2] - 120;
+    
+    /* 小さくすると深くなる */
+    injection[0].initalDeg = -3;
+    injection[1].initalDeg = -50;
+ 
+    injection[0].targetRps[0] = -25;
+    injection[0].targetRps[1] = 25;
+    injection[1].targetRps[0] = -25;
+    injection[1].targetRps[1] = 25;
+
+
+    /* メインループ ----------------------------------------------------------*/
+    while (1) {
+
+        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) {
+            for (int i = 0; i < 2; i++) {
+                /* 動作状態を決める */
+                injection[i].isMove = 
+                    isMoveInjection && injection[!i].WaitNextShot.read_ms() > 400;
+                injection[i].MainFunction(i);
+            }
+            /* 出力 */
+            for (int i = 0; i < 6; i++) {
+                MovMotor[i] = Range(injection[i / 3].moveMotor[i % 3], 100);
+            }
+        }
+
+        /* 停止処理 */
+        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;
+                TarTheta=NowLoc.Theta;
+        }
+
+        /* 足回り自動 */
+        else if (operate == 1) {
+            int 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;
+                Rod = Target[targetNum].optionRod;
+
+
+        // pc.printf("num:%d ",targetNum);
+        // 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 = Map(DS3.LX, 64, 127, 0, 100);
+            int y_val = Map(DS3.LY, 64, 127, 0, -100);
+            int r_val = Map(DS3.RX, 64, 127, 0, 100);
+
+            /* 目標角更新 */
+            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;
+
+            Rod = DS3.L1;
+        }
+
+        /* 実行処理 */
+        else if (operate == 3) {
+        }
+
+        /* 改行を入れる */
+        // pc.printf("\n\r");
+
+        // pc.printf("Loc:%4.0f %4.0f %1.2f  ",NowLoc.X,NowLoc.Y,NowLoc.Theta);
+        /* データチェック */
+        // pc.printf("ope:%d  ", operate);
+        // pc.printf("tim:%6d ",msTimer);
+        // 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("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("Lim %d %d ",
+            //    injection[i].limitTop!=0,injection[i].limitMagazine!=0);
+        }
+
+        /* 周期を保存 */
+        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(int num) {
+    /* 上のリミットと接触したらEncリセット */
+    if (limitTop) encDeg[0] = encDeg[1] = 0;
+
+    /* 装填機構のリミットと接触したら, 装填処理*/
+    if (limitMagazine != 0) BallLoadTim.start();
+
+    /* 装填機構動作 */
+    moveMotor[2] = LoadingPID((double)initalDeg,encDeg[2]);
+
+    // pc.printf("%4d,%4.1f,",initalDeg,encDeg[2]);
+    // if(num==1)
+    // pc.printf("\n\r");
+
+static double array[100][2][2]={0};
+static int cnt = 0;
+static int flag = 0;
+if(DS3.L2) flag=1;
+if(flag){
+    for(int i = 0;i < cnt; i++)
+        for(int j = 0;j < 10; j++)
+            pc.printf("%f,%f,%f,%f\n\r",array[i][0][0],array[i][0][1],array[i][1][0],array[i][1][1]);
+                    flag=0;
+                    cnt=0;
+                }
+    switch (actNum) {
+        case 0:
+
+            /* 上昇したら終了 */
+            if (encDeg[0] < -900) {
+                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() > 300) {
+                /* 左右それぞれ一秒あたりの回転数を計算する */
+                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);
+
+                    array[cnt][i][0]=(double)targetRps[i];
+                    array[cnt][i][1]=rps[i];
+                }
+                    cnt++;
+            }
+            break;
+
+        case 1:
+            /* 目標更新，装填機構動作 */
+            if (BallLoadTim.read_ms() > 450 && isMove) {
+                isBallLoading = 1;
+                initalDeg -= 120;
+                WaitNextShot.reset();
+            }
+
+            /* 装填機構を動作させる */
+            if (isBallLoading) {
+                BallLoadTim.stop();
+                BallLoadTim.reset();
+
+                if (limitTop) {
+                    /* 誤差範囲なら終了フラグ立て */
+                    /* 装填機構の目標角度更新 */
+                    if (abs(initalDeg - encDeg[2]) < 6.0) {
+                        isBallLoading = 0;
+                        BallShotTim.reset();
+                        actNum++;
+                    }
+                }
+                /* 射出機構戻ってなかったら途中までで上書き */
+                else {
+                    /* 下降中に途中まで回す */
+                    moveMotor[2] = initalDeg + 40 - encDeg[2];
+                    moveMotor[2] = Range(moveMotor[2], 20);
+                }
+            }
+
+            /* リミットに触れるまで下降 */
+            if (limitTop) {
+                for (int i = 0; i < 2; i++) moveMotor[i] = 0;
+            } 
+            else if (encDeg[0] < -500) {
+                for (int i = 0; i < 2; i++) {
+                    moveMotor[i] = -400 - encDeg[i];
+                    moveMotor[i] = Range(moveMotor[i], 65);
+                }
+            } else {
+                /* 固定値で一番下まで下降 */
+                for (int i = 0; i < 2; i++) moveMotor[i] = -20 * (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;
+
+    /* ワインドアップ */
+    // if ((velDev[motorNum][0] < 0 && velDev[motorNum][1] > 0) ||
+    //     (velDev[motorNum][0] > 0 && velDev[motorNum][1] < 0))
+    //     I = 0;
+
+    /* 合計値を操作量として返す */
+    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 = 0.3;
+    const double ki = 0.04;
+    const double kd = 0.25;
+    /* 比例項,積分項,微分項の計算結果を保存する変数*/
+    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,3) + D,50);
+}
+
+/*******************************************************************************
+ * @概要　　制限する
+ * @引数　　対象,限界値
+ * @返り値　制限後の値
+ *******************************************************************************/
+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;
+}
+
+/*******************************************************************************
+ * @概要　　制限する
+ * @引数　　対象,限界値
+ * @返り値　制限後の値
+ *******************************************************************************/
+double range(double value, double limit) {
+    if (value > limit)
+        return limit;
+    else if (value < -limit)
+        return -limit;
+    else
+        return value;
+}
+
+/*******************************************************************************
+ * @概要　　回転角度を求める関数
+ * @引数　　角度, エンコーダー番号, 機体番号
+ * @返り値　周期あたりの角速度
+ *******************************************************************************/
+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;
+}
+
+/***************************************************************************
+ * @概要　　マッピング
+ * @引数　　対象, 範囲
+ * @返り値　マッピング後の値
+ ******************************************************************************/
+int Map(int value, int fromMin, int fromMax, int toMin, int toMax) {
+    return (value - fromMin) * (toMax - toMin) / (fromMax - fromMin) + toMin;
+}
+
+/* UART受信割り込み ******************************************/
+void uartRX() {
+    /* 受信データ */
+    static char data[2] = {0};
+    data[1] = data[0];
+    data[0] = UART.getc();
+
+    timerMode = data[0] >> 7;
+
+    if (!timerMode && ((data[0] & 0x7F) != (data[1] & 0x7F)))
+        msTimer = (data[0] & 0x7F) * 1000.0;
+
+    /* 送信データ */
+    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++) {
+                //            pc.printf("[%d]:%4d ",i,data[i]);
+                //            if(i==EncoderMAX-1) pc.printf("\n\r");
+                /* 前回の値更新
+                 * 今回の値更新(エンコーダの値(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;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Mon Sep 13 02:40:49 2021 +0000
@@ -0,0 +1,126 @@
+/* Includes ------------------------------------------------------------------*/
+
+#ifndef MAIN_H
+#define MAIN_H
+
+#include <mbed.h>
+#include <BNO055.h>
+#include <SBDBT.h>
+#include <YKNCT_I2C.h>
+#include <YKNCT_MD.h>
+// #include <encoder.h>
+
+/* 型定義 --------------------------------------------------------------------*/
+
+struct LOCATION {
+    double X;
+    double Y;
+    double Theta;
+    int msTime;
+    
+public:
+    /* 全部リセットしちゃうぞぉ♡ */
+    void reset(void){
+        X=-2000;
+        Y=0;
+        Theta=0;
+        msTime=0;
+    }
+};
+
+struct COORDINATE {
+    double X;
+    double Y;
+    double Theta;
+    int Time;
+    bool optionInjection;
+    bool optionRod;
+    bool optionRibbon;
+};
+
+
+/* 射出管理構造体 */
+class INJECTION
+{
+public:
+    INJECTION(PinName p1,PinName p2,PinName p3) 
+    : limitTop(p1),limitBottom(p2),limitMagazine(p3){
+
+        BallShotTim.start();
+        WaitNextShot.start();
+
+        encDeg[0]=0;
+        encDeg[1]=0;
+        encDeg[2]=0;
+        actNum=1;
+        
+        targetRps[0]=-35;
+        targetRps[1]=35;
+    }
+    
+    int actNum;             // 射出シーケンス
+    int initalDeg;          // 装填機構の目標角度
+    double encDeg[3];          // 機体別の現在角度保存領域
+    int moveMotor[3];       // 機体別モーター値保存領域
+    double updateVal[2][2]; // 速度型PID
+    int isBallLoading;      //ボールが装填準備状態になっているか
+    
+    int isMove;
+    
+    double EncDegDiff[2][2];
+
+    /* 速度PID 偏差の一時保存領域 */
+    double velDev[2][3];
+
+    /* 速度PID 偏差の一時保存領域 */
+    double loadDev[2];
+    
+    /* 目標回転数 */
+    int targetRps[2];
+
+    DigitalIn limitTop;//上リミット
+    DigitalIn limitBottom;//下リミット
+    DigitalIn limitMagazine;//装填リミット
+
+    Timer BallLoadTim;  //グルグルに乗るまで
+    Timer BallShotTim;  //グルグルからカップに乗るまで
+    Timer WaitNextShot; //
+    
+    void MainFunction(int num);
+    double RotationPerWhile(double degree, int num);
+    double VelocityPID(double Target_value, double Current_value, int motorNum);
+    double LoadingPID(double Target_value, double Current_value);
+private:
+
+};
+
+/* 定数定義 ------------------------------------------------------------------*/
+
+/* π */
+#define PI 3.1415926535
+
+/* モーターの最大数 */
+#define MotorMAX 9
+
+/* エンコーダーの最大数 */
+#define EncoderMAX 8
+
+
+/* マクロ定義 ----------------------------------------------------------------*/
+
+/* Enc角度から距離に変換 */
+#define DEG_TO_DIS(__DEGREE__) (100 * PI * (__DEGREE__) / 360)
+
+/* radian degree 変換 */
+#define DEG_TO_RAD(__DEGREE__) ((__DEGREE__)*PI / 180)
+
+#define ROUND(_DATA_) ((int)((_DATA_)*10)%10<=3?(_DATA_):(_DATA_)+1)
+
+/* コントローラー SerectとStartを実装 */
+//#define Select (DS3.LEFTkey+DS3.RIGHTkey==2)
+//#define Start (DS3.UPkey+DS3.DOWNkey==2)
+
+/* 関数プロトタイプ宣言 -------------------------------------------------------*/
+/* 変数定義 ------------------------------------------------------------------*/
+/* クラス定義 ----------------------------------------------------------------*/
+#endif /* MAIN_H */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Sep 13 02:40:49 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/target.h	Mon Sep 13 02:40:49 2021 +0000
@@ -0,0 +1,56 @@
+#include <main.h>
+
+/* 目標地点数 */
+const int TargetMAX = 46;
+
+/* 目標座標 X,Y,Theta,Time(ms) ジャグ機構 竿射出 リボン回転 */
+const COORDINATE Target[TargetMAX] = {
+    {2000*cos(8/8.0 * PI), 2000*sin(8/8.0 * PI), 0, 20000*0/4 ,0 ,0 ,0},
+    {2000*cos(7/8.0 * PI), 2000*sin(7/8.0 * PI), 0, 20000*1/4 ,0 ,0 ,0},
+    {2000*cos(6/8.0 * PI), 2000*sin(6/8.0 * PI), 0, 20000*2/4 ,0 ,0 ,0},
+    {2000*cos(5/8.0 * PI), 2000*sin(5/8.0 * PI), 0, 20000*3/4 ,0 ,0 ,0},
+    {2000*cos(4/8.0 * PI), 2000*sin(4/8.0 * PI), 0, 20000*4/4 ,0 ,0 ,0},
+    {0, 2000, 0, 37000 ,1 ,0 ,0},
+    {0, 2000, 0, 40000 ,0 ,0 ,0},
+    {2000*cos(5/8.0 * PI), 2000*sin(5/8.0 * PI), 0, 40000+5000*1/4 ,0 ,0 ,0},
+    {2000*cos(6/8.0 * PI), 2000*sin(6/8.0 * PI), 0, 40000+5000*2/4 ,0 ,0 ,0},
+    {2000*cos(7/8.0 * PI), 2000*sin(7/8.0 * PI), 0, 40000+5000*3/4 ,0 ,0 ,0},
+    {2000*cos(8/8.0 * PI), 2000*sin(8/8.0 * PI), 0, 40000+5000*4/4 ,0 ,0 ,0},
+    {-2000, -2000, 0, 50000 ,0 ,0 ,0},
+    {2000, -2000, 0, 60000 ,0 ,0 ,1},//上げる
+    {2000, -2000, 720, 70000 ,0 ,0 ,1},//回転
+    {2000, 0, 720, 75000 ,0 ,0 ,0},//円開始
+    {2000, 0, 720, 76000 ,0 ,0 ,0},//円開始
+    {2000*cos(16/8.0 * PI), 2000*sin(16/8.0 * PI), 720, 76000+22000*0/24 ,0 ,0 ,0},
+    {2000*cos(15/8.0 * PI), 2000*sin(15/8.0 * PI), 720, 76000+22000*1/24 ,0 ,0 ,0},
+    {2000*cos(14/8.0 * PI), 2000*sin(14/8.0 * PI), 720, 76000+22000*2/24 ,0 ,0 ,0},
+    {2000*cos(13/8.0 * PI), 2000*sin(13/8.0 * PI), 720, 76000+22000*3/24 ,0 ,0 ,0},
+    {2000*cos(12/8.0 * PI), 2000*sin(12/8.0 * PI), 720, 76000+22000*4/24 ,0 ,0 ,0},
+    {2000*cos(11/8.0 * PI), 2000*sin(11/8.0 * PI), 720, 76000+22000*5/24 ,0 ,0 ,0},
+    {2000*cos(10/8.0 * PI), 2000*sin(10/8.0 * PI), 720, 76000+22000*6/24 ,0 ,0 ,0},
+    {2000*cos(9/8.0 * PI), 2000*sin(9/8.0 * PI), 720, 76000+22000*7/24 ,0 ,0 ,0},
+    {2000*cos(8/8.0 * PI), 2000*sin(8/8.0 * PI), 720, 76000+22000*8/24 ,0 ,0 ,0},
+    {2000*cos(7/8.0 * PI), 2000*sin(7/8.0 * PI), 720, 76000+22000*9/24 ,0 ,0 ,0},
+    {2000*cos(6/8.0 * PI), 2000*sin(6/8.0 * PI), 720, 76000+22000*10/24 ,0 ,0 ,0},
+    {2000*cos(5/8.0 * PI), 2000*sin(5/8.0 * PI), 720, 76000+22000*11/24 ,0 ,0 ,0},
+    {2000*cos(4/8.0 * PI), 2000*sin(4/8.0 * PI), 720, 76000+22000*12/24 ,0 ,0 ,0},
+    {2000*cos(3/8.0 * PI), 2000*sin(3/8.0 * PI), 720, 76000+22000*13/24 ,0 ,0 ,0},
+    {2000*cos(2/8.0 * PI), 2000*sin(2/8.0 * PI), 720, 76000+22000*14/24 ,0 ,0 ,0},
+    {2000*cos(1/8.0 * PI), 2000*sin(1/8.0 * PI), 720, 76000+22000*15/24 ,0 ,0 ,0},
+    {2000*cos(0/8.0 * PI), 2000*sin(0/8.0 * PI), 720, 76000+22000*16/24 ,0 ,0 ,0},
+    {2000*cos(-1/8.0 * PI), 2000*sin(-1/8.0 * PI), 720, 76000+22000*17/24 ,0 ,0 ,0},
+    {2000*cos(-2/8.0 * PI), 2000*sin(-2/8.0 * PI), 720, 76000+22000*18/24 ,0 ,0 ,0},
+    {2000*cos(-3/8.0 * PI), 2000*sin(-3/8.0 * PI), 720, 76000+22000*19/24 ,0 ,0 ,0},
+    {2000*cos(-4/8.0 * PI), 2000*sin(-4/8.0 * PI), 720, 76000+22000*20/24 ,0 ,0 ,0},
+    {2000*cos(-5/8.0 * PI), 2000*sin(-5/8.0 * PI), 720, 76000+22000*21/24 ,0 ,0 ,0},
+    {2000*cos(-6/8.0 * PI), 2000*sin(-6/8.0 * PI), 720, 76000+22000*22/24 ,0 ,0 ,0},
+    {2000*cos(-7/8.0 * PI), 2000*sin(-7/8.0 * PI), 720, 76000+22000*23/24 ,0 ,0 ,0},
+    {2000*cos(-8/8.0 * PI), 2000*sin(8/8.0 * PI), 720, 76000+22000*24/24 ,0 ,0 ,0},
+    {-2000, 2000, 585, 103000 ,0 ,0 ,0},
+    {-2000, 2000, 585, 105000 ,0 ,0 ,0},
+    {-2000, 2000, 585, 110000 ,0 ,1 ,0},//射出
+    {-2000, 2000, 180, 118000 ,0 ,0 ,0},
+    {-2000, 2000, 180, 120000 ,0 ,0 ,0},
+};
+extern const COORDINATE Target[TargetMAX];
+extern const int TargetMAX;
\ No newline at end of file