2020_TeamA
/
Yoshida_MiniRobo
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed YKNCT_Movement SBDBT BNO055 YKNCT_MD YKNCT_I2C
Diff: main.cpp
- Revision:
- 22:9d77148a3f09
- Parent:
- 21:22b863d32705
- Child:
- 23:98b634305bab
--- a/main.cpp Fri Mar 20 04:12:22 2020 +0000
+++ b/main.cpp Mon Mar 23 05:34:40 2020 +0000
@@ -37,7 +37,7 @@
/* ライン補正 */
int LineCor(int Line_Dire);
-/* */
+/* オムニ加算式 */
void SubOmuni(int X,int Y,int R);
/* 変数定義 ------------------------------------------------------------------*/
@@ -57,9 +57,12 @@
/* 自動yaw補整目標角度 */
double TarTheta=0;
-/* ライン補正移動方向 */
+/* ライン補正移動方向 0…左 1…右 2…直進*/
int Line_Dire=0;
+/* ライン番号 */
+int Line_Num=0;
+
/* クラス定義 ----------------------------------------------------------------*/
/* 割り込み用クラス */
@@ -85,6 +88,9 @@
/* ジャイロ用タイマー */
Timer yawCnt;
+/* 前進用タイマー */
+Timer Forward;
+
/* 足回り動作クラス定義 */
Move omuni(MovMotor,NowLoc.theta);
@@ -124,6 +130,9 @@
In.Init(0,2);
In.Init(0,3);
+ /* タイマーリセット */
+ Forward.reset();
+
telemetry.printf("\n\rMainLoopStart");
/* メインループ --------------------------------------------------------------*/
while(1) {
@@ -197,22 +206,112 @@
if(DS3.START) auto_mode++;
break;
- /* X座標移動処理 */
+ /* X座標移動 */
case 1:
+ /* 壁あて実行 */
+ ToWall();
/* 動作終了時次のステップに */
- if(XCooMove(1000,50)==0)
+ if(Line_Num==0&&XCooMove(-1000,30)==0) {
+ Line_Num++;
+ auto_mode++;
+ }
+ if(Line_Num==1&&XCooMove(-1500,30)==0) {
+ Line_Num++;
+ auto_mode++;
+ }
+ if(Line_Num==2&&XCooMove(-2200,30)==0) {
+ Line_Num=0;
+ auto_mode++;
+ }
+
+ break;
+
+ /* ラインまで移動 */
+ case 2:
+ /* ライン到達後次のステップに */
+ Line_Dire=0;
+ if(LineCor(Line_Dire)==1)
+ auto_mode++;
+
+ break;
+ /* */
+ case 3:
+ /* ライン補正しながら直進 */
+ Line_Dire=2;
+ LineCor(Line_Dire);
+ Forward.start();
+ SubOmuni(0,20,0);
+ /* 動作終了時次のステップに */
+ if(Forward.read_ms()==2500) {
+ auto_mode++;
+ Forward.reset();
+ }
+
+ break;
+
+ case 4:
+ /* ライン補正しながら後ろへ移動 */
+ LineCor(Line_Dire);
+ ToWall();
+ if(In.Get(0)==1&&In.Get(1)==1)
auto_mode++;
break;
- /* ライン補正処理 */
- case 2:
- /* ライン補正完了後次のステップに */
- if(LineCor(Line_Dire)==1)
+ case 5:
+ /* 壁あて実行 */
+ ToWall();
+ /* 右へ移動 */
+ if(Line_Num==1&&XCooMove(1200,10)==0)
+ auto_mode++;
+ if(Line_Num==2&&XCooMove(1700,10)==0)
+ auto_mode++;
+ if(Line_Num==3&&XCooMove(2400,10)==0)
auto_mode++;
break;
+ case 6:
+ /* 宝物置き場付近まで直進 */
+ Forward.start();
+ SubOmuni(0,20,0);
+ /* 動作終了時次のステップに */
+ if(Forward.read_ms()==4000) {
+ auto_mode++;
+ Forward.reset();
+ }
+
+ break;
+
+ case 7:
+ /* 宝物置き場のラインまで移動 */
+ Line_Dire=1;
+ if(LineCor(Line_Dire)==1)
+ auto_mode++;
+
+ break;
+
+ case 8:
+ /* 宝物置き場前まで移動 */
+ Forward.start();
+ SubOmuni(0,20,0);
+ /* 動作終了時次のステップに */
+ if(Forward.read_ms()==2000) {
+ auto_mode++;
+ Forward.reset();
+ }
+
+ break;
+
+ case 9:
+ /* スタートゾーンに移動 */
+ ToWall();
+ if(In.Get(0)==1&&In.Get(1)==1)
+ auto_mode++;
+
+ break;
+
+
/* 終了処理 */
default:
auto_mode=0;
@@ -232,11 +331,11 @@
*******************************************************************************/
void LocEstimate(void)
{
- static double GyroDeg[2]= {0};
+ static double GyroDeg[2]= {0};
static double EncDeg[2][2]= {0};
static double disp[3]= {0};
-
+
/* ジャイロの値取得 */
bno.get_angles();
@@ -330,6 +429,7 @@
/* 右側リミットに応じて左2輪の操作量を決める */
if(In.Get(1)==0)
for(int i=2; i<4; i++) MovMotor[i]+=10;
+
}
@@ -405,18 +505,18 @@
R=Rest(R,100);
-
+
/* オムニ計算結果をtmpに */
int tmp[4]= {0};
-
+
/* 一度データをtmpに保存 */
- for(int i=0;i<4;i++)
+ for(int i=0; i<4; i++)
tmp[i]=MovMotor[i];
-
+
/* オムニ計算 */
omuni.XmarkOmni_Move(X,Y,R);
-
+
/* 計算結果を加算する */
for(int i=0; i<4; i++)
MovMotor[i]+=tmp[i];
@@ -427,7 +527,7 @@
/*******************************************************************************
* @概要 ライン補正関数
-* @引数 なし
+* @引数 Line_Dire:初期移動方向
* @返り値 なし
*******************************************************************************/
int LineCor(int Line_Dire)
@@ -440,7 +540,7 @@
static bool LineFlag=0;
if(LineFlag==0) {
- /* 初期値設定 0…左 1…右 */
+ /* 初期値設定 0…左 1…右 2…直進*/
Last_val=Line_Dire;
/* 初期化フラグ立てる */
LineFlag=1;
@@ -450,6 +550,7 @@
if(In.Get(2)==0&&In.Get(3)==0) {
if(Last_val==0) val_x-=10;
if(Last_val==1) val_x+=10;
+ else val_x+=0;
}
/* 左側のみ */