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: DriveConroller IMU MDD Mycan Odometer PID RotaryEncoder UART USS mbed
Fork of
AR_MastarNode
by 
MEC-B
Diff: stateLib.h
- Revision:
- 15:ae2043fde152
- Parent:
- 14:8334c241bb0a
- Child:
- 16:621f04b15f86
--- a/stateLib.h Mon Sep 17 08:52:05 2018 +0000
+++ b/stateLib.h Sat Sep 22 13:41:19 2018 +0000
@@ -1,6 +1,6 @@
-#define LIBNUM 18
+#define LIBNUM 45
//(X, Y, θ, speed, angle)
-float state_lib[LIBNUM][10] ={{0.0, 0.0, 0, 0, 90, 0},//スタートゾーン
+float state_lib[LIBNUM][10] ={{0.0, 0.0, 0, 0, 0},//スタートゾーン
/*
//2段テーブル1周
{-1.740, 1.5, 0, 0, 90, 0},//
@@ -24,47 +24,50 @@
{-0.740, 2.5, -270, 0, 90, 0},//
{-0.740, 1.5, -270, 0, 90, 0},//
*/
- /*
+
//2段テーブル下段のみ
- {-1.740, 1.5, 0, 0, 82, 0},// x移動, 角度変化
- {-1.740, 25.0, 0, 0, 82, 0},// y移動
- {-1.740, 20.0, 0, 0, 82, 0},// 下段に発射
- */
- /*
+ {-1.740, 1.5, 0, 4, 0},// x移動, 角度変化
+ {-1.740, 20.0, 0, 4, 0},// y移動
+ {-1.740, 20.0, 0, 4, 1},// 下段に発射
+ {-1.740, 1.5, 0, 4, 0},// 4
+
+
//2段テーブル上段のみ
- {-1.740, 1.5, 0, 0, 85, 0},// x移動, 角度変化
- {-1.740, 13.0, 0, 0, 85, 0},// USS
- {-1.740, 13.0, 0, 0, 85, 0},// 上段に発射
- */
+ {-1.740, 1.5, 0, 5, 0},// x移動, 角度変化
+ {-1.740, 13.0, 0, 5, 0},// USS
+ {-1.740, 13.0, 0, 5, 1},// 上段に発射
+ {-1.740, 1.5, 0, 5, 0},// 8
+
//2段テーブル上・下
- {-1.740, 1.5, 0, 0, 82, 0},// x移動, 角度変化
- {-1.740, 25.0, 0, 0, 82, 0},// y移動
- {-1.740, 20.0, 0, 0, 82, 0},// 下段に発射
- {-1.740, 20.0, 0, 0, 90, 0},// y戻る, 角度戻る
+ {-1.740, 1.5, 0, 4, 0},// x移動, 角度変化
+ {-1.740, 25.0, 0, 4, 0},// y移動
+ {-1.740, 20.0, 0, 4, 1},// 下段に発射
+ {-1.740, 20.0, 0, 4, 0},// 12 角度戻る
- {-1.740, 25.0, 0, 0, 90, 1},// 補給
- {-1.740, 13.0, 0, 0, 85, 0},// 角度変化
- {-1.740, 13.0, 0, 0, 85, 0},// 上段に発射
+ {-1.740, 20.0, 0, 5, 0},// 補給
+ {-1.740, 13.0, 0, 5, 0},// 角度変化
+ {-1.740, 13.0, 0, 5, 1},// 上段に発射
+ {-1.740, 1.5, 0, 5, 0},// 16 角度変化
- {-2.740, 0.0, 0, 0, 90, 0},
+ {-2.740, 0.0, 0, 0, 0},// 17
//移動テーブル ノーマル
- {-3.740, 0.0, 0, 0, 90, 1},// x移動, 補給
- {-3.740, 25.0, 0, 0, 82, 0},// y移動, 角度変化
- {-3.740, 25.0, 0, 31, 82, 0},// 発射
- {-3.740, 0.0, 0, 0, 90, 0},// y戻る, 角度戻る
+ {-3.740, 0.0, 0, 6, 0},// x移動, 補給
+ {-3.740, 20.0, 0, 6, 0},// y移動, 角度変化
+ {-3.740, 20.0, 0, 6, 1},// 発射
+ {-3.740, 0.0, 0, 6, 0},// 21 y戻る, 角度戻る
- {-4.740, 2.0, 0, 0, 90, 1},// x移動, 補給
- {-4.740, 25.0, 0, 0, 83, 0},// y移動, 角度変化
- {-4.740, 25.0, 0, 38, 83, 0},// 発射
- {-4.740, 0.0, 0, 0, 90, 0},// y戻る, 角度戻る
+ {-4.740, 0.0, 0, 7, 0},// x移動, 補給
+ {-4.740, 20.0, 0, 7, 0},// y移動, 角度変化
+ {-4.740, 20.0, 0, 7, 1},// 発射
+ {-4.740, 0.0, 0, 7, 0},// 25 y戻る, 角度戻る
- {-5.740, 2.0, 0, 0, 90, 1},// x移動, 補給
- {-5.740, 25.0, 0, 0, 85, 0},// y移動, 角度変化
- {-5.740, 25.0, 0, 47, 85, 0},// 発射
- {-5.740, 0.0, 0, 0, 90, 0},// y戻る, 角度戻る
+ {-5.740, 0.0, 0, 8, 0},// x移動, 補給
+ {-5.740, 20.0, 0, 8, 0},// y移動, 角度変化
+ {-5.740, 20.0, 0, 8, 1},// 発射
+ {-5.740, 0.0, 0, 8, 0},// 29 y戻る, 角度戻る
/*
//移動テーブル アクティブムーブ
@@ -81,28 +84,79 @@
{-5.740, 25.0, 0, 0, 85, 0},// 発射
{-5.740, 0.0, 0, 0, 90, 0},// y戻る, 角度戻る
*/
- /*
+
//固定テーブル
- {-0.3, 1.5, -90, 0, 90, 0},
- {25, 1.5, -90, 0, 90, 0},
- {25, 1.5, -90, 0, 90, 0},// 発射
+ {-0.3, 1.5, -90, 1, 0},
+ {25, 1.5, -90, 1, 0},
+ {25, 1.5, -90, 1, 1},// 発射
+ {-0.8, 1.5, -90, 1, 0},// 33
- {-0.8, 1.5, -90, 0, 90, 0},
+ {-0.8, 2.5, -90, 2, 0},
+ {25, 2.5, -90, 2, 0},
+ {25, 2.5, -90, 2, 1},// 発射
+ {-0.8, 2.5, -90, 2, 0},// 37
+
+ {-0.8, 3.5, -90, 3, 0},
+ {25, 3.5, -90, 3, 0},
+ {25, 3.5, -90, 3, 1},// 発射
+ {-0.8, 3.5, -90, 3, 0},// 41
+
+ {-0.8, 1.5, 0, 0, 0},// 42
- {-0.8, 2.5, -90, 0, 90, 0},
- {25, 2.5, -90, 0, 90, 0},
- {25, 2.5, -90, 0, 90, 0},// 発射
-
- {-0.8, 3.5, -90, 0, 90, 0},
- {25, 3.5, -90, 0, 90, 0},
- {25, 3.5, -90, 0, 90, 0},// 発射
- {-0.8, 3.5, -90, 0, 90, 0},
-
- {-0.8, 1.5, 0, 0, 90, 0},
- */
- {0.0, 0.0, 0, 0, 90, 0}// 補給, スタートゾーン戻る
+ {0.0, 0.0, 0, 9, 0}// 補給, スタートゾーン戻る
};
+
+float A_1[4][3] = {{0, 90, 1},
+ {0, 84, 0},
+ {28, 84, 0},
+ {0, 90, 0},
+ };
+
+float A_2[4][3] = {{0, 90, 1},
+ {0, 84, 0},
+ {28, 84, 0},
+ {0, 90, 0},
+ };
+
+float A_3[4][3] = {{0, 90, 1},
+ {0, 84, 0},
+ {28, 84, 0},
+ {0, 90, 0},
+ };
+
+float B_1[4][3] = {{0, 90, 1},
+ {0, 82.5, 0},
+ {31, 82.5, 0},
+ {0, 90, 0},
+ };
+
+float B_2[4][3] = {{0, 90, 1},
+ {0, 84.8, 0},
+ {58, 84.8, 0},
+ {0, 90, 0},
+ };
+
+float C_1[4][3] = {{0, 90, 1},
+ {0, 82.5, 0},
+ {33, 82.5, 0},
+ {0, 90, 0},
+ };
+
+float C_2[4][3] = {{0, 90, 1},
+ {0, 83.5, 0},
+ {40, 83.5, 0},
+ {0, 90, 0},
+ };
+
+float C_3[4][3] = {{0, 90, 1},
+ {0, 85.5, 0},
+ {44, 85.5, 0},
+ {0, 90, 0},
+ };
+
+float state_tops[4][3];
+
typedef struct state
{
float x, y , theta, shoot, angle, supply;