YKNCT
Ver Tikutaikai
Dependencies: mbed SBDBT BNO055 YKNCT_MD YKNCT_I2C
Diff: main.cpp
- Revision:
- 1:fa3652aca312
- Parent:
- 0:c527a14d991a
--- a/main.cpp Tue Oct 12 03:57:59 2021 +0000
+++ b/main.cpp Mon Oct 25 10:37:05 2021 +0000
@@ -13,7 +13,6 @@
/* 定数定義
* -------------------------------------------------------------------*/
-
/* 関数プロトタイプ宣言
* --------------------------------------------------------*/
void uartRX(void);
@@ -92,7 +91,6 @@
/* ジャイロのピン設定 */
BNO055 bno(PB_9, PB_8);
-
DigitalOut led(LED2);
/* リボン射出ピン */
DigitalOut Rod(PC_2);
@@ -100,15 +98,13 @@
/* エンコーダーピン設定 */
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};
+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
+INJECTION injection[2] = {INJECTION(PB_10, PA_8, PA_9),
+ INJECTION(PA_4, PB_0, PC_1)};
DigitalIn limitRibbonTop(PC_0);
DigitalIn limitRibbonBottom(PC_3);
@@ -116,8 +112,7 @@
Timer WaitNextShot;
/*----------------------------------- main ----------------------------------*/
-int 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);
@@ -143,18 +138,17 @@
WaitNextShot.start();
/* 小さくすると深くなる */
- injection[0].initalDeg = 29;
+ injection[0].initalDeg = 5;
injection[1].initalDeg = 52;
- injection[0].targetRps[0] = -28;
- injection[0].targetRps[1] = 28;
- injection[1].targetRps[0] = -25;
- injection[1].targetRps[1] = 25;
-
+ injection[0].targetRps[0] = -27;
+ injection[0].targetRps[1] = 27
+ ;
+ injection[1].targetRps[0] = -27;
+ injection[1].targetRps[1] = 27;
/* メインループ ----------------------------------------------------------*/
while (1) {
-
led = !led;
for (int i = 0; i < MotorMAX; i++) {
@@ -178,27 +172,26 @@
/* STARTで座標系リセット */
if (DS3.START) {
NowLoc.reset();
- TarTheta=0.0;
+ TarTheta = 0.0;
}
/* CS送信ステータス更新 */
- statious=(operate==1)+1;
+ 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++;
+ if (isMoveInjection && WaitNextShot.read_ms() > STEP(targetNum) &&
+ !injection[!nextCnt].isMove) {
+ injection[nextCnt].isMove = 1;
+ nextCnt = !nextCnt;
}
-
+ // if(isMoveInjection) {
+ // injection[0].isMove = injection[1].isMove = 1;
+ // }
for (int i = 0; i < 2; i++) {
/* 動作状態を決める */
-// injection[i].isMove = isMoveInjection && injection[!i].WaitNextShot.read_ms() > step;
injection[i].MainFunction();
}
@@ -208,18 +201,13 @@
}
/* リボン */
- 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 (isMoveRibbon) {
+ if (!limitRibbonTop)
+ MovMotor[9] = 73;
+ else if (isMoveRibbon == 2)
+ MovMotor[10] = 50;
+ } else if (!limitRibbonBottom)
+ MovMotor[9] = -60;
}
/* 停止処理 */
@@ -230,7 +218,7 @@
for (int i = 0; i < 2; i++)
injection[0].updateVal[i][1] = injection[1].updateVal[i][1] = 0;
Rod = 0;
- TarTheta=NowLoc.Theta;
+ TarTheta = NowLoc.Theta;
}
/* 足回り自動 */
@@ -243,43 +231,54 @@
operate = 0;
else {
double divTim = 0;
- divTim = (double)(msTimer - Target[targetNum - 1].Time) / (double)(Target[targetNum].Time - Target[targetNum - 1].Time);
+ 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;
+ 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;
+ 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);
+ 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);
+ // 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;
+ 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();
@@ -290,27 +289,25 @@
InverseTriOmni(MovMotor, x_val, y_val, r_val);
isMoveInjection = DS3.R1;
- isMoveRibbon = DS3.R2+DS3.L2;
+ isMoveRibbon = DS3.R2 + DS3.L2;
Rod = DS3.L1;
}
-
- else if(operate == 3) {
+ else if (operate == 3) {
isMoveInjection = DS3.R1;
- isMoveRibbon = DS3.R2+DS3.L2;
+ 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("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("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,
@@ -319,81 +316,83 @@
// 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 %d ", MovMotor[6], MovMotor[7],
+ // MovMotor[8]);
-// pc.printf("Motor %d %d ", MovMotor[9], MovMotor[10]);
+ // pc.printf("Motor %d %d ", MovMotor[9], MovMotor[10]);
-// pc.printf("Enc %3.0f %3.0f ",EncoderDeg[0],EncoderDeg[1]);
+ // 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(),
+ // pc.printf("inj[%d]is:%d ", i, injection[i].isMove);
+ // 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);
+ pc.printf("Enc %3.0f %3.0f %3.0f ",
+ injection[i].encDeg[0],
+ injection[i].encDeg[1],
+ injection[i].encDeg[2]);
+ // pc.printf("Enc %5.0f %5.0f %5.0f ", injection[i].encDeg[0],
+ // injection[i].encDeg[1], injection[i].encDeg[2]);
+ // pc.printf("dif %5.0f ",
+ // injection[i].encDeg[0]+injection[i].encDeg[1]);
+ // 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);
+ if (timerMode) msTimer += (double)whileTimer.read() * 1000.0;
+ if (whileTime_us > 50000) I2C hoge(PB_9, PB_8);
/* 周期リセット */
whileTimer.reset();
} // while
} // main
-
-
-
/*******************************************************************************
* @概要 射出機構のメイン関数
* @引数 なし
* @返り値 なし
*******************************************************************************/
-void INJECTION::MainFunction(void)
-{
+void INJECTION::MainFunction(void) {
/* 上のリミットと接触したらEncリセット */
if (limitTop) encDeg[0] = encDeg[1] = 0;
+ if (limitBottom) encDeg[0] = -1140, encDeg[1] = 1140;
/* 下のリミットと接触したらエンコーダー角度を合わせる */
- 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(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();
+ 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;
+ }
+
/* 装填機構動作 */
- moveMotor[2] = LoadingPID((double)initalDeg,encDeg[2]);
+ 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;
- }
+ } else if (BallShotTim.read_ms() > 180) {
/* 操作量を更新 */
for (int i = 0; i < 2; i++) {
@@ -402,19 +401,17 @@
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
+ if (BallLoadTim.read_ms() > 200 && isMove) {
isBallLoading = 1;
initalDeg -= 120;
isMove = 0;
WaitNextShot.reset();
- WaitNextShot.start();
}
/* 装填機構を動作させる */
@@ -434,21 +431,52 @@
/* 射出機構戻ってなかったら途中までで上書き */
else {
/* 下降中に途中まで回す */
- moveMotor[2] = LoadingPID(initalDeg+40, encDeg[2]);
+ moveMotor[2] = LoadingPID(initalDeg + 40, encDeg[2]);
}
}
/* リミットに触れるまで下降 */
+ // if (limitTop) {
+ // 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;
+ // }
+ // }
+ // else if (encDeg[1] < 0) {
+ // /* 固定値で一番下まで下降 */
+ // for (int i = 0; i < 2; i++) moveMotor[i] = -15 * (i * 2 -
+ // 1);
+
+ // }
+ // else{
+ // double tarRps[2]={0};
+ // tarRps[1] = -(encDeg[1] - 200)/100.0;
+ // if(tarRps[1] < 0 && tarRps[1] > -1.5) tarRps[1] = -1.5;
+ // if(tarRps[1] > 0 && tarRps[1] < 1.5) tarRps[1] = 1.5;
+ // tarRps[0] = -tarRps[1];
+ // pc.printf("%f,%f,",tarRps[0],tarRps[1]);
+
+ // /* 操作量を更新 */
+ // for (int i = 0; i < 2; i++) {
+ // updateVal[i][0] = updateVal[i][1];
+ // updateVal[i][1] = updateVal[i][0] +
+ // VelocityPID(tarRps[i], rps[i], i);
+
+ // /* 値を制限する */
+ // moveMotor[i] = Range(updateVal[i][1], 100);
+ // }
+ // }
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);
+ moveMotor[i] = Range(moveMotor[i], 50);
}
} else {
/* 固定値で一番下まで下降 */
- for (int i = 0; i < 2; i++) moveMotor[i] = -10 * (i * 2 - 1);
+ for (int i = 0; i < 2; i++) moveMotor[i] = -15 * (i * 2 - 1);
}
break;
@@ -463,8 +491,8 @@
* @引数 目標値, 現在値, モーター番号
* @返り値 操作量
*******************************************************************************/
-double INJECTION::VelocityPID(double Target_value, double Current_value, int motorNum)
-{
+double INJECTION::VelocityPID(double Target_value, double Current_value,
+ int motorNum) {
/* 比例定数,積分定数,微分定数 */
const double kp = 1.0;
const double ki = 0.3;
@@ -492,8 +520,7 @@
* @引数 目標値, 現在値
* @返り値 操作量
*******************************************************************************/
-double LocationPID(double Target_value, double Current_value)
-{
+double LocationPID(double Target_value, double Current_value) {
/* 比例定数,積分定数,微分定数 */
const double kp = 0.5;
const double ki = 0;
@@ -526,12 +553,11 @@
* @引数 目標値, 現在値
* @返り値 操作量
*******************************************************************************/
-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;
+double INJECTION::LoadingPID(double Target_value, double Current_value) {
+ /* 比例定数,積分定数,微分定数 */
+ const double kp = 3.5;
+ const double ki = 0.08;
+ const double kd = 0.5;
/* 比例項,積分項,微分項の計算結果を保存する変数*/
static double P = 0;
static double I = 0;
@@ -546,12 +572,13 @@
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;
+ if ((loadDev[0] < 0 && loadDev[1] > 0) ||
+ (loadDev[0] > 0 && loadDev[1] < 0))
+ I = 0;
/* 合計値を操作量として返す */
- return Range(P + Range(I,6) + D, 80);
+ return Range(P + Range(I, 6) + D, 70);
}
/*******************************************************************************
@@ -559,8 +586,7 @@
* @引数 対象,限界値
* @返り値 制限後の値
*******************************************************************************/
-double Range(double value, double limit)
-{
+double Range(double value, double limit) {
if (value > limit)
return limit;
else if (value < -limit)
@@ -574,8 +600,7 @@
* @引数 角度, エンコーダー番号, 機体番号
* @返り値 周期あたりの角速度
*******************************************************************************/
-double INJECTION::RotationPerWhile(double degree, int num)
-{
+double INJECTION::RotationPerWhile(double degree, int num) {
/* 前回の値更新 今回の値更新 */
EncDegDiff[num][1] = EncDegDiff[num][0];
EncDegDiff[num][0] = degree;
@@ -589,8 +614,7 @@
* @引数 なし
* @返り値 なし
*******************************************************************************/
-void LocEstimate(void)
-{
+void LocEstimate(void) {
static double GyroDeg[2] = {0};
static double EncDeg[2][2] = {0};
static double disp[3] = {0};
@@ -632,8 +656,7 @@
* @引数 角度, エンコーダー番号, 機体番号
* @返り値 周期あたりの角速度
*******************************************************************************/
-int Rotation(int degree, int num, int robotNum)
-{
+int Rotation(int degree, int num, int robotNum) {
static int EncDeg[2][2][2] = {0};
static int NowRotate[2][2] = {0};
@@ -652,8 +675,7 @@
* @引数 モータの配列・x,y,r ベクトル
* @返り値 なし
******************************************************************************/
-void InverseTriOmni(int *duty, double x, double y, double r)
-{
+void InverseTriOmni(int *duty, double x, double y, double r) {
int duty_max = 0;
double rad = DEG_TO_RAD(NowLoc.Theta);
@@ -670,22 +692,20 @@
if (duty_max > 99) duty[i] = duty[i] * 99 / duty_max;
}
-
-/* UART受信割り込み *************************************************************/
-void uartRX()
-{
+/* 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;
+ 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);
+ for (int i = 0; i < 3; i++) tmp |= buf[i] << (14 - i * 7);
//チェックサム計算
uint8_t checkSum = 0;
@@ -693,8 +713,7 @@
checkSum += tmp / (int)pow(10.0, (double)i) % 10;
//誤り検知陰性
- if(checkSum == (data&0x3F))
- msTimer = tmp;
+ if (checkSum == (data & 0x3F) && (DS3.SELECT == 0)) msTimer = tmp;
num = 0;
} else {
@@ -702,16 +721,15 @@
num++;
}
-
/* 送信データ */
- char sendData = 2<<2;
- sendData|=(statious&0b11);
+ char sendData = 2 << 2;
+ sendData |= (statious & 0b11);
UART.putc(sendData);
}
-/* 割り込み(100us) *************************************************************/
-void IT_CallBack(void)
-{
+/* 割り込み(100us)
+ * *************************************************************/
+void IT_CallBack(void) {
static int cnt = 0;
static int data[EncoderMAX] = {0};
static double EncDeg[EncoderMAX][2] = {0};