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@26:322309c42aa4, 2018-10-23 (annotated)

Committer:
TanakaTarou
Date:
Tue Oct 23 14:14:06 2018 +0000
Revision:
26:322309c42aa4
Parent:
25:0176b8d3d042
Child:
27:ac970a07ef47
10/23

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