YKNCT
Ver Tikutaikai
Dependencies: mbed SBDBT BNO055 YKNCT_MD YKNCT_I2C
Diff: main.cpp
- Revision:
- 0:c527a14d991a
- Child:
- 1:fa3652aca312
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Oct 12 03:57:59 2021 +0000
@@ -0,0 +1,763 @@
+/* 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;
+}
\ No newline at end of file