ichinoseki_Bteam_2019 / Mbed 2 deprecated 2019_AR_Itsuki

Dependencies:   IMU mbed Odometer PID MDD RotaryEncoder UART Mycan DriveConroller

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