RTOS / Mbed OS RTOS_Project

project van rtos

Dependencies:   mbed-os

Fork of MFRC522 by Martin Olejar

main.cpp@10:65c046928e5d, 2018-05-17 (annotated)

Committer:
Bram122
Date:
Thu May 17 13:52:41 2018 +0000
Revision:
10:65c046928e5d
Parent:
9:f1f71e42e267 
Gefixt doordat Danilo zijn laptop kut deed ;p

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Onon 2:c719944e080b 1
Onon 2:c719944e080b 2 #include "mbed.h"
Onon 2:c719944e080b 3 #include "MFRC522.h"
Bram122 4:325954591d0f 4 #include "rtos.h"
Onon 2:c719944e080b 5
Onon 2:c719944e080b 6 //KL25Z Pins for MFRC522 SPI interface
Onon 2:c719944e080b 7 #define SPI_MOSI p5
Onon 2:c719944e080b 8 #define SPI_MISO p6
Onon 2:c719944e080b 9 #define SPI_SCLK p7
Onon 2:c719944e080b 10 #define SPI_CS p9
Onon 2:c719944e080b 11 // KL25Z Pin for MFRC522 reset
Onon 2:c719944e080b 12 #define MF_RESET p8
Onon 2:c719944e080b 13 /**
Onon 2:c719944e080b 14 // KL25Z Pins for Debug UART port
Onon 2:c719944e080b 15 #define UART_RX gnd
Onon 2:c719944e080b 16 #define UART_TX gnd
Onon 2:c719944e080b 17
Onon 2:c719944e080b 18 Serial DebugUART(UART_TX, UART_RX);
Onon 2:c719944e080b 19 **/
Onon 2:c719944e080b 20 MFRC522 RfChip(SPI_MOSI, SPI_MISO, SPI_SCLK, SPI_CS, MF_RESET);
Onon 2:c719944e080b 21 Serial pc(USBTX, USBRX);
Onon 2:c719944e080b 22
Bram122 5:e45681989201 23 DigitalOut led1(LED1);
Bram122 5:e45681989201 24 DigitalOut led2(LED2);
Bram122 5:e45681989201 25 DigitalOut led3(LED3);
Bram122 5:e45681989201 26 DigitalOut led4(LED4);
Onon 3:80837b470692 27 DigitalOut dg0(p27);
Onon 3:80837b470692 28 DigitalOut dg1(p28);
Onon 3:80837b470692 29 DigitalOut dg2(p29);
Onon 3:80837b470692 30 DigitalOut dg3(p30);
Onon 3:80837b470692 31
Onon 3:80837b470692 32 DigitalOut da(p26);
Onon 3:80837b470692 33 DigitalOut db(p25);
Onon 3:80837b470692 34 DigitalOut dc(p24);
Onon 3:80837b470692 35 DigitalOut dd(p23);
Onon 3:80837b470692 36 DigitalOut de(p22);
Onon 3:80837b470692 37 DigitalOut df(p21);
Onon 3:80837b470692 38 DigitalOut dg(p19);
Onon 3:80837b470692 39 DigitalOut ddp(p18);
Onon 3:80837b470692 40
Onon 3:80837b470692 41 AnalogIn a0(p20);
Onon 3:80837b470692 42
Bram122 5:e45681989201 43 DigitalOut IN1(p11);
Bram122 5:e45681989201 44 DigitalOut IN2(p12);
Bram122 5:e45681989201 45 DigitalOut IN3(p13);
Bram122 5:e45681989201 46 DigitalOut IN4(p14);
Bram122 5:e45681989201 47
Bram122 10:65c046928e5d 48 DigitalIn POWER(p17);
Bram122 10:65c046928e5d 49
Bram122 5:e45681989201 50 float sensorValue = 0.0000;
Bram122 4:325954591d0f 51
Bram122 6:c034f08291ab 52 int speedcontrol = 0.00001;
Bram122 6:c034f08291ab 53
Bram122 10:65c046928e5d 54 bool loggedIn = 0;
Bram122 10:65c046928e5d 55
Onon 3:80837b470692 56 int sevenSeg;
Onon 3:80837b470692 57
Onon 3:80837b470692 58 void light7Seg(int nr, int output) {
Onon 3:80837b470692 59 da = 0;
Onon 3:80837b470692 60 db = 0;
Onon 3:80837b470692 61 dc = 0;
Onon 3:80837b470692 62 dd = 0;
Onon 3:80837b470692 63 de = 0;
Onon 3:80837b470692 64 df = 0;
Onon 3:80837b470692 65 dg = 0;
Onon 3:80837b470692 66 ddp = 0;
Onon 3:80837b470692 67 dg0 = 1;
Onon 3:80837b470692 68 dg1 = 1;
Onon 3:80837b470692 69 dg2 = 1;
Onon 3:80837b470692 70 dg3 = 1;
Onon 3:80837b470692 71 switch(output) {
Onon 3:80837b470692 72 case 0:
Onon 3:80837b470692 73 dg0 = 0;
Onon 3:80837b470692 74 break;
Onon 3:80837b470692 75 case 1:
Onon 3:80837b470692 76 dg1 = 0;
Onon 3:80837b470692 77 break;
Onon 3:80837b470692 78 case 2:
Onon 3:80837b470692 79 dg2 = 0;
Onon 3:80837b470692 80 break;
Onon 3:80837b470692 81 case 3:
Onon 3:80837b470692 82 dg3 = 0;
Onon 3:80837b470692 83 ddp = 1;
Onon 3:80837b470692 84 break;
Onon 3:80837b470692 85 default:
Onon 3:80837b470692 86 break;
Onon 3:80837b470692 87 }
Onon 3:80837b470692 88 switch(nr) {
Onon 3:80837b470692 89 case 0:
Onon 3:80837b470692 90 da = 1;
Onon 3:80837b470692 91 db = 1;
Onon 3:80837b470692 92 dc = 1;
Onon 3:80837b470692 93 dd = 1;
Onon 3:80837b470692 94 de = 1;
Onon 3:80837b470692 95 df = 1;
Onon 3:80837b470692 96 dg = 0;
Onon 3:80837b470692 97 break;
Onon 3:80837b470692 98 case 1:
Onon 3:80837b470692 99 da = 0;
Onon 3:80837b470692 100 db = 1;
Onon 3:80837b470692 101 dc = 1;
Onon 3:80837b470692 102 dd = 0;
Onon 3:80837b470692 103 de = 0;
Onon 3:80837b470692 104 df = 0;
Onon 3:80837b470692 105 dg = 0;
Onon 3:80837b470692 106 break;
Onon 3:80837b470692 107 case 2:
Onon 3:80837b470692 108 da = 1;
Onon 3:80837b470692 109 db = 1;
Onon 3:80837b470692 110 dc = 0;
Onon 3:80837b470692 111 dd = 1;
Onon 3:80837b470692 112 de = 1;
Onon 3:80837b470692 113 df = 0;
Onon 3:80837b470692 114 dg = 1;
Onon 3:80837b470692 115 break;
Onon 3:80837b470692 116 case 3:
Onon 3:80837b470692 117 da = 1;
Onon 3:80837b470692 118 db = 1;
Onon 3:80837b470692 119 dc = 1;
Onon 3:80837b470692 120 dd = 1;
Onon 3:80837b470692 121 de = 0;
Onon 3:80837b470692 122 df = 0;
Onon 3:80837b470692 123 dg = 1;
Onon 3:80837b470692 124 break;
Onon 3:80837b470692 125 case 4:
Onon 3:80837b470692 126 da = 0;
Onon 3:80837b470692 127 db = 1;
Onon 3:80837b470692 128 dc = 1;
Onon 3:80837b470692 129 dd = 0;
Onon 3:80837b470692 130 de = 0;
Onon 3:80837b470692 131 df = 1;
Onon 3:80837b470692 132 dg = 1;
Onon 3:80837b470692 133 break;
Onon 3:80837b470692 134 case 5:
Onon 3:80837b470692 135 da = 1;
Onon 3:80837b470692 136 db = 0;
Onon 3:80837b470692 137 dc = 1;
Onon 3:80837b470692 138 dd = 1;
Onon 3:80837b470692 139 de = 0;
Onon 3:80837b470692 140 df = 1;
Onon 3:80837b470692 141 dg = 1;
Onon 3:80837b470692 142 break;
Onon 3:80837b470692 143 case 6:
Onon 3:80837b470692 144 da = 1;
Onon 3:80837b470692 145 db = 0;
Onon 3:80837b470692 146 dc = 1;
Onon 3:80837b470692 147 dd = 1;
Onon 3:80837b470692 148 de = 1;
Onon 3:80837b470692 149 df = 1;
Onon 3:80837b470692 150 dg = 1;
Onon 3:80837b470692 151 break;
Onon 3:80837b470692 152 case 7:
Onon 3:80837b470692 153 da = 1;
Onon 3:80837b470692 154 db = 1;
Onon 3:80837b470692 155 dc = 1;
Onon 3:80837b470692 156 dd = 0;
Onon 3:80837b470692 157 de = 0;
Onon 3:80837b470692 158 df = 0;
Onon 3:80837b470692 159 dg = 0;
Onon 3:80837b470692 160 break;
Onon 3:80837b470692 161 case 8:
Onon 3:80837b470692 162 da = 1;
Onon 3:80837b470692 163 db = 1;
Onon 3:80837b470692 164 dc = 1;
Onon 3:80837b470692 165 dd = 1;
Onon 3:80837b470692 166 de = 1;
Onon 3:80837b470692 167 df = 1;
Onon 3:80837b470692 168 dg = 1;
Onon 3:80837b470692 169 break;
Onon 3:80837b470692 170 case 9:
Onon 3:80837b470692 171 da = 1;
Onon 3:80837b470692 172 db = 1;
Onon 3:80837b470692 173 dc = 1;
Onon 3:80837b470692 174 dd = 1;
Onon 3:80837b470692 175 de = 0;
Onon 3:80837b470692 176 df = 1;
Onon 3:80837b470692 177 dg = 1;
Onon 3:80837b470692 178 break;
Onon 3:80837b470692 179 case 10:
Onon 3:80837b470692 180 da = 1;
Onon 3:80837b470692 181 db = 1;
Onon 3:80837b470692 182 dc = 1;
Onon 3:80837b470692 183 dd = 0;
Onon 3:80837b470692 184 de = 1;
Onon 3:80837b470692 185 df = 1;
Onon 3:80837b470692 186 dg = 1;
Onon 3:80837b470692 187 break;
Onon 3:80837b470692 188 case 11:
Onon 3:80837b470692 189 da = 0;
Onon 3:80837b470692 190 db = 0;
Onon 3:80837b470692 191 dc = 1;
Onon 3:80837b470692 192 dd = 1;
Onon 3:80837b470692 193 de = 1;
Onon 3:80837b470692 194 df = 1;
Onon 3:80837b470692 195 dg = 1;
Onon 3:80837b470692 196 break;
Onon 3:80837b470692 197 case 12:
Onon 3:80837b470692 198 da = 1;
Onon 3:80837b470692 199 db = 0;
Onon 3:80837b470692 200 dc = 0;
Onon 3:80837b470692 201 dd = 1;
Onon 3:80837b470692 202 de = 1;
Onon 3:80837b470692 203 df = 1;
Onon 3:80837b470692 204 dg = 0;
Onon 3:80837b470692 205 break;
Onon 3:80837b470692 206 case 13:
Onon 3:80837b470692 207 da = 0;
Onon 3:80837b470692 208 db = 1;
Onon 3:80837b470692 209 dc = 1;
Onon 3:80837b470692 210 dd = 1;
Onon 3:80837b470692 211 de = 1;
Onon 3:80837b470692 212 df = 0;
Onon 3:80837b470692 213 dg = 1;
Onon 3:80837b470692 214 break;
Onon 3:80837b470692 215 case 14:
Onon 3:80837b470692 216 da = 1;
Onon 3:80837b470692 217 db = 0;
Onon 3:80837b470692 218 dc = 0;
Onon 3:80837b470692 219 dd = 1;
Onon 3:80837b470692 220 de = 1;
Onon 3:80837b470692 221 df = 1;
Onon 3:80837b470692 222 dg = 1;
Onon 3:80837b470692 223 break;
Onon 3:80837b470692 224 case 15:
Onon 3:80837b470692 225 da = 1;
Onon 3:80837b470692 226 db = 0;
Onon 3:80837b470692 227 dc = 0;
Onon 3:80837b470692 228 dd = 0;
Onon 3:80837b470692 229 de = 1;
Onon 3:80837b470692 230 df = 1;
Onon 3:80837b470692 231 dg = 1;
Onon 3:80837b470692 232 break;
Onon 3:80837b470692 233 default:
Onon 3:80837b470692 234 da = 0;
Onon 3:80837b470692 235 db = 0;
Onon 3:80837b470692 236 dc = 0;
Onon 3:80837b470692 237 dd = 0;
Onon 3:80837b470692 238 de = 0;
Onon 3:80837b470692 239 df = 0;
Onon 3:80837b470692 240 dg = 0;
Onon 3:80837b470692 241 ddp = 0;
Onon 3:80837b470692 242 }
Onon 3:80837b470692 243 }
Onon 3:80837b470692 244
Bram122 5:e45681989201 245 void control() {
Bram122 5:e45681989201 246 while (true) {
Bram122 5:e45681989201 247 led1 = IN1;
Bram122 5:e45681989201 248 led2 = IN2;
Bram122 5:e45681989201 249 led3 = IN3;
Bram122 5:e45681989201 250 led4 = IN4;
Bram122 5:e45681989201 251 }
Bram122 5:e45681989201 252 }
Bram122 5:e45681989201 253
Bram122 5:e45681989201 254 void motorControl() {
Bram122 5:e45681989201 255 while(true) {
Bram122 10:65c046928e5d 256 bool PWR = POWER.read();
Bram122 10:65c046928e5d 257 if(loggedIn == 1 && PWR == 1) {
Bram122 6:c034f08291ab 258 IN1 = 1;
Bram122 6:c034f08291ab 259 IN2 = 0;
Bram122 6:c034f08291ab 260 IN3 = 1;
Bram122 6:c034f08291ab 261 IN4 = 0;
Bram122 6:c034f08291ab 262
Onon 9:f1f71e42e267 263 //Thread::wait(speedcontrol - speedcontrol/sensorValue);
Onon 9:f1f71e42e267 264 Thread::wait(100-100*sensorValue);
Bram122 6:c034f08291ab 265
Bram122 6:c034f08291ab 266 IN1 = 1;
Bram122 6:c034f08291ab 267 IN2 = 0;
Bram122 6:c034f08291ab 268 IN3 = 0;
Bram122 6:c034f08291ab 269 IN4 = 1;
Bram122 6:c034f08291ab 270
Onon 9:f1f71e42e267 271 //Thread::wait(speedcontrol - speedcontrol/sensorValue);
Onon 9:f1f71e42e267 272 Thread::wait(100-100*sensorValue);
Bram122 6:c034f08291ab 273
Bram122 6:c034f08291ab 274 IN1 = 0;
Bram122 6:c034f08291ab 275 IN2 = 1;
Bram122 6:c034f08291ab 276 IN3 = 0;
Bram122 6:c034f08291ab 277 IN4 = 1;
Bram122 6:c034f08291ab 278
Onon 9:f1f71e42e267 279 //Thread::wait(speedcontrol - speedcontrol/sensorValue);
Onon 9:f1f71e42e267 280 Thread::wait(100-100*sensorValue);
Bram122 6:c034f08291ab 281
Bram122 6:c034f08291ab 282 IN1 = 0;
Bram122 6:c034f08291ab 283 IN2 = 1;
Bram122 6:c034f08291ab 284 IN3 = 1;
Bram122 6:c034f08291ab 285 IN4 = 0;
Bram122 6:c034f08291ab 286
Onon 9:f1f71e42e267 287 //Thread::wait(speedcontrol - speedcontrol/sensorValue);
Onon 9:f1f71e42e267 288 Thread::wait(100-100*sensorValue);
Bram122 10:65c046928e5d 289 } else {
Bram122 10:65c046928e5d 290 Thread::wait(100);
Bram122 10:65c046928e5d 291 }
Bram122 5:e45681989201 292 }
Bram122 5:e45681989201 293 }
Bram122 5:e45681989201 294
Bram122 5:e45681989201 295 void displayNumber7Seg() {
Bram122 5:e45681989201 296 while (true) {
Bram122 5:e45681989201 297 float nr = sensorValue;
Bram122 5:e45681989201 298 nr *= 1000;
Bram122 5:e45681989201 299 int nrint = (int) nr;
Bram122 5:e45681989201 300 int digits[4];
Bram122 5:e45681989201 301 int i = 3;
Bram122 5:e45681989201 302 while (nrint >= 0) {
Bram122 5:e45681989201 303 int digit = nrint % 10;
Bram122 5:e45681989201 304 digits[i] = digit;
Bram122 5:e45681989201 305 if(i == 0) {
Bram122 5:e45681989201 306 break;
Bram122 5:e45681989201 307 }
Bram122 5:e45681989201 308 --i;
Bram122 5:e45681989201 309 nrint /= 10;
Bram122 5:e45681989201 310 }
Bram122 5:e45681989201 311 for(int i = 0; i < 4; i++) {
Bram122 5:e45681989201 312 light7Seg(digits[3-i], i);
Bram122 5:e45681989201 313 Thread::wait(100/24); //60Hz
Onon 3:80837b470692 314 }
Onon 3:80837b470692 315 }
Onon 3:80837b470692 316 }
Onon 2:c719944e080b 317
Bram122 4:325954591d0f 318 void samplePotentio() {
Bram122 4:325954591d0f 319 while (true) {
Bram122 4:325954591d0f 320 float samples[20];
Bram122 4:325954591d0f 321 float sum = 0;
Bram122 4:325954591d0f 322 for(int i = 0; i < 20; i++) {
Bram122 4:325954591d0f 323 samples[i] = a0.read();
Bram122 5:e45681989201 324 Thread::wait(5);
Bram122 4:325954591d0f 325 }
Bram122 5:e45681989201 326 for (int i = 0; i < 20; i++) {
Bram122 4:325954591d0f 327 sum = sum + samples[i];
Bram122 4:325954591d0f 328 }
Bram122 4:325954591d0f 329 sensorValue = sum / 20;
Bram122 4:325954591d0f 330 }
Bram122 4:325954591d0f 331 }
Bram122 4:325954591d0f 332
Onon 9:f1f71e42e267 333
Onon 7:1c712013123b 334 void checkRFID() {
Bram122 10:65c046928e5d 335 pc.printf("start rfid");
Bram122 10:65c046928e5d 336 uint8_t mastercard [] = {0x04, 0x89, 0x39, 0x1a, 0xee, 0x32, 0x80};
Bram122 10:65c046928e5d 337 bool mastercardFound = 0;
Onon 9:f1f71e42e267 338 // Init. RC522 Chip
Onon 9:f1f71e42e267 339 RfChip.PCD_Init();
Onon 7:1c712013123b 340 while (true) {
Onon 7:1c712013123b 341
Onon 7:1c712013123b 342 // Look for new cards
Onon 7:1c712013123b 343 if ( ! RfChip.PICC_IsNewCardPresent())
Onon 7:1c712013123b 344 {
Onon 7:1c712013123b 345 Thread::wait(500);
Onon 7:1c712013123b 346 continue;
Onon 7:1c712013123b 347 }
Onon 7:1c712013123b 348
Onon 7:1c712013123b 349 // Select one of the cards
Onon 7:1c712013123b 350 if ( ! RfChip.PICC_ReadCardSerial())
Onon 7:1c712013123b 351 {
Onon 7:1c712013123b 352 Thread::wait(500);
Onon 7:1c712013123b 353 continue;
Onon 7:1c712013123b 354 }
Onon 7:1c712013123b 355
Onon 7:1c712013123b 356 // Print Card UID
Onon 7:1c712013123b 357 pc.printf("Card UID: ");
Onon 7:1c712013123b 358 for (uint8_t i = 0; i < RfChip.uid.size; i++)
Onon 7:1c712013123b 359 {
Bram122 10:65c046928e5d 360 led2 = 1;
Bram122 10:65c046928e5d 361 pc.printf("card: %02x ", RfChip.uid.uidByte[i]);
Bram122 10:65c046928e5d 362 pc.printf("master: %02x ", mastercard[i]);
Bram122 10:65c046928e5d 363 if (RfChip.uid.uidByte[i] != mastercard[i]) {
Bram122 10:65c046928e5d 364 mastercardFound = 0;
Bram122 10:65c046928e5d 365 break;
Bram122 10:65c046928e5d 366 } else {
Bram122 10:65c046928e5d 367 mastercardFound = 1;
Bram122 10:65c046928e5d 368 }
Onon 7:1c712013123b 369 }
Bram122 10:65c046928e5d 370 pc.printf("\r\n");
Onon 7:1c712013123b 371
Bram122 10:65c046928e5d 372 led3 = 1;
Bram122 10:65c046928e5d 373
Bram122 10:65c046928e5d 374 if (mastercardFound == 1) {
Bram122 10:65c046928e5d 375 led4 = 1;
Bram122 10:65c046928e5d 376 loggedIn = !loggedIn;
Bram122 10:65c046928e5d 377 led1 = loggedIn;
Bram122 10:65c046928e5d 378 }
Onon 7:1c712013123b 379 Thread::wait(1000);
Bram122 10:65c046928e5d 380 led2 = 0;
Bram122 10:65c046928e5d 381 led3 = 0;
Bram122 10:65c046928e5d 382 led4 = 0;
Onon 7:1c712013123b 383 }
Onon 7:1c712013123b 384 }
Onon 7:1c712013123b 385
Onon 7:1c712013123b 386
Onon 2:c719944e080b 387 int main(void) {
Onon 2:c719944e080b 388 pc.printf("Start");
Bram122 4:325954591d0f 389
Bram122 5:e45681989201 390 Thread thread_sample;
Bram122 5:e45681989201 391 thread_sample.start(samplePotentio);
Bram122 5:e45681989201 392
Bram122 10:65c046928e5d 393 /*Thread thread_control;
Bram122 10:65c046928e5d 394 thread_control.start(control);*/
Bram122 5:e45681989201 395
Bram122 5:e45681989201 396 Thread thread_motor(osPriorityRealtime);
Bram122 5:e45681989201 397 thread_motor.start(motorControl);
Bram122 5:e45681989201 398
Bram122 5:e45681989201 399 Thread thread_display(osPriorityRealtime);
Bram122 5:e45681989201 400 thread_display.start(displayNumber7Seg);
Bram122 4:325954591d0f 401
Onon 7:1c712013123b 402 Thread thread_rfid;
Onon 7:1c712013123b 403 thread_rfid.start(checkRFID);
Onon 2:c719944e080b 404 // Set debug UART speed
Onon 3:80837b470692 405 //pc.baud(115200);
Onon 3:80837b470692 406
Onon 3:80837b470692 407 dg0 = 0;
Onon 3:80837b470692 408 dg1 = 0;
Onon 3:80837b470692 409 dg2 = 0;
Onon 3:80837b470692 410 dg3 = 0;
Onon 2:c719944e080b 411
Bram122 5:e45681989201 412 while (true) {
Onon 3:80837b470692 413
Onon 2:c719944e080b 414 // Look for new cards
Onon 3:80837b470692 415 /*if ( ! RfChip.PICC_IsNewCardPresent())
Onon 2:c719944e080b 416 {
Onon 2:c719944e080b 417 wait_ms(500);
Onon 2:c719944e080b 418 continue;
Onon 2:c719944e080b 419 }
Onon 2:c719944e080b 420
Onon 2:c719944e080b 421 // Select one of the cards
Onon 2:c719944e080b 422 if ( ! RfChip.PICC_ReadCardSerial())
Onon 2:c719944e080b 423 {
Onon 2:c719944e080b 424 wait_ms(500);
Onon 2:c719944e080b 425 continue;
Onon 2:c719944e080b 426 }
Onon 2:c719944e080b 427
Onon 2:c719944e080b 428 // Print Card UID
Onon 2:c719944e080b 429 pc.printf("Card UID: ");
Onon 2:c719944e080b 430 for (uint8_t i = 0; i < RfChip.uid.size; i++)
Onon 2:c719944e080b 431 {
Onon 2:c719944e080b 432 led = 1;
Onon 2:c719944e080b 433 pc.printf(" %X02", RfChip.uid.uidByte[i]);
Onon 2:c719944e080b 434 }
Onon 2:c719944e080b 435 pc.printf("\n\r");
Onon 2:c719944e080b 436
Onon 2:c719944e080b 437 // Print Card type
Onon 2:c719944e080b 438 uint8_t piccType = RfChip.PICC_GetType(RfChip.uid.sak);
Onon 2:c719944e080b 439 pc.printf("PICC Type: %s \n\r", RfChip.PICC_GetTypeName(piccType));
Onon 2:c719944e080b 440 wait_ms(1000);
Onon 3:80837b470692 441 }*/
Onon 3:80837b470692 442 }
Onon 2:c719944e080b 443 }
Onon 3:80837b470692 444
Onon 3:80837b470692 445
Onon 3:80837b470692 446
Onon 3:80837b470692 447