ichinoseki_Bteam_2019 / Mbed 2 deprecated 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

stateLib.h@1:0f33a68d1390, 2020-01-12 (annotated)

Committer:
TanakaTarou
Date:
Sun Jan 12 08:12:42 2020 +0000
Revision:
1:0f33a68d1390
Parent:
0:56a2c0ed99c5 
Itsuki

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaTarou 0:56a2c0ed99c5 1 #define TQ_NUM 19
TanakaTarou 0:56a2c0ed99c5 2 #define TF_NUM 24
TanakaTarou 0:56a2c0ed99c5 3 #define BQ_NUM 19
TanakaTarou 0:56a2c0ed99c5 4 #define BF_NUM 24
TanakaTarou 0:56a2c0ed99c5 5 #define SQ_NUM 9
TanakaTarou 0:56a2c0ed99c5 6 #define SF_NUM 21
TanakaTarou 1:0f33a68d1390 7 #define SBQ_NUM 25
TanakaTarou 0:56a2c0ed99c5 8 #define TEST_NUM 6
TanakaTarou 0:56a2c0ed99c5 9 #define TOPSNUM 5
TanakaTarou 0:56a2c0ed99c5 10
TanakaTarou 0:56a2c0ed99c5 11 #define NEUTRAL 0
TanakaTarou 0:56a2c0ed99c5 12 #define T_shirt 1
TanakaTarou 0:56a2c0ed99c5 13 #define BathTowel 2
TanakaTarou 0:56a2c0ed99c5 14 #define Sheets 3
TanakaTarou 0:56a2c0ed99c5 15 #define Clothespin 4
TanakaTarou 0:56a2c0ed99c5 16 #define Catch 5
TanakaTarou 0:56a2c0ed99c5 17 #define Slide 6
TanakaTarou 0:56a2c0ed99c5 18
TanakaTarou 0:56a2c0ed99c5 19 #define TQ_x_1 1.800f //予選１つ目
TanakaTarou 0:56a2c0ed99c5 20 #define TQ_x_2 2.550f //予選２つ目
TanakaTarou 0:56a2c0ed99c5 21 #define TQ_x_3 3.300f //予選３つ目
TanakaTarou 0:56a2c0ed99c5 22 #define TF_x_1 1.425f //決勝１つ目
TanakaTarou 0:56a2c0ed99c5 23 #define TF_x_2 1.675f //決勝２つ目
TanakaTarou 0:56a2c0ed99c5 24 #define TF_x_3 2.025f //決勝３つ目
TanakaTarou 0:56a2c0ed99c5 25 #define TF_x_4 2.275f //決勝４つ目
TanakaTarou 0:56a2c0ed99c5 26
TanakaTarou 0:56a2c0ed99c5 27 #define T_y_1 1.700f //Tシャツラインの位置
TanakaTarou 0:56a2c0ed99c5 28 #define T_y_2 2.200f //Tシャツかける直前
TanakaTarou 0:56a2c0ed99c5 29 #define T_y_3 2.480f //Tシャツかける時
TanakaTarou 0:56a2c0ed99c5 30
TanakaTarou 0:56a2c0ed99c5 31 #define BQ_x_1 1.750f //予選１つ目
TanakaTarou 0:56a2c0ed99c5 32 #define BQ_x_2 2.550f //予選２つ目
TanakaTarou 0:56a2c0ed99c5 33 #define BQ_x_3 3.350f //予選３つ目
TanakaTarou 0:56a2c0ed99c5 34 #define BF_x_1 1.555f //決勝１つ目
TanakaTarou 0:56a2c0ed99c5 35 #define BF_x_2 2.165f //決勝２つ目
TanakaTarou 0:56a2c0ed99c5 36 #define BF_x_3 2.835f //決勝３つ目
TanakaTarou 0:56a2c0ed99c5 37 #define BF_x_4 3.445f //決勝４つ目
TanakaTarou 0:56a2c0ed99c5 38
TanakaTarou 0:56a2c0ed99c5 39 #define B_y_1 3.700f //バスタオルラインの位置
TanakaTarou 1:0f33a68d1390 40 #define B_y_2 4.250f //バスタオルかける直前
TanakaTarou 0:56a2c0ed99c5 41 #define B_y_3 4.800f //バスタオルかける時
TanakaTarou 0:56a2c0ed99c5 42
TanakaTarou 0:56a2c0ed99c5 43 #define S_x_1 1.830f //シーツをかける時
TanakaTarou 0:56a2c0ed99c5 44 #define S_x_2 1.600f //洗濯バサミをつける時
TanakaTarou 0:56a2c0ed99c5 45 #define S_x_3 2.560f //横から掴む前
TanakaTarou 0:56a2c0ed99c5 46 #define S_x_4 2.350f //横から掴む時
TanakaTarou 0:56a2c0ed99c5 47 #define S_x_5 3.450f //横に引っ張った後
TanakaTarou 0:56a2c0ed99c5 48
TanakaTarou 0:56a2c0ed99c5 49 #define S_y_1 5.675f //シーツラインの位置
TanakaTarou 0:56a2c0ed99c5 50 #define S_y_2 6.750f //シーツかける時
TanakaTarou 0:56a2c0ed99c5 51 #define S_y_3 6.470f //洗濯バサミをつける時
TanakaTarou 0:56a2c0ed99c5 52 #define S_y_4 6.475f //横から掴む時
TanakaTarou 0:56a2c0ed99c5 53 #define S_y_5 6.300f //横に引っ張る時
TanakaTarou 0:56a2c0ed99c5 54
TanakaTarou 0:56a2c0ed99c5 55 //{X, Y, θ, Spline, Laundry, Tops}
TanakaTarou 0:56a2c0ed99c5 56 float state_lib[6] = {0.00, 0.00, 0.0, -1, 0, 2};
TanakaTarou 0:56a2c0ed99c5 57
TanakaTarou 0:56a2c0ed99c5 58 float TQ_lib[TQ_NUM][6] =
TanakaTarou 0:56a2c0ed99c5 59 {
TanakaTarou 0:56a2c0ed99c5 60 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 61
TanakaTarou 0:56a2c0ed99c5 62 {0.00, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 63 {TQ_x_1, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 64 {TQ_x_1, T_y_3, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 65 {TQ_x_1, T_y_3, 0.0, -1, T_shirt, 1},
TanakaTarou 0:56a2c0ed99c5 66 {TQ_x_1, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 67 {TQ_x_2, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 68 {TQ_x_2, T_y_2, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 69 {TQ_x_2, T_y_3, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 70 {TQ_x_2, T_y_3, 0.0, -1, T_shirt, 1},
TanakaTarou 0:56a2c0ed99c5 71 {TQ_x_2, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 72 {TQ_x_3, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 73 {TQ_x_3, T_y_2, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 74 {TQ_x_3, T_y_3, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 75 {TQ_x_3, T_y_3, 0.0, -1, T_shirt, 1},
TanakaTarou 0:56a2c0ed99c5 76 {TQ_x_3, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 77 {0.00, T_y_1, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 78
TanakaTarou 0:56a2c0ed99c5 79 {0.00, 0.35, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 80 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 81 };
TanakaTarou 0:56a2c0ed99c5 82
TanakaTarou 0:56a2c0ed99c5 83 float TF_lib[TF_NUM][6] =
TanakaTarou 0:56a2c0ed99c5 84 {
TanakaTarou 0:56a2c0ed99c5 85 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 86
TanakaTarou 0:56a2c0ed99c5 87 {0.00, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 88 {TF_x_1, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 89 {TF_x_1, T_y_3, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 90 {TF_x_1, T_y_3, 0.0, -1, T_shirt, 1},
TanakaTarou 0:56a2c0ed99c5 91 {TF_x_1, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 92 {TF_x_2, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 93 {TF_x_2, T_y_2, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 94 {TF_x_2, T_y_3, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 95 {TF_x_2, T_y_3, 0.0, -1, T_shirt, 1},
TanakaTarou 0:56a2c0ed99c5 96 {TF_x_2, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 97 {TF_x_3, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 98 {TF_x_3, T_y_2, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 99 {TF_x_3, T_y_3, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 100 {TF_x_3, T_y_3, 0.0, -1, T_shirt, 1},
TanakaTarou 0:56a2c0ed99c5 101 {TF_x_3, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 102 {TF_x_4, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 103 {TF_x_4, T_y_2, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 104 {TF_x_4, T_y_3, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 105 {TF_x_4, T_y_3, 0.0, -1, T_shirt, 1},
TanakaTarou 0:56a2c0ed99c5 106 {TF_x_4, T_y_1, 0.0, -1, T_shirt, 0},
TanakaTarou 0:56a2c0ed99c5 107 {0.00, T_y_1, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 108
TanakaTarou 0:56a2c0ed99c5 109 {0.00, 0.35, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 110 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 111 };
TanakaTarou 0:56a2c0ed99c5 112
TanakaTarou 0:56a2c0ed99c5 113 float BQ_lib[BQ_NUM][6] =
TanakaTarou 0:56a2c0ed99c5 114 {
TanakaTarou 0:56a2c0ed99c5 115 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 116
TanakaTarou 0:56a2c0ed99c5 117 {0.00, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 118 {BQ_x_1, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 119 {BQ_x_1, B_y_3, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 120 {BQ_x_1, B_y_3, 0.0, -1, BathTowel, 1},
TanakaTarou 0:56a2c0ed99c5 121 {BQ_x_1, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 122 {BQ_x_2, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 123 {BQ_x_2, B_y_2, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 124 {BQ_x_2, B_y_3, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 125 {BQ_x_2, B_y_3, 0.0, -1, BathTowel, 1},
TanakaTarou 0:56a2c0ed99c5 126 {BQ_x_2, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 127 {BQ_x_3, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 128 {BQ_x_3, B_y_2, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 129 {BQ_x_3, B_y_3, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 130 {BQ_x_3, B_y_3, 0.0, -1, BathTowel, 1},
TanakaTarou 0:56a2c0ed99c5 131 {BQ_x_3, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 132 {0.00, B_y_1, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 133
TanakaTarou 0:56a2c0ed99c5 134 {0.00, 0.35, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 135 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 136 };
TanakaTarou 0:56a2c0ed99c5 137
TanakaTarou 0:56a2c0ed99c5 138 float BF_lib[BF_NUM][6] =
TanakaTarou 0:56a2c0ed99c5 139 {
TanakaTarou 0:56a2c0ed99c5 140 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 141
TanakaTarou 0:56a2c0ed99c5 142 {0.00, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 143 {BF_x_1, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 144 {BF_x_1, B_y_3, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 145 {BF_x_1, B_y_3, 0.0, -1, BathTowel, 1},
TanakaTarou 0:56a2c0ed99c5 146 {BF_x_1, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 147 {BF_x_2, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 148 {BF_x_2, B_y_2, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 149 {BF_x_2, B_y_3, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 150 {BF_x_2, B_y_3, 0.0, -1, BathTowel, 1},
TanakaTarou 0:56a2c0ed99c5 151 {BF_x_2, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 152 {BF_x_3, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 153 {BF_x_3, B_y_2, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 154 {BF_x_3, B_y_3, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 155 {BF_x_3, B_y_3, 0.0, -1, BathTowel, 1},
TanakaTarou 0:56a2c0ed99c5 156 {BF_x_3, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 157 {BF_x_4, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 158 {BF_x_4, B_y_2, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 159 {BF_x_4, B_y_3, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 160 {BF_x_4, B_y_3, 0.0, -1, BathTowel, 1},
TanakaTarou 0:56a2c0ed99c5 161 {BF_x_4, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 0:56a2c0ed99c5 162 {0.00, B_y_1, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 163
TanakaTarou 0:56a2c0ed99c5 164 {0.00, 0.35, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 165 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 166 };
TanakaTarou 0:56a2c0ed99c5 167
TanakaTarou 0:56a2c0ed99c5 168 float SQ_lib[SQ_NUM][6] =
TanakaTarou 0:56a2c0ed99c5 169 {
TanakaTarou 0:56a2c0ed99c5 170 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 171
TanakaTarou 0:56a2c0ed99c5 172 {0.00, S_y_1, 0.0, -1, Sheets, 0},
TanakaTarou 0:56a2c0ed99c5 173 {BQ_x_1, S_y_1, 0.0, -1, Sheets, 0},
TanakaTarou 0:56a2c0ed99c5 174 {BQ_x_1, S_y_2, 0.0, -1, Sheets, 0},
TanakaTarou 0:56a2c0ed99c5 175 {BQ_x_1, S_y_2, 0.0, -1, Sheets, 1},
TanakaTarou 0:56a2c0ed99c5 176 {BQ_x_1, S_y_1, 0.0, -1, Sheets, 0},
TanakaTarou 0:56a2c0ed99c5 177 {0.00, S_y_1, 0.0, -1, NEUTRAL, 2},
TanakaTarou 0:56a2c0ed99c5 178
TanakaTarou 0:56a2c0ed99c5 179 {0.00, 0.35, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 180 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 181 };
TanakaTarou 0:56a2c0ed99c5 182
TanakaTarou 0:56a2c0ed99c5 183 float SF_lib[SF_NUM][6] =
TanakaTarou 0:56a2c0ed99c5 184 {
TanakaTarou 0:56a2c0ed99c5 185 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 186
TanakaTarou 0:56a2c0ed99c5 187 {0.00, S_y_1, 0.0, -1, Sheets, 0},
TanakaTarou 0:56a2c0ed99c5 188 {S_x_1, S_y_1, 0.0, -1, Sheets, 0},
TanakaTarou 0:56a2c0ed99c5 189 {S_x_1, S_y_2, 0.0, -1, Sheets, 0},
TanakaTarou 0:56a2c0ed99c5 190 {S_x_1, S_y_2, 0.0, -1, Sheets, 1},
TanakaTarou 0:56a2c0ed99c5 191 {S_x_2, S_y_3, 0.0, -1, Sheets, 1},
TanakaTarou 0:56a2c0ed99c5 192 {S_x_2, S_y_3, 0.0, -1, Clothespin, 1},
TanakaTarou 0:56a2c0ed99c5 193 {S_x_2, S_y_3, 0.0, -1, Sheets, 1},
TanakaTarou 0:56a2c0ed99c5 194 {S_x_2, S_y_1, 0.0, -1, Sheets, 1},
TanakaTarou 0:56a2c0ed99c5 195 {S_x_3, S_y_1, 0.0, -1, Catch, 1},
TanakaTarou 0:56a2c0ed99c5 196 {S_x_3, S_y_4, 0.0, -1, Catch, 1},
TanakaTarou 0:56a2c0ed99c5 197 {S_x_4, S_y_4, 0.0, -1, Catch, 1},
TanakaTarou 0:56a2c0ed99c5 198 {S_x_4, S_y_4, 0.0, -1, Catch, 0},
TanakaTarou 0:56a2c0ed99c5 199 {S_x_4, S_y_5, 0.0, -1, Catch, 0},
TanakaTarou 0:56a2c0ed99c5 200 {S_x_4, S_y_5, 0.0, -1, Slide, 0},
TanakaTarou 0:56a2c0ed99c5 201 {S_x_5, S_y_5, 0.0, -1, Slide, 0},
TanakaTarou 0:56a2c0ed99c5 202 {S_x_5, S_y_5, 0.0, -1, Slide, 1},
TanakaTarou 0:56a2c0ed99c5 203 {S_x_5, S_y_1, 0.0, -1, Slide, 2},
TanakaTarou 0:56a2c0ed99c5 204 {0.00, S_y_1, 0.0, -1, NEUTRAL, 2},
TanakaTarou 0:56a2c0ed99c5 205
TanakaTarou 0:56a2c0ed99c5 206 {0.00, 0.35, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 207 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 208 };
TanakaTarou 0:56a2c0ed99c5 209
TanakaTarou 1:0f33a68d1390 210 float SBQ_lib[30][6] =
TanakaTarou 1:0f33a68d1390 211 {
TanakaTarou 1:0f33a68d1390 212 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 1:0f33a68d1390 213
TanakaTarou 1:0f33a68d1390 214 {0.00, S_y_1, 0.0, -1, Sheets, 0},
TanakaTarou 1:0f33a68d1390 215 {BQ_x_2, S_y_1, 0.0, -1, Sheets, 0},
TanakaTarou 1:0f33a68d1390 216 {BQ_x_2, S_y_2, 0.0, -1, Sheets, 0},
TanakaTarou 1:0f33a68d1390 217 {BQ_x_2, S_y_2, 0.0, -1, Sheets, 1},
TanakaTarou 1:0f33a68d1390 218 {BQ_x_2, S_y_1, 0.0, -1, Sheets, 0},
TanakaTarou 1:0f33a68d1390 219 {0.00, S_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 220
TanakaTarou 1:0f33a68d1390 221 {0.00, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 222 //ここから
TanakaTarou 1:0f33a68d1390 223 {BF_x_2, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 224 {BF_x_2, B_y_3, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 225 {BF_x_2, B_y_3, 0.0, -1, BathTowel, 1},
TanakaTarou 1:0f33a68d1390 226 {BF_x_2, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 227 {BF_x_3, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 228 {BF_x_3, B_y_2, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 229 {BF_x_3, B_y_3, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 230 {BF_x_3, B_y_3, 0.0, -1, BathTowel, 1},
TanakaTarou 1:0f33a68d1390 231 {BF_x_3, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 232 //ここまで変更箇所
TanakaTarou 1:0f33a68d1390 233 {BQ_x_3, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 234 {BQ_x_3, B_y_2, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 235 {BQ_x_3, B_y_3, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 236 {BQ_x_3, B_y_3, 0.0, -1, BathTowel, 1},
TanakaTarou 1:0f33a68d1390 237 {BQ_x_3, B_y_1, 0.0, -1, BathTowel, 0},
TanakaTarou 1:0f33a68d1390 238 {0.00, B_y_1, 0.0, -1, NEUTRAL, 2},
TanakaTarou 1:0f33a68d1390 239
TanakaTarou 1:0f33a68d1390 240 {0.00, 0.35, 0.0, -1, 0, 2},
TanakaTarou 1:0f33a68d1390 241 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 1:0f33a68d1390 242 };
TanakaTarou 1:0f33a68d1390 243
TanakaTarou 0:56a2c0ed99c5 244 float Test_lib[SF_NUM][6] =
TanakaTarou 0:56a2c0ed99c5 245 {
TanakaTarou 0:56a2c0ed99c5 246 {0.00, 0.00, 0.0, -1, NEUTRAL, 2},
TanakaTarou 0:56a2c0ed99c5 247
TanakaTarou 0:56a2c0ed99c5 248 {0.00, B_y_1, 0.0, -1, NEUTRAL, 2},
TanakaTarou 0:56a2c0ed99c5 249 {BQ_x_1, B_y_1, 0.0, -1, NEUTRAL, 2},
TanakaTarou 0:56a2c0ed99c5 250 {0.00, B_y_1, 0.0, -1, NEUTRAL, 2},
TanakaTarou 0:56a2c0ed99c5 251
TanakaTarou 0:56a2c0ed99c5 252 {0.00, 0.35, 0.0, -1, NEUTRAL, 2},
TanakaTarou 0:56a2c0ed99c5 253 {0.00, 0.00, 0.0, -1, 0, 2},
TanakaTarou 0:56a2c0ed99c5 254 };
TanakaTarou 0:56a2c0ed99c5 255
TanakaTarou 0:56a2c0ed99c5 256 //{Lift, wide, supply, hand, hand_mode}
TanakaTarou 0:56a2c0ed99c5 257 float tops_lib[TOPSNUM][5] =
TanakaTarou 0:56a2c0ed99c5 258 {
TanakaTarou 0:56a2c0ed99c5 259 {0.20, 0.00, 0.00, 0, 1}, //NEUTRAL
TanakaTarou 0:56a2c0ed99c5 260 {0.20, 0.00, 0.00, 0, 1}, //洗濯物の高さまで下げる
TanakaTarou 0:56a2c0ed99c5 261 {0.20, 0.00, 0.00, 1, 1}, //掴む
TanakaTarou 0:56a2c0ed99c5 262 {0.20, 0.00, 0.00, 1, 1}, //指定の高さまで持ち上げる, NEUTRAL以上の高さになったらsupplyを動かす
TanakaTarou 0:56a2c0ed99c5 263 {0.20, 0.00, 0.00, 0, 1}, //竿の真上まで来たら離す
TanakaTarou 0:56a2c0ed99c5 264 };
TanakaTarou 0:56a2c0ed99c5 265
TanakaTarou 0:56a2c0ed99c5 266 typedef struct topsPara
TanakaTarou 0:56a2c0ed99c5 267 {
TanakaTarou 0:56a2c0ed99c5 268 float lift, wide;
TanakaTarou 0:56a2c0ed99c5 269 }topsPara;
TanakaTarou 0:56a2c0ed99c5 270
TanakaTarou 0:56a2c0ed99c5 271 topsPara para_NEUTRAL = {0.60, 0.0};
TanakaTarou 0:56a2c0ed99c5 272 topsPara para_T_shirt = {2.19, 0.0};
TanakaTarou 0:56a2c0ed99c5 273 topsPara para_BathTowel = {5.13, 0.0};
TanakaTarou 0:56a2c0ed99c5 274 topsPara para_Sheets = {7.64, 0.0};
TanakaTarou 0:56a2c0ed99c5 275 topsPara para_Clothespin = {6.58, 0.0};
TanakaTarou 0:56a2c0ed99c5 276 topsPara para_Catch = {3.25, 0.0};
TanakaTarou 0:56a2c0ed99c5 277 topsPara para_Slide = {7.64, 0.0};
TanakaTarou 0:56a2c0ed99c5 278
TanakaTarou 0:56a2c0ed99c5 279 topsPara para_lib[7] =
TanakaTarou 0:56a2c0ed99c5 280 {
TanakaTarou 0:56a2c0ed99c5 281 para_NEUTRAL,
TanakaTarou 0:56a2c0ed99c5 282 para_T_shirt,
TanakaTarou 0:56a2c0ed99c5 283 para_BathTowel,
TanakaTarou 0:56a2c0ed99c5 284 para_Sheets,
TanakaTarou 0:56a2c0ed99c5 285 para_Clothespin,
TanakaTarou 0:56a2c0ed99c5 286 para_Catch,
TanakaTarou 0:56a2c0ed99c5 287 para_Slide,
TanakaTarou 0:56a2c0ed99c5 288 };
TanakaTarou 0:56a2c0ed99c5 289
TanakaTarou 0:56a2c0ed99c5 290 float supply_lib[4] = {0.00, 1.67, 3.20, 4.73};
TanakaTarou 0:56a2c0ed99c5 291
TanakaTarou 0:56a2c0ed99c5 292 typedef struct state
TanakaTarou 0:56a2c0ed99c5 293 {
TanakaTarou 0:56a2c0ed99c5 294 float x, y , theta, lift, wide, supply;
TanakaTarou 0:56a2c0ed99c5 295 bool hand, hand_mode;
TanakaTarou 0:56a2c0ed99c5 296 int now_spline, now_route;
TanakaTarou 0:56a2c0ed99c5 297 }state;
TanakaTarou 0:56a2c0ed99c5 298
TanakaTarou 0:56a2c0ed99c5 299 typedef struct elements
TanakaTarou 0:56a2c0ed99c5 300 {
TanakaTarou 0:56a2c0ed99c5 301 float x, y , yaw;
TanakaTarou 0:56a2c0ed99c5 302 }elements;