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:
- 24:91ecfbbf23ee
- Parent:
- 21:4912835805a8
- Child:
- 25:0176b8d3d042
--- a/stateLib.h Wed Oct 10 12:50:54 2018 +0000
+++ b/stateLib.h Fri Oct 12 08:40:01 2018 +0000
@@ -1,4 +1,4 @@
-#define LIBNUM 45
+#define LIBNUM 78
//(X, Y, θ, speed, angle)
#define NEUTRAL 0
#define A1 1
@@ -10,8 +10,16 @@
#define C2 7
#define C3 8
-#define B_x 1.840f
-#define B_y 1.5f
+#define B_x_1 0.840f
+#define B_x_2 1.840f
+#define B_x_3 2.840f
+#define B_x_s1 1.7695f
+#define B_x_s3 1.9105f
+#define B_y_1 1.5f
+#define B_y_2 2.5f
+#define B_y_3 3.5f
+#define B_y_s1 2.4295f
+#define B_y_s3 2.5705f
#define e_posi 2.840f
#define C1_x 3.840f
#define C2_x 4.840f
@@ -21,40 +29,28 @@
#define A2_y 2.5f
#define A3_y 3.5f
#define A_yow 90.0f
-/*
-#define B_x 0.3f
-#define B_y 0.3f
-#define e_posi 0.4f
-#define C1_x 0.5f
-#define C2_x 0.7f
-#define C3_x 0.9f
-#define A_x 0.2f
-#define A1_y 0.2f
-#define A2_y 0.4f
-#define A3_y 0.6f
-#define A_yow 90.0f
-*/
+
float state_lib[LIBNUM][5] ={{0.0, 0.0, 0, NEUTRAL, 0},//スタートゾーン
- {B_x, B_y, 0, B1, 0},
- {B_x, 13.0, 0, B1, 0},
- {B_x, 13.0, 0, B1, 1},
- {B_x, B_y, 0, B1, 0},//4
+ {B_x_2, B_y_1, 0, B1, 0},
+ {B_x_2, 103.0, 0, B1, 0},
+ {B_x_2, 103.0, 0, B1, 1},
+ {B_x_2, B_y_1, 0, B1, 0},//4
- {B_x, B_y, 0, B2, 0},
- {B_x, 13.0, 0, B2, 0},
- {B_x, 13.0, 0, B2, 1},
- {B_x, B_y, 0, B2, 0},//8
+ {B_x_2, B_y_1, 0, B2, 0},
+ {B_x_2, 103.0, 0, B2, 0},
+ {B_x_2, 103.0, 0, B2, 1},
+ {B_x_2, B_y_1, 0, B2, 0},//8
- {B_x, B_y, 0, B2, 0},
- {B_x, 13.0, 0, B2, 0},
- {B_x, 13.0, 0, B2, 1},
- {B_x, 13.0, 0, B2, 0},//12
+ {B_x_2, B_y_1, 0, B2, 0},
+ {B_x_2, 103.0, 0, B2, 0},
+ {B_x_2, 103.0, 0, B2, 1},
+ {B_x_2, 103.0, 0, B2, 0},//12
- {B_x, 13.0, 0, B1, 0},
- {B_x, 13.0, 0, B1, 0},
- {B_x, 13.0, 0, B1, 1},
- {B_x, B_y, 0, B1, 0},//16
+ {B_x_2, 103.0, 0, B1, 0},
+ {B_x_2, 103.0, 0, B1, 0},
+ {B_x_2, 103.0, 0, B1, 1},
+ {B_x_2, B_y_1, 0, B1, 0},//16
{e_posi, 0.0, 0, NEUTRAL, 0},//17
@@ -92,6 +88,43 @@
{A_x, A1_y, 0, NEUTRAL, 0},// 43
+ {B_x_s1, B_y_1, 0, B2, 0},
+ {B_x_s1, 103.0, 0, B2, 0},
+ {B_x_s1, 103.0, 0, B2, 1},//B2手前 手前
+ {B_x_s1, B_y_1, 0, B2, 0},
+ {B_x_2, 103.0, 0, B2, 0},
+ {B_x_2, 103.0, 0, B2, 1},//B2手前 真ん中
+ {B_x_2, B_y_1, 0, B2, 0},
+ {B_x_s3, 103.0, 0, B2, 0},
+ {B_x_s3, 103.0, 0, B2, 1},//B2手前 奥
+ {B_x_s3, B_y_1, 0, B2, 0},//53
+
+ {0, B_x_1, 0, B2, 0},
+ {-B_y_2, B_x_1, 0, B2, 0},
+ {-B_y_2, 103.0, 0, B2, 0},
+ {-B_y_2, 103.0, 0, B2, 1},
+ {-B_y_1, B_x_1, 0, B2, 0},//58
+
+ {-B_x_1, 0, 0, B2, 0},
+ {-B_x_1, -B_y_3, 0, B2, 0},
+ {-B_x_s1,-B_y_3, 0, B2, 0},
+ {-B_x_s1, 103.0, 0, B2, 0},
+ {-B_x_s1, 103.0, 0, B2, 1},//B2奥 手前
+ {-B_x_s1, 103.0, 0, B2, 0},
+ {-B_x_2, 103.0, 0, B2, 0},
+ {-B_x_2, 103.0, 0, B2, 1},//B2奥 真ん中
+ {-B_x_2, 103.0, 0, B2, 0},
+ {-B_x_s3, 103.0, 0, B2, 0},
+ {-B_x_s3, 103.0, 0, B2, 1},//B2奥 奥
+ {-B_x_1, -B_y_3, 0, B2, 0},
+ {-B_x_1, -B_y_1, 0, B2, 0},//71
+
+ {0, -B_x_3, 0, B2, 0},
+ {B_y_2, -B_x_3, 0, B2, 0},
+ {B_y_2, 103.0, 0, B2, 0},
+ {B_y_2, 103.0, 0, B2, 1},
+ {B_y_1, -B_x_3, 0, B2, 0},//76
+
{0.0, 0.0, 0, NEUTRAL, 0}// 補給, スタートゾーン戻る
};
@@ -120,7 +153,7 @@
topsPara para_C1 = {32, 84.0};
topsPara para_C2 = {40, 85.0};
-topsPara para_C3 = {46, 86.3};
+topsPara para_C3 = {46, 86.1};
/*
topsPara para_A1 = {0, 84};
topsPara para_A2 = {0, 84};