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
stateLib.h
- Committer:
- TanakaTarou
- Date:
- 2020-01-12
- Revision:
- 1:0f33a68d1390
- Parent:
- 0:56a2c0ed99c5
File content as of revision 1:0f33a68d1390:
#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 SBQ_NUM 25 #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 //予選1つ目 #define TQ_x_2 2.550f //予選2つ目 #define TQ_x_3 3.300f //予選3つ目 #define TF_x_1 1.425f //決勝1つ目 #define TF_x_2 1.675f //決勝2つ目 #define TF_x_3 2.025f //決勝3つ目 #define TF_x_4 2.275f //決勝4つ目 #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 //予選1つ目 #define BQ_x_2 2.550f //予選2つ目 #define BQ_x_3 3.350f //予選3つ目 #define BF_x_1 1.555f //決勝1つ目 #define BF_x_2 2.165f //決勝2つ目 #define BF_x_3 2.835f //決勝3つ目 #define BF_x_4 3.445f //決勝4つ目 #define B_y_1 3.700f //バスタオルラインの位置 #define B_y_2 4.250f //バスタオルかける直前 #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 SBQ_lib[30][6] = { {0.00, 0.00, 0.0, -1, 0, 2}, {0.00, S_y_1, 0.0, -1, Sheets, 0}, {BQ_x_2, S_y_1, 0.0, -1, Sheets, 0}, {BQ_x_2, S_y_2, 0.0, -1, Sheets, 0}, {BQ_x_2, S_y_2, 0.0, -1, Sheets, 1}, {BQ_x_2, S_y_1, 0.0, -1, Sheets, 0}, {0.00, S_y_1, 0.0, -1, BathTowel, 0}, {0.00, B_y_1, 0.0, -1, BathTowel, 0}, //ここから {BF_x_2, B_y_1, 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}, //ここまで変更箇所 {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, 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;