ichinoseki_Bteam_2019 / Mbed 2 deprecated 2019_AR_Itsuki

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;