Artem Solomatin / Mbed 2 deprecated pendudu

fsdfds

Dependencies:   mbed

main.cpp@9:e8c6a414e226, 2020-05-21 (annotated)

Committer:
teamat
Date:
Thu May 21 15:35:04 2020 +0000
Revision:
9:e8c6a414e226
Parent:
8:b3ce040fdebc
Child:
10:ca38e4d775c8
new commit in the wall

Who changed what in which revision?

UserRevisionLine numberNew contents of line
teamat 3:8708e61475fe 1 #include "mbed.h"
teamat 3:8708e61475fe 2
teamat 3:8708e61475fe 3 #define M_PI 3.14159265358979323846f
teamat 3:8708e61475fe 4 #define smoothingNumber 6
teamat 3:8708e61475fe 5
teamat 3:8708e61475fe 6 #define STATE_ERROR 0
teamat 3:8708e61475fe 7 #define STATE_INIT 1
teamat 3:8708e61475fe 8 #define STATE_GOTO_START 2
teamat 3:8708e61475fe 9 #define STATE_GOTO_END_COUNTING 3
teamat 3:8708e61475fe 10 #define STATE_GOTO_MIDDLE 4
teamat 3:8708e61475fe 11 #define STATE_WAITING 5
teamat 3:8708e61475fe 12 #define STATE_GOTO_SWING 6
teamat 3:8708e61475fe 13 #define STATE_START_SWING 7
teamat 3:8708e61475fe 14 #define STATE_SWING_RIGHT 8
teamat 3:8708e61475fe 15 #define STATE_SWING_LEFT 9
teamat 3:8708e61475fe 16 #define STATE_EMPTY_SWING 10
teamat 3:8708e61475fe 17
teamat 3:8708e61475fe 18 #define DIR_LEFT 0
teamat 3:8708e61475fe 19 #define DIR_RIGHT 1
teamat 3:8708e61475fe 20
teamat 3:8708e61475fe 21 DigitalOut RCout(D10);
teamat 3:8708e61475fe 22 DigitalOut dir(D9);
teamat 3:8708e61475fe 23 InterruptIn leftSwitch(D2);
teamat 3:8708e61475fe 24 InterruptIn rightSwitch(D3);
teamat 3:8708e61475fe 25 Ticker tick;
teamat 4:fd49edfabfb2 26 Ticker speedTicker;
teamat 4:fd49edfabfb2 27 Ticker swingTicker;
teamat 6:999e8ae7d969 28 Ticker sendDataTicker;
teamat 3:8708e61475fe 29 DigitalOut myled(LED2);
teamat 3:8708e61475fe 30 RawSerial rpc(D1,D0,9600);
teamat 3:8708e61475fe 31
teamat 3:8708e61475fe 32 int period_us = 26;//300 26
teamat 7:ca62dda005d5 33 int posCounter = 0;
teamat 3:8708e61475fe 34
teamat 3:8708e61475fe 35 long pos=0;
teamat 3:8708e61475fe 36
teamat 3:8708e61475fe 37 long railLength = 0;
teamat 3:8708e61475fe 38
teamat 3:8708e61475fe 39 uint8_t state=STATE_INIT;
teamat 3:8708e61475fe 40
teamat 3:8708e61475fe 41 Timer t;
teamat 3:8708e61475fe 42 SPI spi(D11,D12,D13);// mosi, miso, sclk
teamat 3:8708e61475fe 43 DigitalOut cs(D5);
teamat 3:8708e61475fe 44
teamat 4:fd49edfabfb2 45 float radius = 0.0025;
teamat 3:8708e61475fe 46
teamat 3:8708e61475fe 47 float angularPosNew = 0;
teamat 3:8708e61475fe 48 float angularPosOld = 0;
teamat 3:8708e61475fe 49
teamat 3:8708e61475fe 50 float timeOld = 0.0f;
teamat 3:8708e61475fe 51 float timeStart = 0.0f;
teamat 3:8708e61475fe 52
teamat 3:8708e61475fe 53 float dx = 0;
teamat 3:8708e61475fe 54 float xPosNew = 0;
teamat 3:8708e61475fe 55 float xPosOld = 0;
teamat 8:b3ce040fdebc 56 float speed = -1;
teamat 8:b3ce040fdebc 57 float posOffset = 0.0f;
teamat 7:ca62dda005d5 58 float posMap[4] = {0.0f, 0.0f, 0.0f, 0.0f};
teamat 3:8708e61475fe 59
teamat 3:8708e61475fe 60 double dAngle = 0.0f;
teamat 3:8708e61475fe 61 double anSpd = 0.0f;
teamat 3:8708e61475fe 62
teamat 3:8708e61475fe 63 float timeOldPos = 0.0f;
teamat 3:8708e61475fe 64 float timeStartPos = 0.0f;
teamat 3:8708e61475fe 65
teamat 4:fd49edfabfb2 66 float angle=0.0f;
teamat 3:8708e61475fe 67 float angleOffset = 0;
teamat 3:8708e61475fe 68
teamat 4:fd49edfabfb2 69 float control = 0.0f;
teamat 4:fd49edfabfb2 70
teamat 3:8708e61475fe 71 double PIPI = 6.28;
teamat 3:8708e61475fe 72
teamat 3:8708e61475fe 73 bool canSend = false;
teamat 8:b3ce040fdebc 74 bool periodUpdated = false;
teamat 8:b3ce040fdebc 75 bool calibrated = false;
teamat 9:e8c6a414e226 76 bool dirUpdated = false;
teamat 3:8708e61475fe 77
teamat 3:8708e61475fe 78 typedef union {
teamat 4:fd49edfabfb2 79 float number[6];
teamat 4:fd49edfabfb2 80 uint8_t numberCh[24];
teamat 3:8708e61475fe 81 } my_union;
teamat 3:8708e61475fe 82
teamat 3:8708e61475fe 83 my_union myUnion;
teamat 3:8708e61475fe 84
teamat 3:8708e61475fe 85 bool isPendulumSwinging() {
teamat 3:8708e61475fe 86 return state == STATE_SWING_RIGHT || state == STATE_SWING_LEFT;
teamat 3:8708e61475fe 87 }
teamat 3:8708e61475fe 88
teamat 3:8708e61475fe 89 float getPosMM() {
teamat 3:8708e61475fe 90 //return (pos-railLength/2) * 550.0f/railLength;
teamat 3:8708e61475fe 91 //return (pos - railLength / 2) * (350.0f / railLength);
teamat 3:8708e61475fe 92 return pos / 3500.0f;
teamat 3:8708e61475fe 93 }
teamat 3:8708e61475fe 94
teamat 3:8708e61475fe 95 uint16_t getPendulumPos(){
teamat 3:8708e61475fe 96 cs=0;
teamat 3:8708e61475fe 97 wait_ms(1);
teamat 3:8708e61475fe 98 uint16_t d=spi.write((short)0x00);
teamat 3:8708e61475fe 99 d=d<<1;//fucking shithole fakebit
teamat 3:8708e61475fe 100 d=d>>6;//no need debug info
teamat 3:8708e61475fe 101 cs=1;
teamat 3:8708e61475fe 102 wait_ms(1);
teamat 3:8708e61475fe 103 return (uint16_t)d;
teamat 3:8708e61475fe 104 }
teamat 3:8708e61475fe 105
teamat 3:8708e61475fe 106
teamat 3:8708e61475fe 107 float getPendulumAngle(){
teamat 3:8708e61475fe 108 angle = getPendulumPos();
teamat 3:8708e61475fe 109 angle = angle * 6.28f / 1024.0f;
teamat 4:fd49edfabfb2 110 angle += angleOffset;
teamat 8:b3ce040fdebc 111 if (angle > 3.14) {
teamat 8:b3ce040fdebc 112 angle = angle - PIPI;
teamat 8:b3ce040fdebc 113 }
teamat 8:b3ce040fdebc 114 /*if (angle > PIPI + 0.01) {
teamat 4:fd49edfabfb2 115 angle = fmod((angle + 3.14), PIPI) - 3.14;
teamat 4:fd49edfabfb2 116 if (angle < -3.14) {
teamat 4:fd49edfabfb2 117 angle += PIPI;
teamat 4:fd49edfabfb2 118 }
teamat 9:e8c6a414e226 119 }*/
teamat 8:b3ce040fdebc 120 /*if (calibrated) {
teamat 8:b3ce040fdebc 121 float test = fmodf(angle, M_PI);
teamat 8:b3ce040fdebc 122 if (test >= 1) {
teamat 8:b3ce040fdebc 123 angle = -test;
teamat 8:b3ce040fdebc 124 } else {
teamat 8:b3ce040fdebc 125 angle = test;
teamat 8:b3ce040fdebc 126 }
teamat 8:b3ce040fdebc 127 }*/
teamat 3:8708e61475fe 128 return angle;
teamat 3:8708e61475fe 129 }
teamat 3:8708e61475fe 130
teamat 3:8708e61475fe 131
teamat 3:8708e61475fe 132 /*
teamat 3:8708e61475fe 133 ANGULAR SPEED CALC
teamat 3:8708e61475fe 134 */
teamat 3:8708e61475fe 135
teamat 3:8708e61475fe 136 void getDeltaAng(){
teamat 3:8708e61475fe 137
teamat 3:8708e61475fe 138 angularPosNew = getPendulumAngle();
teamat 3:8708e61475fe 139
teamat 3:8708e61475fe 140 dAngle = fmod((angularPosNew - angularPosOld + 3.14), PIPI) - 3.14;
teamat 3:8708e61475fe 141 if (dAngle < -3.14) {
teamat 3:8708e61475fe 142 dAngle += PIPI;
teamat 3:8708e61475fe 143 }
teamat 3:8708e61475fe 144
teamat 3:8708e61475fe 145 angularPosOld = angularPosNew;
teamat 3:8708e61475fe 146
teamat 3:8708e61475fe 147 }
teamat 3:8708e61475fe 148
teamat 3:8708e61475fe 149 void getAngularSpeed(){
teamat 3:8708e61475fe 150 float deltaTime;
teamat 3:8708e61475fe 151
teamat 3:8708e61475fe 152 timeStart = float(t.read());
teamat 3:8708e61475fe 153 deltaTime = (timeStart - timeOld);
teamat 3:8708e61475fe 154 getDeltaAng();
teamat 3:8708e61475fe 155 anSpd = dAngle / deltaTime;
teamat 3:8708e61475fe 156 timeOld=timeStart;
teamat 3:8708e61475fe 157 //взятие по модулю, спросить
teamat 3:8708e61475fe 158 }
teamat 3:8708e61475fe 159
teamat 3:8708e61475fe 160 /*
teamat 3:8708e61475fe 161 SPEED CALC
teamat 3:8708e61475fe 162 */
teamat 3:8708e61475fe 163
teamat 4:fd49edfabfb2 164 void calcSpeed() {
teamat 7:ca62dda005d5 165 /*posMap[posCounter] = getPosMM() / 1000;
teamat 7:ca62dda005d5 166 if (posCounter == 3) {
teamat 7:ca62dda005d5 167 posCounter = 0;
teamat 7:ca62dda005d5 168 float pathSum = 0;
teamat 7:ca62dda005d5 169 for (int i = 0; i < 3; i++) {
teamat 7:ca62dda005d5 170 float pathDiff = posMap[i + 1] - posMap[i];
teamat 7:ca62dda005d5 171 if ((pathDiff) < 0) {
teamat 7:ca62dda005d5 172 pathSum += -pathDiff;
teamat 7:ca62dda005d5 173 } else {
teamat 7:ca62dda005d5 174 pathSum += pathDiff;
teamat 7:ca62dda005d5 175 }
teamat 7:ca62dda005d5 176 }
teamat 7:ca62dda005d5 177 speed = pathSum;
teamat 7:ca62dda005d5 178 } else {
teamat 7:ca62dda005d5 179 posCounter += 1;
teamat 7:ca62dda005d5 180 }*/
teamat 8:b3ce040fdebc 181 xPosNew = (getPosMM() - posOffset) / 1000;
teamat 4:fd49edfabfb2 182 speed = xPosNew - xPosOld;
teamat 8:b3ce040fdebc 183 if (dir == DIR_LEFT) {
teamat 8:b3ce040fdebc 184 speed = -speed;
teamat 8:b3ce040fdebc 185 }
teamat 9:e8c6a414e226 186 //speed = -speed;
teamat 8:b3ce040fdebc 187
teamat 4:fd49edfabfb2 188 xPosOld = xPosNew;
teamat 4:fd49edfabfb2 189 }
teamat 4:fd49edfabfb2 190
teamat 4:fd49edfabfb2 191 void calcControl() {
teamat 6:999e8ae7d969 192 //float frequency = 1000000 / (period_us / 2);
teamat 6:999e8ae7d969 193 //float rates = frequency / 6400;
teamat 6:999e8ae7d969 194 //control = rates * PIPI * radius;
teamat 8:b3ce040fdebc 195 if (dir = DIR_RIGHT) {
teamat 8:b3ce040fdebc 196 control = -period_us;
teamat 8:b3ce040fdebc 197 } else {
teamat 8:b3ce040fdebc 198 control = period_us;
teamat 8:b3ce040fdebc 199 }
teamat 3:8708e61475fe 200 }
teamat 3:8708e61475fe 201
teamat 3:8708e61475fe 202
teamat 3:8708e61475fe 203
teamat 3:8708e61475fe 204 void stepperFlip() {
teamat 3:8708e61475fe 205 if (state != STATE_WAITING && state != STATE_ERROR && state != STATE_INIT && state){
teamat 3:8708e61475fe 206 RCout = !RCout;
teamat 3:8708e61475fe 207 pos += (dir.read() * 2 - 1);
teamat 3:8708e61475fe 208 }
teamat 3:8708e61475fe 209 if (state == STATE_GOTO_MIDDLE && pos == railLength / 2) {
teamat 8:b3ce040fdebc 210 posOffset = 40;
teamat 3:8708e61475fe 211 state = STATE_WAITING;
teamat 3:8708e61475fe 212 }
teamat 3:8708e61475fe 213 if (state == STATE_SWING_LEFT && state==STATE_SWING_RIGHT) {
teamat 3:8708e61475fe 214 pos += (dir.read() * 2 - 1);
teamat 3:8708e61475fe 215 RCout = !RCout;
teamat 3:8708e61475fe 216 }
teamat 3:8708e61475fe 217 }
teamat 3:8708e61475fe 218
teamat 3:8708e61475fe 219
teamat 3:8708e61475fe 220 void updatePeriod(){
teamat 3:8708e61475fe 221 tick.detach();
teamat 3:8708e61475fe 222 tick.attach_us (&stepperFlip, period_us / 2.0f);
teamat 9:e8c6a414e226 223 wait_ms(20);
teamat 3:8708e61475fe 224 }
teamat 3:8708e61475fe 225
teamat 3:8708e61475fe 226 void leftEnd() {
teamat 3:8708e61475fe 227 dir = DIR_RIGHT;
teamat 3:8708e61475fe 228 if (state == STATE_GOTO_START) {
teamat 3:8708e61475fe 229 state = STATE_GOTO_END_COUNTING;
teamat 3:8708e61475fe 230 pos = 0;
teamat 3:8708e61475fe 231 }
teamat 3:8708e61475fe 232 else if (isPendulumSwinging()) {
teamat 9:e8c6a414e226 233 //state = STATE_GOTO_MIDDLE;
teamat 3:8708e61475fe 234 //angleOffset -= 0.006191;
teamat 9:e8c6a414e226 235 state = STATE_ERROR;
teamat 3:8708e61475fe 236 }
teamat 9:e8c6a414e226 237 if (state == STATE_GOTO_SWING) {
teamat 9:e8c6a414e226 238 state = STATE_ERROR;
teamat 9:e8c6a414e226 239 }
teamat 3:8708e61475fe 240 }
teamat 3:8708e61475fe 241
teamat 3:8708e61475fe 242 void rightEnd() {
teamat 3:8708e61475fe 243 dir=DIR_LEFT;
teamat 3:8708e61475fe 244 if (state == STATE_GOTO_END_COUNTING) {
teamat 3:8708e61475fe 245 railLength=pos;
teamat 3:8708e61475fe 246 state = STATE_GOTO_MIDDLE;
teamat 3:8708e61475fe 247 }
teamat 3:8708e61475fe 248 else if (isPendulumSwinging()) {
teamat 9:e8c6a414e226 249 //state = STATE_GOTO_MIDDLE;
teamat 3:8708e61475fe 250 //angleOffset += 0.006191;
teamat 9:e8c6a414e226 251 state = STATE_ERROR;
teamat 9:e8c6a414e226 252 }
teamat 9:e8c6a414e226 253 if (state == STATE_GOTO_SWING) {
teamat 9:e8c6a414e226 254 state = STATE_ERROR;
teamat 3:8708e61475fe 255 }
teamat 3:8708e61475fe 256 }
teamat 3:8708e61475fe 257
teamat 9:e8c6a414e226 258 int swingCounter = 0;
teamat 3:8708e61475fe 259 void getSwingDirectory() {
teamat 9:e8c6a414e226 260 swingCounter += 1;
teamat 7:ca62dda005d5 261 control = -control;
teamat 4:fd49edfabfb2 262 if (dir == DIR_RIGHT) {
teamat 3:8708e61475fe 263 state = STATE_SWING_RIGHT;
teamat 3:8708e61475fe 264 dir = DIR_LEFT;
teamat 3:8708e61475fe 265 return;
teamat 3:8708e61475fe 266 }
teamat 4:fd49edfabfb2 267 if (dir == DIR_LEFT) {
teamat 3:8708e61475fe 268 state = STATE_SWING_LEFT;
teamat 3:8708e61475fe 269 dir = DIR_RIGHT;
teamat 3:8708e61475fe 270 return;
teamat 3:8708e61475fe 271 }
teamat 3:8708e61475fe 272 }
teamat 3:8708e61475fe 273
teamat 3:8708e61475fe 274 void sendData() {
teamat 3:8708e61475fe 275 myUnion.number[0] = t.read();
teamat 8:b3ce040fdebc 276 myUnion.number[1] = (getPosMM()- posOffset) / 1000 ;
teamat 4:fd49edfabfb2 277 myUnion.number[2] = speed;
teamat 4:fd49edfabfb2 278 //myUnion.number[3] = dAngle;
teamat 4:fd49edfabfb2 279 myUnion.number[3] = angle;
teamat 3:8708e61475fe 280 myUnion.number[4] = anSpd;
teamat 4:fd49edfabfb2 281 myUnion.number[5] = control;
teamat 4:fd49edfabfb2 282 for(int i = 0; i < 24; i++) {
teamat 3:8708e61475fe 283 rpc.putc(myUnion.numberCh[i]);
teamat 3:8708e61475fe 284 }
teamat 3:8708e61475fe 285 }
teamat 3:8708e61475fe 286
teamat 3:8708e61475fe 287 void Rx_interrupt() {
teamat 4:fd49edfabfb2 288 int command = rpc.getc();
teamat 8:b3ce040fdebc 289 switch (command) {
teamat 8:b3ce040fdebc 290 case 50:
teamat 8:b3ce040fdebc 291 canSend = true;
teamat 8:b3ce040fdebc 292 break;
teamat 8:b3ce040fdebc 293 case 60:
teamat 8:b3ce040fdebc 294 state = STATE_GOTO_SWING;
teamat 8:b3ce040fdebc 295 speedTicker.attach(calcSpeed, 1);
teamat 8:b3ce040fdebc 296 break;
teamat 8:b3ce040fdebc 297 case 65:
teamat 8:b3ce040fdebc 298 int direction = rpc.getc();
teamat 8:b3ce040fdebc 299 if (direction <= 0) {
teamat 9:e8c6a414e226 300 if (dir != DIR_RIGHT) {
teamat 8:b3ce040fdebc 301 dir = DIR_RIGHT;
teamat 9:e8c6a414e226 302 }
teamat 8:b3ce040fdebc 303 } else if (direction > 0) {
teamat 9:e8c6a414e226 304 if (dir != DIR_LEFT) {
teamat 9:e8c6a414e226 305 dir = DIR_LEFT;
teamat 9:e8c6a414e226 306 }
teamat 8:b3ce040fdebc 307 }
teamat 8:b3ce040fdebc 308 break;
teamat 8:b3ce040fdebc 309 case 70:
teamat 8:b3ce040fdebc 310 int newPeriod = rpc.getc();
teamat 8:b3ce040fdebc 311 if (newPeriod < 26) {
teamat 8:b3ce040fdebc 312 tick.detach();
teamat 9:e8c6a414e226 313 } else if (period_us != newPeriod) {
teamat 8:b3ce040fdebc 314 period_us = newPeriod;
teamat 8:b3ce040fdebc 315 periodUpdated = true;
teamat 8:b3ce040fdebc 316 }
teamat 8:b3ce040fdebc 317 break;
teamat 8:b3ce040fdebc 318 default:
teamat 8:b3ce040fdebc 319 break;
teamat 3:8708e61475fe 320 }
teamat 3:8708e61475fe 321 return;
teamat 3:8708e61475fe 322 }
teamat 3:8708e61475fe 323
teamat 3:8708e61475fe 324 int main() {
teamat 3:8708e61475fe 325 RCout = 1;
teamat 3:8708e61475fe 326 wait_ms(500);
teamat 3:8708e61475fe 327 spi.format(16,2);
teamat 3:8708e61475fe 328 spi.frequency(1000000);
teamat 3:8708e61475fe 329 t.start();
teamat 3:8708e61475fe 330 leftSwitch.rise(&leftEnd);
teamat 3:8708e61475fe 331 rightSwitch.rise(&rightEnd);
teamat 4:fd49edfabfb2 332 rpc.attach(&Rx_interrupt, Serial::RxIrq);
teamat 3:8708e61475fe 333 for (int i=5; i>0; i--) {
teamat 3:8708e61475fe 334 getPendulumAngle();
teamat 3:8708e61475fe 335 wait_ms(500);
teamat 3:8708e61475fe 336 }
teamat 4:fd49edfabfb2 337 angleOffset= 3.14 - angle;
teamat 8:b3ce040fdebc 338 calibrated = true;
teamat 8:b3ce040fdebc 339 //wait(12);
teamat 8:b3ce040fdebc 340 wait(7);
teamat 3:8708e61475fe 341 updatePeriod();
teamat 6:999e8ae7d969 342 calcControl();
teamat 3:8708e61475fe 343 state=STATE_GOTO_START;
teamat 3:8708e61475fe 344 dir=DIR_LEFT;
teamat 3:8708e61475fe 345 while(1) {
teamat 4:fd49edfabfb2 346 getAngularSpeed();
teamat 8:b3ce040fdebc 347 if (canSend) {
teamat 3:8708e61475fe 348 sendData();
teamat 3:8708e61475fe 349 }
teamat 9:e8c6a414e226 350 /*if (periodUpdated) {
teamat 8:b3ce040fdebc 351 calcControl();
teamat 8:b3ce040fdebc 352 updatePeriod();
teamat 8:b3ce040fdebc 353 periodUpdated = false;
teamat 9:e8c6a414e226 354 }*/
teamat 3:8708e61475fe 355 switch(state) {
teamat 3:8708e61475fe 356 case STATE_WAITING:
teamat 8:b3ce040fdebc 357 //state = STATE_GOTO_SWING;
teamat 3:8708e61475fe 358 break;
teamat 3:8708e61475fe 359 case STATE_GOTO_START:
teamat 3:8708e61475fe 360 break;
teamat 3:8708e61475fe 361 case STATE_GOTO_END_COUNTING:
teamat 3:8708e61475fe 362 break;
teamat 3:8708e61475fe 363 case STATE_GOTO_SWING:
teamat 9:e8c6a414e226 364 //speedTicker.attach(calcSpeed, 1);
teamat 8:b3ce040fdebc 365 //swingTicker.attach(getSwingDirectory, 1);
teamat 8:b3ce040fdebc 366 //state = STATE_SWING_LEFT;
teamat 3:8708e61475fe 367 break;
teamat 3:8708e61475fe 368 case STATE_SWING_LEFT:
teamat 9:e8c6a414e226 369 /*if (swingCounter == 6) {
teamat 9:e8c6a414e226 370 swingCounter = 0;
teamat 9:e8c6a414e226 371 period_us += 4;
teamat 9:e8c6a414e226 372 periodUpdated = true;
teamat 9:e8c6a414e226 373 }*/
teamat 3:8708e61475fe 374 break;
teamat 3:8708e61475fe 375 case STATE_SWING_RIGHT:
teamat 9:e8c6a414e226 376 /*if (swingCounter == 6) {
teamat 9:e8c6a414e226 377 swingCounter = 0;
teamat 9:e8c6a414e226 378 period_us += 4;
teamat 9:e8c6a414e226 379 periodUpdated = true;
teamat 9:e8c6a414e226 380 }*/
teamat 3:8708e61475fe 381 break;
teamat 3:8708e61475fe 382 default:
teamat 3:8708e61475fe 383 break;
teamat 3:8708e61475fe 384 }
teamat 9:e8c6a414e226 385 wait_ms(100);
teamat 3:8708e61475fe 386 }
teamat 3:8708e61475fe 387 }