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@29:ec1a10c7c4c8, 2018-10-25 (annotated)

Committer:
TanakaTarou
Date:
Thu Oct 25 13:46:50 2018 +0000
Revision:
29:ec1a10c7c4c8
Parent:
28:f3d000638fe1
Child:
30:0e6f5ed45445
10/25   ?????

Who changed what in which revision?

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