ichinoseki_Bteam_2019
/
2019_AR_Itsuki
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: IMU mbed Odometer PID MDD RotaryEncoder UART Mycan DriveConroller
Diff: stateLib.h
- Revision:
- 0:56a2c0ed99c5
- Child:
- 1:0f33a68d1390
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stateLib.h Mon Oct 21 14:25:38 2019 +0000
@@ -0,0 +1,267 @@
+#define TQ_NUM 19
+#define TF_NUM 24
+#define BQ_NUM 19
+#define BF_NUM 24
+#define SQ_NUM 9
+#define SF_NUM 21
+#define TEST_NUM 6
+#define TOPSNUM 5
+
+#define NEUTRAL 0
+#define T_shirt 1
+#define BathTowel 2
+#define Sheets 3
+#define Clothespin 4
+#define Catch 5
+#define Slide 6
+
+#define TQ_x_1 1.800f //予選１つ目
+#define TQ_x_2 2.550f //予選２つ目
+#define TQ_x_3 3.300f //予選３つ目
+#define TF_x_1 1.425f //決勝１つ目
+#define TF_x_2 1.675f //決勝２つ目
+#define TF_x_3 2.025f //決勝３つ目
+#define TF_x_4 2.275f //決勝４つ目
+
+#define T_y_1 1.700f //Tシャツラインの位置
+#define T_y_2 2.200f //Tシャツかける直前
+#define T_y_3 2.480f //Tシャツかける時
+
+#define BQ_x_1 1.750f //予選１つ目
+#define BQ_x_2 2.550f //予選２つ目
+#define BQ_x_3 3.350f //予選３つ目
+#define BF_x_1 1.555f //決勝１つ目
+#define BF_x_2 2.165f //決勝２つ目
+#define BF_x_3 2.835f //決勝３つ目
+#define BF_x_4 3.445f //決勝４つ目
+
+#define B_y_1 3.700f //バスタオルラインの位置
+#define B_y_2 4.350f //バスタオルかける直前
+#define B_y_3 4.800f //バスタオルかける時
+
+#define S_x_1 1.830f //シーツをかける時
+#define S_x_2 1.600f //洗濯バサミをつける時
+#define S_x_3 2.560f //横から掴む前
+#define S_x_4 2.350f //横から掴む時
+#define S_x_5 3.450f //横に引っ張った後
+
+#define S_y_1 5.675f //シーツラインの位置
+#define S_y_2 6.750f //シーツかける時
+#define S_y_3 6.470f //洗濯バサミをつける時
+#define S_y_4 6.475f //横から掴む時
+#define S_y_5 6.300f //横に引っ張る時
+
+//{X, Y, θ, Spline, Laundry, Tops}
+float state_lib[6] = {0.00, 0.00, 0.0, -1, 0, 2};
+
+float TQ_lib[TQ_NUM][6] =
+{
+ {0.00, 0.00, 0.0, -1, 0, 2},
+
+ {0.00, T_y_1, 0.0, -1, T_shirt, 0},
+ {TQ_x_1, T_y_1, 0.0, -1, T_shirt, 0},
+ {TQ_x_1, T_y_3, 0.0, -1, T_shirt, 0},
+ {TQ_x_1, T_y_3, 0.0, -1, T_shirt, 1},
+ {TQ_x_1, T_y_1, 0.0, -1, T_shirt, 0},
+ {TQ_x_2, T_y_1, 0.0, -1, T_shirt, 0},
+ {TQ_x_2, T_y_2, 0.0, -1, T_shirt, 0},
+ {TQ_x_2, T_y_3, 0.0, -1, T_shirt, 0},
+ {TQ_x_2, T_y_3, 0.0, -1, T_shirt, 1},
+ {TQ_x_2, T_y_1, 0.0, -1, T_shirt, 0},
+ {TQ_x_3, T_y_1, 0.0, -1, T_shirt, 0},
+ {TQ_x_3, T_y_2, 0.0, -1, T_shirt, 0},
+ {TQ_x_3, T_y_3, 0.0, -1, T_shirt, 0},
+ {TQ_x_3, T_y_3, 0.0, -1, T_shirt, 1},
+ {TQ_x_3, T_y_1, 0.0, -1, T_shirt, 0},
+ {0.00, T_y_1, 0.0, -1, 0, 2},
+
+ {0.00, 0.35, 0.0, -1, 0, 2},
+ {0.00, 0.00, 0.0, -1, 0, 2},
+};
+
+float TF_lib[TF_NUM][6] =
+{
+ {0.00, 0.00, 0.0, -1, 0, 2},
+
+ {0.00, T_y_1, 0.0, -1, T_shirt, 0},
+ {TF_x_1, T_y_1, 0.0, -1, T_shirt, 0},
+ {TF_x_1, T_y_3, 0.0, -1, T_shirt, 0},
+ {TF_x_1, T_y_3, 0.0, -1, T_shirt, 1},
+ {TF_x_1, T_y_1, 0.0, -1, T_shirt, 0},
+ {TF_x_2, T_y_1, 0.0, -1, T_shirt, 0},
+ {TF_x_2, T_y_2, 0.0, -1, T_shirt, 0},
+ {TF_x_2, T_y_3, 0.0, -1, T_shirt, 0},
+ {TF_x_2, T_y_3, 0.0, -1, T_shirt, 1},
+ {TF_x_2, T_y_1, 0.0, -1, T_shirt, 0},
+ {TF_x_3, T_y_1, 0.0, -1, T_shirt, 0},
+ {TF_x_3, T_y_2, 0.0, -1, T_shirt, 0},
+ {TF_x_3, T_y_3, 0.0, -1, T_shirt, 0},
+ {TF_x_3, T_y_3, 0.0, -1, T_shirt, 1},
+ {TF_x_3, T_y_1, 0.0, -1, T_shirt, 0},
+ {TF_x_4, T_y_1, 0.0, -1, T_shirt, 0},
+ {TF_x_4, T_y_2, 0.0, -1, T_shirt, 0},
+ {TF_x_4, T_y_3, 0.0, -1, T_shirt, 0},
+ {TF_x_4, T_y_3, 0.0, -1, T_shirt, 1},
+ {TF_x_4, T_y_1, 0.0, -1, T_shirt, 0},
+ {0.00, T_y_1, 0.0, -1, 0, 2},
+
+ {0.00, 0.35, 0.0, -1, 0, 2},
+ {0.00, 0.00, 0.0, -1, 0, 2},
+};
+
+float BQ_lib[BQ_NUM][6] =
+{
+ {0.00, 0.00, 0.0, -1, 0, 2},
+
+ {0.00, B_y_1, 0.0, -1, BathTowel, 0},
+ {BQ_x_1, B_y_1, 0.0, -1, BathTowel, 0},
+ {BQ_x_1, B_y_3, 0.0, -1, BathTowel, 0},
+ {BQ_x_1, B_y_3, 0.0, -1, BathTowel, 1},
+ {BQ_x_1, B_y_1, 0.0, -1, BathTowel, 0},
+ {BQ_x_2, B_y_1, 0.0, -1, BathTowel, 0},
+ {BQ_x_2, B_y_2, 0.0, -1, BathTowel, 0},
+ {BQ_x_2, B_y_3, 0.0, -1, BathTowel, 0},
+ {BQ_x_2, B_y_3, 0.0, -1, BathTowel, 1},
+ {BQ_x_2, B_y_1, 0.0, -1, BathTowel, 0},
+ {BQ_x_3, B_y_1, 0.0, -1, BathTowel, 0},
+ {BQ_x_3, B_y_2, 0.0, -1, BathTowel, 0},
+ {BQ_x_3, B_y_3, 0.0, -1, BathTowel, 0},
+ {BQ_x_3, B_y_3, 0.0, -1, BathTowel, 1},
+ {BQ_x_3, B_y_1, 0.0, -1, BathTowel, 0},
+ {0.00, B_y_1, 0.0, -1, 0, 2},
+
+ {0.00, 0.35, 0.0, -1, 0, 2},
+ {0.00, 0.00, 0.0, -1, 0, 2},
+};
+
+float BF_lib[BF_NUM][6] =
+{
+ {0.00, 0.00, 0.0, -1, 0, 2},
+
+ {0.00, B_y_1, 0.0, -1, BathTowel, 0},
+ {BF_x_1, B_y_1, 0.0, -1, BathTowel, 0},
+ {BF_x_1, B_y_3, 0.0, -1, BathTowel, 0},
+ {BF_x_1, B_y_3, 0.0, -1, BathTowel, 1},
+ {BF_x_1, B_y_1, 0.0, -1, BathTowel, 0},
+ {BF_x_2, B_y_1, 0.0, -1, BathTowel, 0},
+ {BF_x_2, B_y_2, 0.0, -1, BathTowel, 0},
+ {BF_x_2, B_y_3, 0.0, -1, BathTowel, 0},
+ {BF_x_2, B_y_3, 0.0, -1, BathTowel, 1},
+ {BF_x_2, B_y_1, 0.0, -1, BathTowel, 0},
+ {BF_x_3, B_y_1, 0.0, -1, BathTowel, 0},
+ {BF_x_3, B_y_2, 0.0, -1, BathTowel, 0},
+ {BF_x_3, B_y_3, 0.0, -1, BathTowel, 0},
+ {BF_x_3, B_y_3, 0.0, -1, BathTowel, 1},
+ {BF_x_3, B_y_1, 0.0, -1, BathTowel, 0},
+ {BF_x_4, B_y_1, 0.0, -1, BathTowel, 0},
+ {BF_x_4, B_y_2, 0.0, -1, BathTowel, 0},
+ {BF_x_4, B_y_3, 0.0, -1, BathTowel, 0},
+ {BF_x_4, B_y_3, 0.0, -1, BathTowel, 1},
+ {BF_x_4, B_y_1, 0.0, -1, BathTowel, 0},
+ {0.00, B_y_1, 0.0, -1, 0, 2},
+
+ {0.00, 0.35, 0.0, -1, 0, 2},
+ {0.00, 0.00, 0.0, -1, 0, 2},
+};
+
+float SQ_lib[SQ_NUM][6] =
+{
+ {0.00, 0.00, 0.0, -1, 0, 2},
+
+ {0.00, S_y_1, 0.0, -1, Sheets, 0},
+ {BQ_x_1, S_y_1, 0.0, -1, Sheets, 0},
+ {BQ_x_1, S_y_2, 0.0, -1, Sheets, 0},
+ {BQ_x_1, S_y_2, 0.0, -1, Sheets, 1},
+ {BQ_x_1, S_y_1, 0.0, -1, Sheets, 0},
+ {0.00, S_y_1, 0.0, -1, NEUTRAL, 2},
+
+ {0.00, 0.35, 0.0, -1, 0, 2},
+ {0.00, 0.00, 0.0, -1, 0, 2},
+};
+
+float SF_lib[SF_NUM][6] =
+{
+ {0.00, 0.00, 0.0, -1, 0, 2},
+
+ {0.00, S_y_1, 0.0, -1, Sheets, 0},
+ {S_x_1, S_y_1, 0.0, -1, Sheets, 0},
+ {S_x_1, S_y_2, 0.0, -1, Sheets, 0},
+ {S_x_1, S_y_2, 0.0, -1, Sheets, 1},
+ {S_x_2, S_y_3, 0.0, -1, Sheets, 1},
+ {S_x_2, S_y_3, 0.0, -1, Clothespin, 1},
+ {S_x_2, S_y_3, 0.0, -1, Sheets, 1},
+ {S_x_2, S_y_1, 0.0, -1, Sheets, 1},
+ {S_x_3, S_y_1, 0.0, -1, Catch, 1},
+ {S_x_3, S_y_4, 0.0, -1, Catch, 1},
+ {S_x_4, S_y_4, 0.0, -1, Catch, 1},
+ {S_x_4, S_y_4, 0.0, -1, Catch, 0},
+ {S_x_4, S_y_5, 0.0, -1, Catch, 0},
+ {S_x_4, S_y_5, 0.0, -1, Slide, 0},
+ {S_x_5, S_y_5, 0.0, -1, Slide, 0},
+ {S_x_5, S_y_5, 0.0, -1, Slide, 1},
+ {S_x_5, S_y_1, 0.0, -1, Slide, 2},
+ {0.00, S_y_1, 0.0, -1, NEUTRAL, 2},
+
+ {0.00, 0.35, 0.0, -1, 0, 2},
+ {0.00, 0.00, 0.0, -1, 0, 2},
+};
+
+float Test_lib[SF_NUM][6] =
+{
+ {0.00, 0.00, 0.0, -1, NEUTRAL, 2},
+
+ {0.00, B_y_1, 0.0, -1, NEUTRAL, 2},
+ {BQ_x_1, B_y_1, 0.0, -1, NEUTRAL, 2},
+ {0.00, B_y_1, 0.0, -1, NEUTRAL, 2},
+
+ {0.00, 0.35, 0.0, -1, NEUTRAL, 2},
+ {0.00, 0.00, 0.0, -1, 0, 2},
+};
+
+//{Lift, wide, supply, hand, hand_mode}
+float tops_lib[TOPSNUM][5] =
+{
+ {0.20, 0.00, 0.00, 0, 1}, //NEUTRAL
+ {0.20, 0.00, 0.00, 0, 1}, //洗濯物の高さまで下げる
+ {0.20, 0.00, 0.00, 1, 1}, //掴む
+ {0.20, 0.00, 0.00, 1, 1}, //指定の高さまで持ち上げる, NEUTRAL以上の高さになったらsupplyを動かす
+ {0.20, 0.00, 0.00, 0, 1}, //竿の真上まで来たら離す
+};
+
+typedef struct topsPara
+{
+ float lift, wide;
+}topsPara;
+
+topsPara para_NEUTRAL = {0.60, 0.0};
+topsPara para_T_shirt = {2.19, 0.0};
+topsPara para_BathTowel = {5.13, 0.0};
+topsPara para_Sheets = {7.64, 0.0};
+topsPara para_Clothespin = {6.58, 0.0};
+topsPara para_Catch = {3.25, 0.0};
+topsPara para_Slide = {7.64, 0.0};
+
+topsPara para_lib[7] =
+{
+ para_NEUTRAL,
+ para_T_shirt,
+ para_BathTowel,
+ para_Sheets,
+ para_Clothespin,
+ para_Catch,
+ para_Slide,
+};
+
+float supply_lib[4] = {0.00, 1.67, 3.20, 4.73};
+
+typedef struct state
+{
+ float x, y , theta, lift, wide, supply;
+ bool hand, hand_mode;
+ int now_spline, now_route;
+}state;
+
+typedef struct elements
+{
+ float x, y , yaw;
+}elements;
\ No newline at end of file