Takushima Yukimasa
0913 hikitugi
Dependencies: mbed SBDBT BNO055 YKNCT_MD YKNCT_I2C
Diff: main.cpp
- Revision:
- 0:ad3dd7fb650a
--- /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