MEC-B / Mbed 2 deprecated AR_MastarNode_copy

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:   DriveConroller IMU MDD Mycan Odometer PID RotaryEncoder UART USS mbed

Fork of AR_MastarNode by MEC-B

stateLib.h@30:0e6f5ed45445, 2018-11-07 (annotated)

Committer:
TanakaTarou
Date:
Wed Nov 07 13:50:24 2018 +0000
Revision:
30:0e6f5ed45445
Parent:
29:ec1a10c7c4c8
Child:
32:952bf02857f8
11/07    B2?10?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaTarou 30:0e6f5ed45445 1 #define LIBNUM 86
TanakaTarou 26:322309c42aa4 2
soyooo 18:268ab2ab0b2a 3 #define NEUTRAL 0
soyooo 18:268ab2ab0b2a 4 #define A1 1
soyooo 18:268ab2ab0b2a 5 #define A2 2
soyooo 18:268ab2ab0b2a 6 #define A3 3
soyooo 18:268ab2ab0b2a 7 #define B1 4
TanakaTarou 30:0e6f5ed45445 8 #define B1_a 5
TanakaTarou 30:0e6f5ed45445 9 #define B1_b 6
TanakaTarou 30:0e6f5ed45445 10 #define B2 7
TanakaTarou 30:0e6f5ed45445 11 #define B2_a 8
TanakaTarou 30:0e6f5ed45445 12 #define B2_b 9
TanakaTarou 30:0e6f5ed45445 13 #define B2_c 10
TanakaTarou 30:0e6f5ed45445 14 #define C1 11
TanakaTarou 30:0e6f5ed45445 15 #define C2 12
TanakaTarou 30:0e6f5ed45445 16 #define C3 13
TanakaTarou 26:322309c42aa4 17
TanakaTarou 24:91ecfbbf23ee 18 #define B_x_1 0.840f
TanakaTarou 24:91ecfbbf23ee 19 #define B_x_2 1.840f
TanakaTarou 24:91ecfbbf23ee 20 #define B_x_3 2.840f
TanakaTarou 26:322309c42aa4 21 #define B_x_s1 1.750f
TanakaTarou 26:322309c42aa4 22 #define B_x_s3 1.930f
TanakaTarou 30:0e6f5ed45445 23 #define B_x_s4 1.795f
TanakaTarou 30:0e6f5ed45445 24 #define B_x_s5 1.885f
TanakaTarou 24:91ecfbbf23ee 25 #define B_y_1 1.5f
TanakaTarou 24:91ecfbbf23ee 26 #define B_y_2 2.5f
TanakaTarou 24:91ecfbbf23ee 27 #define B_y_3 3.5f
TanakaTarou 30:0e6f5ed45445 28 #define B_uss 105.0f
TanakaTarou 21:4912835805a8 29 #define e_posi 2.840f
TanakaTarou 21:4912835805a8 30 #define C1_x 3.840f
TanakaTarou 21:4912835805a8 31 #define C2_x 4.840f
TanakaTarou 21:4912835805a8 32 #define C3_x 5.840f
TanakaTarou 26:322309c42aa4 33 #define A_x 0.8f
TanakaTarou 19:bdb503dd1e8c 34 #define A1_y 1.5f
TanakaTarou 19:bdb503dd1e8c 35 #define A2_y 2.5f
TanakaTarou 19:bdb503dd1e8c 36 #define A3_y 3.5f
TanakaTarou 20:1957c67ab740 37 #define A_yow 90.0f
TanakaTarou 26:322309c42aa4 38 /*
TanakaTarou 26:322309c42aa4 39 #define B_x_1 0.280f
TanakaTarou 26:322309c42aa4 40 #define B_x_2 0.613f
TanakaTarou 26:322309c42aa4 41 #define B_x_3 0.947f
TanakaTarou 30:0e6f5ed45445 42 #define B_x_s1 0.513f
TanakaTarou 30:0e6f5ed45445 43 #define B_x_s3 0.713f
TanakaTarou 26:322309c42aa4 44 #define B_y_1 0.5f
TanakaTarou 26:322309c42aa4 45 #define B_y_2 0.833f
TanakaTarou 26:322309c42aa4 46 #define B_y_3 1.167f
TanakaTarou 30:0e6f5ed45445 47 #define B_uss 105.0f
TanakaTarou 26:322309c42aa4 48 #define e_posi 0.947f
TanakaTarou 26:322309c42aa4 49 #define C1_x 1.28f
TanakaTarou 30:0e6f5ed45445 50 #define C2_x 1.613f
TanakaTarou 30:0e6f5ed45445 51 #define C3_x 1.947f
TanakaTarou 26:322309c42aa4 52 #define A_x 0.3f
TanakaTarou 26:322309c42aa4 53 #define A1_y 0.5f
TanakaTarou 26:322309c42aa4 54 #define A2_y 0.833f
TanakaTarou 26:322309c42aa4 55 #define A3_y 1.167f
TanakaTarou 26:322309c42aa4 56 #define A_yow 90.0f
TanakaTarou 25:0176b8d3d042 57 */
TanakaTarou 30:0e6f5ed45445 58
TanakaTarou 19:bdb503dd1e8c 59 float state_lib[LIBNUM][5] ={{0.0, 0.0, 0, NEUTRAL, 0},//スタートゾーン
TanakaTarou 19:bdb503dd1e8c 60
TanakaTarou 30:0e6f5ed45445 61 {B_x_1, B_y_2, -90, B2, 0},//手前側面単発
TanakaTarou 30:0e6f5ed45445 62 {B_uss, B_y_2, -90, B2, 0},
TanakaTarou 30:0e6f5ed45445 63 {B_uss, B_y_2, -90, B2, 1},
TanakaTarou 28:f3d000638fe1 64 {B_x_1, B_y_1, -90, B2, 0},//4
TanakaTarou 28:f3d000638fe1 65
TanakaTarou 30:0e6f5ed45445 66 {B_x_2, 0, 0, B2, 0},//奥側面単発
TanakaTarou 30:0e6f5ed45445 67 {B_x_3, B_y_2, 90, B2, 0},
TanakaTarou 30:0e6f5ed45445 68 {-B_uss, B_y_2, 90, B2, 0},
TanakaTarou 30:0e6f5ed45445 69 {-B_uss, B_y_2, 90, B2, 1},
TanakaTarou 30:0e6f5ed45445 70 {B_x_3, B_y_1, 0, B2, 0},//9
TanakaTarou 28:f3d000638fe1 71
TanakaTarou 30:0e6f5ed45445 72 {B_x_s1, B_y_1, 0, B1_b, 0},//B1 7発
TanakaTarou 30:0e6f5ed45445 73 {B_x_s1, B_uss, 0, B1_b, 0},
TanakaTarou 30:0e6f5ed45445 74 {B_x_s1, B_uss, 0, B1_b, 1},
TanakaTarou 30:0e6f5ed45445 75 {B_x_2, B_uss, 0, B1_b, 0},
TanakaTarou 30:0e6f5ed45445 76 {B_x_2, B_uss, 0, B1_b, 1},
TanakaTarou 30:0e6f5ed45445 77 {B_x_s3, B_uss, 0, B1_b, 0},
TanakaTarou 30:0e6f5ed45445 78 {B_x_s3, B_uss, 0, B1_b, 1},//16
TanakaTarou 24:91ecfbbf23ee 79
TanakaTarou 30:0e6f5ed45445 80 {B_x_s1, B_y_1, 0, B1_a, 0},
TanakaTarou 30:0e6f5ed45445 81 {B_x_s1, B_uss, 0, B1_a, 0},
TanakaTarou 30:0e6f5ed45445 82 {B_x_s1, B_uss, 0, B1_a, 1},
TanakaTarou 30:0e6f5ed45445 83 {B_x_2, B_uss, 0, B1_a, 0},
TanakaTarou 30:0e6f5ed45445 84 {B_x_2, B_uss, 0, B1_a, 1},
TanakaTarou 30:0e6f5ed45445 85 {B_x_s3, B_uss, 0, B1_a, 0},
TanakaTarou 30:0e6f5ed45445 86 {B_x_s3, B_uss, 0, B1_a, 1},//23
TanakaTarou 29:ec1a10c7c4c8 87
TanakaTarou 30:0e6f5ed45445 88 {B_x_s4, B_y_1, 0, B2_c, 0},//B2 奥5発
TanakaTarou 30:0e6f5ed45445 89 {B_x_s4, B_uss, 0, B2_c, 0},
TanakaTarou 30:0e6f5ed45445 90 {B_x_s4, B_uss, 0, B2_c, 1},
TanakaTarou 30:0e6f5ed45445 91 {B_x_s5, B_uss, 0, B2_c, 0},
TanakaTarou 30:0e6f5ed45445 92 {B_x_s5, B_uss, 0, B2_c, 1},
TanakaTarou 30:0e6f5ed45445 93 {B_x_s1, B_uss, 0, B2_b, 0},
TanakaTarou 30:0e6f5ed45445 94 {B_x_s1, B_uss, 0, B2_b, 1},
TanakaTarou 30:0e6f5ed45445 95 {B_x_2, B_uss, 0, B2_b, 0},
TanakaTarou 30:0e6f5ed45445 96 {B_x_2, B_uss, 0, B2_b, 1},
TanakaTarou 30:0e6f5ed45445 97 {B_x_s3, B_uss, 0, B2_b, 0},
TanakaTarou 30:0e6f5ed45445 98 {B_x_s3, B_uss, 0, B2_b, 1},//34
TanakaTarou 29:ec1a10c7c4c8 99
TanakaTarou 30:0e6f5ed45445 100 {B_x_s1, B_y_1, 0, B2_a, 0},//B2 真ん中3発
TanakaTarou 30:0e6f5ed45445 101 {B_x_s1, B_uss, 0, B2_a, 0},
TanakaTarou 30:0e6f5ed45445 102 {B_x_s1, B_uss, 0, B2_a, 1},
TanakaTarou 30:0e6f5ed45445 103 {B_x_2, B_uss, 0, B2_a, 0},
TanakaTarou 30:0e6f5ed45445 104 {B_x_2, B_uss, 0, B2_a, 1},
TanakaTarou 30:0e6f5ed45445 105 {B_x_s3, B_uss, 0, B2_a, 0},
TanakaTarou 30:0e6f5ed45445 106 {B_x_s3, B_uss, 0, B2_a, 1},//41
TanakaTarou 29:ec1a10c7c4c8 107
TanakaTarou 30:0e6f5ed45445 108 {B_x_s4, B_uss, 0, B2, 0},//B2 手前2発
TanakaTarou 30:0e6f5ed45445 109 {B_x_s4, B_uss, 0, B2, 1},
TanakaTarou 30:0e6f5ed45445 110 {B_x_s5, B_uss, 0, B2, 0},
TanakaTarou 30:0e6f5ed45445 111 {B_x_s5, B_uss, 0, B2, 1},//45
TanakaTarou 29:ec1a10c7c4c8 112
TanakaTarou 30:0e6f5ed45445 113 {B_x_2, B_y_1, 0, B1, 0},//B1単発
TanakaTarou 30:0e6f5ed45445 114 {B_x_2, B_uss, 0, B1, 0},
TanakaTarou 30:0e6f5ed45445 115 {B_x_2, B_uss, 0, B1, 1},//48
TanakaTarou 30:0e6f5ed45445 116
TanakaTarou 30:0e6f5ed45445 117 {B_x_2, B_y_1, 0, B2, 0},//B2単発
TanakaTarou 30:0e6f5ed45445 118 {B_x_2, B_uss, 0, B2, 0},
TanakaTarou 30:0e6f5ed45445 119 {B_x_2, B_uss, 0, B2, 1},//51
TanakaTarou 28:f3d000638fe1 120
TanakaTarou 30:0e6f5ed45445 121 {B_x_2, B_y_1, 0, B2, 0},//B1・B2連続射出
TanakaTarou 30:0e6f5ed45445 122 {B_x_2, B_uss, 0, B2, 0},
TanakaTarou 30:0e6f5ed45445 123 {B_x_2, B_uss, 0, B2, 1},
TanakaTarou 30:0e6f5ed45445 124 {B_x_2, B_uss, 0, B1, 0},
TanakaTarou 30:0e6f5ed45445 125 {B_x_2, B_uss, 0, B1, 0},
TanakaTarou 30:0e6f5ed45445 126 {B_x_2, B_uss, 0, B1, 1},//57
TanakaTarou 30:0e6f5ed45445 127
TanakaTarou 30:0e6f5ed45445 128 {e_posi, 0.0, 0, NEUTRAL, 0},//58
TanakaTarou 28:f3d000638fe1 129
TanakaTarou 28:f3d000638fe1 130 {C1_x, 0.0, 0, C1, 0},
TanakaTarou 28:f3d000638fe1 131 {C1_x, 20.0, 0, C1, 0},
TanakaTarou 28:f3d000638fe1 132 {C1_x, 20.0, 0, C1, 1},
TanakaTarou 30:0e6f5ed45445 133 {C1_x, 0.0, 0, C1, 0},//62
TanakaTarou 28:f3d000638fe1 134
TanakaTarou 28:f3d000638fe1 135 {C2_x, 0.0, 0, C2, 0},
TanakaTarou 28:f3d000638fe1 136 {C2_x, 20.0, 0, C2, 0},
TanakaTarou 28:f3d000638fe1 137 {C2_x, 20.0, 0, C2, 1},
TanakaTarou 30:0e6f5ed45445 138 {C2_x, 0.0, 0, C2, 0},//66
TanakaTarou 28:f3d000638fe1 139
TanakaTarou 28:f3d000638fe1 140 {C3_x, 0.0, 0, C3, 0},
TanakaTarou 28:f3d000638fe1 141 {C3_x, 20.0, 0, C3, 0},
TanakaTarou 28:f3d000638fe1 142 {C3_x, 20.0, 0, C3, 1},
TanakaTarou 30:0e6f5ed45445 143 {C3_x, 0.0, 0, C3, 0},//70
TanakaTarou 24:91ecfbbf23ee 144
TanakaTarou 30:0e6f5ed45445 145 {e_posi, 0.0, 0, NEUTRAL, 0},//71
TanakaTarou 28:f3d000638fe1 146
TanakaTarou 28:f3d000638fe1 147 {0.3, A1_y, A_yow, A1, 0},
TanakaTarou 28:f3d000638fe1 148 {-17, A1_y, A_yow, A1, 0},
TanakaTarou 28:f3d000638fe1 149 {-17, A1_y, A_yow, A1, 1},
TanakaTarou 30:0e6f5ed45445 150 {A_x, A1_y, A_yow, A1, 0},//75
TanakaTarou 28:f3d000638fe1 151
TanakaTarou 28:f3d000638fe1 152 {A_x, A2_y, A_yow, A2, 0},
TanakaTarou 28:f3d000638fe1 153 {-17, A2_y, A_yow, A2, 0},
TanakaTarou 28:f3d000638fe1 154 {-17, A2_y, A_yow, A2, 1},
TanakaTarou 30:0e6f5ed45445 155 {A_x, A2_y, A_yow, A2, 0},//79
TanakaTarou 28:f3d000638fe1 156
TanakaTarou 28:f3d000638fe1 157 {A_x, A3_y, A_yow, A3, 0},
TanakaTarou 28:f3d000638fe1 158 {-17, A3_y, A_yow, A3, 0},
TanakaTarou 28:f3d000638fe1 159 {-17, A3_y, A_yow, A3, 1},
TanakaTarou 30:0e6f5ed45445 160 {A_x, A3_y, A_yow, A3, 0},//83
TanakaTarou 28:f3d000638fe1 161
TanakaTarou 30:0e6f5ed45445 162 {A_x, A1_y, 0, NEUTRAL, 0},//84
TanakaTarou 24:91ecfbbf23ee 163
soyooo 18:268ab2ab0b2a 164 {0.0, 0.0, 0, NEUTRAL, 0}// 補給, スタートゾーン戻る
soyooo 18:268ab2ab0b2a 165 };
soyooo 18:268ab2ab0b2a 166
soyooo 18:268ab2ab0b2a 167 float tops_lib[4][3] =
soyooo 18:268ab2ab0b2a 168 {
TanakaTarou 30:0e6f5ed45445 169 { 0, 89.6, 0},
TanakaTarou 30:0e6f5ed45445 170 { 0, 89.6, 1},
TanakaTarou 30:0e6f5ed45445 171 { 0, 89.6, 0},
TanakaTarou 30:0e6f5ed45445 172 { 0, 89.6, 0}
soyooo 18:268ab2ab0b2a 173 };
soyooo 18:268ab2ab0b2a 174
soyooo 18:268ab2ab0b2a 175 typedef struct topsPara
soyooo 18:268ab2ab0b2a 176 {
soyooo 18:268ab2ab0b2a 177 float vel, angle;
soyooo 18:268ab2ab0b2a 178 }topsPara;
soyooo 18:268ab2ab0b2a 179
TanakaTarou 30:0e6f5ed45445 180 topsPara para_NEUTRAL = {0, 89.6};
TanakaTarou 27:ac970a07ef47 181
TanakaTarou 20:1957c67ab740 182 topsPara para_A1 = {18, 83};
TanakaTarou 21:4912835805a8 183 topsPara para_A2 = {26, 83.5};
TanakaTarou 20:1957c67ab740 184 topsPara para_A3 = {18, 83};
TanakaTarou 19:bdb503dd1e8c 185
TanakaTarou 30:0e6f5ed45445 186 topsPara para_B1 = {32, 85.2};
TanakaTarou 30:0e6f5ed45445 187 topsPara para_B1_a = {35, 84.5};
TanakaTarou 30:0e6f5ed45445 188 topsPara para_B1_b = {38, 83.8};
soyooo 18:268ab2ab0b2a 189
TanakaTarou 30:0e6f5ed45445 190 topsPara para_B2 = {58, 85.0};
TanakaTarou 30:0e6f5ed45445 191 topsPara para_B2_a = {58, 84.4};
TanakaTarou 30:0e6f5ed45445 192 topsPara para_B2_b = {58, 83.9};
TanakaTarou 30:0e6f5ed45445 193 topsPara para_B2_c = {58, 83.5};
TanakaTarou 30:0e6f5ed45445 194
TanakaTarou 30:0e6f5ed45445 195 topsPara para_C1 = {34, 84.2};
TanakaTarou 30:0e6f5ed45445 196 topsPara para_C2 = {41, 85.2};
TanakaTarou 30:0e6f5ed45445 197 topsPara para_C3 = {48, 86.0};
TanakaTarou 27:ac970a07ef47 198 /*
TanakaTarou 26:322309c42aa4 199 topsPara para_A1 = {0, 83};
TanakaTarou 26:322309c42aa4 200 topsPara para_A2 = {0, 83.5};
TanakaTarou 26:322309c42aa4 201 topsPara para_A3 = {0, 83};
soyooo 18:268ab2ab0b2a 202
TanakaTarou 30:0e6f5ed45445 203 topsPara para_B1 = {0, 85.2};
TanakaTarou 30:0e6f5ed45445 204 topsPara para_B1_a = {0, 84.5};
TanakaTarou 30:0e6f5ed45445 205 topsPara para_B1_b = {0, 83.8};
soyooo 18:268ab2ab0b2a 206
TanakaTarou 30:0e6f5ed45445 207 topsPara para_B2 = {0, 85.0};
TanakaTarou 30:0e6f5ed45445 208 topsPara para_B2_a = {0, 84.4};
TanakaTarou 30:0e6f5ed45445 209 topsPara para_B2_b = {0, 83.9};
TanakaTarou 30:0e6f5ed45445 210 topsPara para_B2_c = {0, 83.5};
TanakaTarou 30:0e6f5ed45445 211
TanakaTarou 30:0e6f5ed45445 212 topsPara para_C1 = {0, 84.2};
TanakaTarou 30:0e6f5ed45445 213 topsPara para_C2 = {0, 85.2};
TanakaTarou 30:0e6f5ed45445 214 topsPara para_C3 = {0, 86.0};
TanakaTarou 27:ac970a07ef47 215 */
TanakaTarou 30:0e6f5ed45445 216 topsPara para_lib[14] =
soyooo 18:268ab2ab0b2a 217 {
soyooo 18:268ab2ab0b2a 218 para_NEUTRAL,
soyooo 18:268ab2ab0b2a 219 para_A1,
soyooo 18:268ab2ab0b2a 220 para_A2,
soyooo 18:268ab2ab0b2a 221 para_A3,
soyooo 18:268ab2ab0b2a 222 para_B1,
TanakaTarou 30:0e6f5ed45445 223 para_B1_a,
TanakaTarou 30:0e6f5ed45445 224 para_B1_b,
soyooo 18:268ab2ab0b2a 225 para_B2,
TanakaTarou 27:ac970a07ef47 226 para_B2_a,
TanakaTarou 26:322309c42aa4 227 para_B2_b,
TanakaTarou 30:0e6f5ed45445 228 para_B2_c,
soyooo 18:268ab2ab0b2a 229 para_C1,
soyooo 18:268ab2ab0b2a 230 para_C2,
soyooo 18:268ab2ab0b2a 231 para_C3
soyooo 18:268ab2ab0b2a 232 };
soyooo 9:ce5a1315fe0d 233
soyooo 9:ce5a1315fe0d 234 typedef struct state
soyooo 9:ce5a1315fe0d 235 {
soyooo 9:ce5a1315fe0d 236 float x, y , theta, shoot, angle, supply;
soyooo 9:ce5a1315fe0d 237 }state;
soyooo 9:ce5a1315fe0d 238
soyooo 9:ce5a1315fe0d 239 typedef struct elements
soyooo 9:ce5a1315fe0d 240 {
soyooo 9:ce5a1315fe0d 241 float x, y , theta;
soyooo 18:268ab2ab0b2a 242 }elements;