z ysaito / Mbed 2 deprecated Pro_B2_4ZNR_B2DS_ResolverCtrl_824MAX

2018.07.26

Dependencies:   QEI mbed-rtos mbed

4_main.cpp@3:85eb7e954bfa, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:21:04 2018 +0000
Revision:
3:85eb7e954bfa
Parent:
2:c62dc496b79a 
2018.07.26

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:b1136bf48c38 1 /**********************************************************
sayzyas 0:b1136bf48c38 2 * Project: B2 (1F-1)
sayzyas 0:b1136bf48c38 3 * Title: CrExp B2 Motor Ctrl Main
sayzyas 0:b1136bf48c38 4 * Target: LPCXpresso824-Max
sayzyas 0:b1136bf48c38 5 * Author: zinsor
sayzyas 0:b1136bf48c38 6 * Date(Latest update) 2015.12.21(Mon)
sayzyas 0:b1136bf48c38 7 * --------------------------------------------------------
sayzyas 0:b1136bf48c38 8 * Article
sayzyas 3:85eb7e954bfa 9 * Notification: Moving and Fix Winch rotation direction
sayzyas 3:85eb7e954bfa 10 * is opposit.
sayzyas 0:b1136bf48c38 11 *
sayzyas 0:b1136bf48c38 12 * --------------------------------------------------------
sayzyas 0:b1136bf48c38 13 *
sayzyas 0:b1136bf48c38 14 * LPCXpresso 824-MAX
sayzyas 0:b1136bf48c38 15 * +---------USB---------+
sayzyas 0:b1136bf48c38 16 * | |
sayzyas 0:b1136bf48c38 17 * | |
sayzyas 0:b1136bf48c38 18 * | |
sayzyas 0:b1136bf48c38 19 * | |
sayzyas 0:b1136bf48c38 20 * | | SCL P0_10 D15 -> IIC SCL
sayzyas 0:b1136bf48c38 21 * | ## ### ## | SDA P0_11 D14 -> IIC SDA
sayzyas 0:b1136bf48c38 22 * | # # # # # # | AVDD
sayzyas 0:b1136bf48c38 23 * N/A | # # # # # | GND
sayzyas 0:b1136bf48c38 24 * +3V3 | ## # # # | SCK P0_24 D13
sayzyas 0:b1136bf48c38 25 * NRST | # # # # # | MISO P0_25 D12
sayzyas 0:b1136bf48c38 26 * +3V3 | # # # # ###### | MOSI P0_26 D11 -> Resolver B
sayzyas 0:b1136bf48c38 27 * +5V | ## ##### # | SSEL P0_15 D10 -> Resolver A
sayzyas 0:b1136bf48c38 28 * GND | | P0_27 D9
sayzyas 0:b1136bf48c38 29 * GND | | P0_13 D8
sayzyas 0:b1136bf48c38 30 * N/A | | P0_17 D7
sayzyas 0:b1136bf48c38 31 * | | P0_16 D6
sayzyas 0:b1136bf48c38 32 * A0 P0_6 | | PWM P0_28 D5
sayzyas 0:b1136bf48c38 33 * A1 P0_14 | | PWM P0_18 D4
sayzyas 0:b1136bf48c38 34 * A2 P0_23 | | PWM P0_12 D3
sayzyas 0:b1136bf48c38 35 * A3 P0_22 | | PWM P0_19 D2
sayzyas 0:b1136bf48c38 36 * A4 P0_21 | | TX P0_4 D1
sayzyas 0:b1136bf48c38 37 * A5 P0_20 | | RX P0_0 D0
sayzyas 0:b1136bf48c38 38 * +---------------------+
sayzyas 0:b1136bf48c38 39 *
sayzyas 0:b1136bf48c38 40 ***************************************/
sayzyas 0:b1136bf48c38 41
sayzyas 0:b1136bf48c38 42 #include "mbed.h"
sayzyas 0:b1136bf48c38 43 #include "rtos.h"
sayzyas 0:b1136bf48c38 44 #include "QEI.h"
sayzyas 0:b1136bf48c38 45 #include "common.h"
sayzyas 0:b1136bf48c38 46 #include <math.h>
sayzyas 0:b1136bf48c38 47
sayzyas 0:b1136bf48c38 48 // Hardware setting
sayzyas 0:b1136bf48c38 49
sayzyas 0:b1136bf48c38 50 Serial pc(USBTX, USBRX); // tx, rx
sayzyas 0:b1136bf48c38 51
sayzyas 3:85eb7e954bfa 52 #ifdef FFWinchPhaseSetting // For Fix Fallong Winch
sayzyas 3:85eb7e954bfa 53 QEI wheel( P0_26, P0_15, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
sayzyas 3:85eb7e954bfa 54 #else //For Falling Position Moving Winch
sayzyas 0:b1136bf48c38 55 QEI wheel( P0_15, P0_26, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
sayzyas 3:85eb7e954bfa 56 #endif // FFWinchPhaseSetting
sayzyas 0:b1136bf48c38 57 /*
sayzyas 0:b1136bf48c38 58 Dram r = 60mm
sayzyas 0:b1136bf48c38 59 Cable fai = 3
sayzyas 0:b1136bf48c38 60
sayzyas 0:b1136bf48c38 61 (60+3)*3.14 = 197.82mm ==> 2^12*4 = 4096 * 4 = 16384 pulse
sayzyas 0:b1136bf48c38 62 1(mm) = 82.82(pulse)
sayzyas 0:b1136bf48c38 63 0.01208(mm) = 1(pulse)
sayzyas 0:b1136bf48c38 64
sayzyas 0:b1136bf48c38 65 */
sayzyas 0:b1136bf48c38 66 DigitalOut led1(LED1); // Red
sayzyas 0:b1136bf48c38 67 DigitalOut led2(LED2); // Green
sayzyas 0:b1136bf48c38 68 DigitalOut led3(LED3); // Blue
sayzyas 0:b1136bf48c38 69
sayzyas 0:b1136bf48c38 70 I2CSlave slave(P0_11, P0_10); //I2C SDA SCL
sayzyas 0:b1136bf48c38 71
sayzyas 0:b1136bf48c38 72 int i2c_saddress = 0;
sayzyas 0:b1136bf48c38 73
sayzyas 0:b1136bf48c38 74 void led_demo(){
sayzyas 0:b1136bf48c38 75 int i;
sayzyas 0:b1136bf48c38 76 for( i = 0; i < 20; i++ ) {
sayzyas 0:b1136bf48c38 77 led1 = LED_ON;
sayzyas 0:b1136bf48c38 78 led2 = LED_OFF;
sayzyas 0:b1136bf48c38 79 led3 = LED_OFF;
sayzyas 0:b1136bf48c38 80 wait_ms(20);
sayzyas 0:b1136bf48c38 81 led1 = LED_OFF;
sayzyas 0:b1136bf48c38 82 led2 = LED_OFF;
sayzyas 0:b1136bf48c38 83 led3 = LED_OFF;
sayzyas 0:b1136bf48c38 84 wait_ms(20);
sayzyas 0:b1136bf48c38 85 led1 = LED_OFF;
sayzyas 0:b1136bf48c38 86 led2 = LED_ON;
sayzyas 0:b1136bf48c38 87 led3 = LED_OFF;
sayzyas 0:b1136bf48c38 88 wait_ms(20);
sayzyas 0:b1136bf48c38 89 led1 = LED_OFF;
sayzyas 0:b1136bf48c38 90 led2 = LED_OFF;
sayzyas 0:b1136bf48c38 91 led3 = LED_OFF;
sayzyas 0:b1136bf48c38 92 wait_ms(20);
sayzyas 0:b1136bf48c38 93 led1 = LED_OFF;
sayzyas 0:b1136bf48c38 94 led2 = LED_OFF;
sayzyas 0:b1136bf48c38 95 led3 = LED_ON;
sayzyas 0:b1136bf48c38 96 wait_ms(20);
sayzyas 0:b1136bf48c38 97 led1 = LED_OFF;
sayzyas 0:b1136bf48c38 98 led2 = LED_OFF;
sayzyas 0:b1136bf48c38 99 led3 = LED_OFF;
sayzyas 0:b1136bf48c38 100 wait_ms(20);
sayzyas 0:b1136bf48c38 101 }
sayzyas 0:b1136bf48c38 102 // wait_ms(3000);
sayzyas 0:b1136bf48c38 103 }
sayzyas 0:b1136bf48c38 104
sayzyas 0:b1136bf48c38 105 // ========================================================================
sayzyas 0:b1136bf48c38 106 // Thread: Resolver Control Task
sayzyas 0:b1136bf48c38 107 // ========================================================================
sayzyas 0:b1136bf48c38 108 void ResolverCtrl_task(void const *) {
sayzyas 0:b1136bf48c38 109
sayzyas 0:b1136bf48c38 110
sayzyas 0:b1136bf48c38 111 }
sayzyas 0:b1136bf48c38 112
sayzyas 0:b1136bf48c38 113 // ========================================================================
sayzyas 0:b1136bf48c38 114 // Thread: Main Controller Interface Task
sayzyas 0:b1136bf48c38 115 // Get Command from Main controller and do the task.
sayzyas 0:b1136bf48c38 116 // ========================================================================
sayzyas 0:b1136bf48c38 117 void MainCtrl_Interface_task(void const *) {
sayzyas 0:b1136bf48c38 118
sayzyas 2:c62dc496b79a 119 char buf[14]; // command buffer
sayzyas 0:b1136bf48c38 120 char msg[] = "C000000";
sayzyas 0:b1136bf48c38 121 char res_msg[] = "Slave!";
sayzyas 0:b1136bf48c38 122 char res_msg2[] = "Bravo!";
sayzyas 0:b1136bf48c38 123
sayzyas 0:b1136bf48c38 124 int flg_readCurrent = 0;
sayzyas 0:b1136bf48c38 125
sayzyas 3:85eb7e954bfa 126 int pulse;
sayzyas 3:85eb7e954bfa 127 int16_t distance;
sayzyas 3:85eb7e954bfa 128 int16_t position_offset = 0;;
sayzyas 0:b1136bf48c38 129
sayzyas 0:b1136bf48c38 130 uint16_t dram_diameter;
sayzyas 0:b1136bf48c38 131 uint16_t ccable_diameter;
sayzyas 0:b1136bf48c38 132 uint8_t rresolution;
sayzyas 0:b1136bf48c38 133
sayzyas 0:b1136bf48c38 134 int tmp;
sayzyas 0:b1136bf48c38 135
sayzyas 0:b1136bf48c38 136 slave.address(i2c_saddress);
sayzyas 0:b1136bf48c38 137
sayzyas 3:85eb7e954bfa 138 DEBUG_PRINT_L1("*** Start Resolver thread ***\r\n");
sayzyas 0:b1136bf48c38 139
sayzyas 0:b1136bf48c38 140 /*
sayzyas 0:b1136bf48c38 141 * I2C Access
sayzyas 0:b1136bf48c38 142 */
sayzyas 0:b1136bf48c38 143 while(1) {
sayzyas 0:b1136bf48c38 144 // DEBUG_PRINT_L1(".");
sayzyas 0:b1136bf48c38 145 int i = slave.receive();
sayzyas 0:b1136bf48c38 146 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:b1136bf48c38 147 // DEBUG_PRINT_L1("?");
sayzyas 0:b1136bf48c38 148 // DEBUG_PRINT_L1("Slave Received\r\n");
sayzyas 0:b1136bf48c38 149 switch (i) {
sayzyas 0:b1136bf48c38 150 case I2CSlave::NoData:
sayzyas 0:b1136bf48c38 151 // DEBUG_PRINT_L1("the slave has not been addressed\r\n");
sayzyas 0:b1136bf48c38 152 // read_motor_current(2);
sayzyas 0:b1136bf48c38 153 break;
sayzyas 3:85eb7e954bfa 154 //
sayzyas 3:85eb7e954bfa 155 // Slave(this) <== Master
sayzyas 3:85eb7e954bfa 156 //
sayzyas 0:b1136bf48c38 157 case I2CSlave::ReadAddressed:
sayzyas 0:b1136bf48c38 158 /* Master is reading winch current position here */
sayzyas 0:b1136bf48c38 159 // DEBUG_PRINT_L1("the master has requested a read from this slave\r\n");
sayzyas 0:b1136bf48c38 160 pulse = wheel.getPulses();
sayzyas 0:b1136bf48c38 161
sayzyas 3:85eb7e954bfa 162 distance = (int16_t)( pulse * ( (double)( (((double)dram_diameter+(double)ccable_diameter)/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
sayzyas 3:85eb7e954bfa 163 // distance = (int16_t)((double)distance * (double)0.99776);
sayzyas 3:85eb7e954bfa 164 distance = (int16_t)((double)distance * (double)0.9986);
sayzyas 3:85eb7e954bfa 165 // distance = (int16_t)((double)distance * (double)0.999);
sayzyas 3:85eb7e954bfa 166 // distance = (int16_t)((double)distance * (double)1.0);
sayzyas 3:85eb7e954bfa 167 // -------------------------------
sayzyas 3:85eb7e954bfa 168 distance += position_offset; // 2016.11.17
sayzyas 3:85eb7e954bfa 169 // -------------------------------
sayzyas 3:85eb7e954bfa 170 // distance = (int16_t)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATIO
sayzyas 3:85eb7e954bfa 171
sayzyas 0:b1136bf48c38 172 DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
sayzyas 3:85eb7e954bfa 173 DEBUG_PRINT_L1("DISTANCE(OFFSET): %04d(%4d) mm", distance, position_offset);
sayzyas 3:85eb7e954bfa 174 /*
sayzyas 3:85eb7e954bfa 175 2016.11.09
sayzyas 3:85eb7e954bfa 176 Add dummy data to read winch position value.
sayzyas 3:85eb7e954bfa 177 byte[0]: Dummy data = 0x12 <--- New added !
sayzyas 3:85eb7e954bfa 178 byte[1]: Winch position upper byte
sayzyas 3:85eb7e954bfa 179 byte[2]: Winch position lower byte
sayzyas 3:85eb7e954bfa 180 byte[3]: Dummy data = 0x34 <--- New added !
sayzyas 3:85eb7e954bfa 181 */
sayzyas 3:85eb7e954bfa 182 res_msg2[0] = 0x12; // Dummy data
sayzyas 3:85eb7e954bfa 183 res_msg2[1] = distance & 0xFF;
sayzyas 3:85eb7e954bfa 184 res_msg2[2] = (distance >> 8)&0xFF;
sayzyas 3:85eb7e954bfa 185 res_msg2[3] = 0x34; // Dummy data
sayzyas 3:85eb7e954bfa 186 slave.write(res_msg2, 4); // Includes null char
sayzyas 3:85eb7e954bfa 187 tmp = (res_msg2[2] < 8)&0xFF00 | res_msg2[1]&0xFF;
sayzyas 3:85eb7e954bfa 188 DEBUG_PRINT_L1("\tSend data : %04d(%02x,%02x)\r\n", tmp, res_msg2[1], res_msg2[0]);
sayzyas 0:b1136bf48c38 189 break;
sayzyas 0:b1136bf48c38 190 case I2CSlave::WriteGeneral:
sayzyas 0:b1136bf48c38 191 DEBUG_PRINT_L1("Bd4> the master is writing to all slave\r\n");
sayzyas 0:b1136bf48c38 192 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:b1136bf48c38 193 DEBUG_PRINT_L1("Bd4> Read G: %s\r\n", buf);
sayzyas 0:b1136bf48c38 194 break;
sayzyas 3:85eb7e954bfa 195 //
sayzyas 3:85eb7e954bfa 196 // Master ==> Slave(this)
sayzyas 3:85eb7e954bfa 197 //
sayzyas 0:b1136bf48c38 198 case I2CSlave::WriteAddressed:
sayzyas 0:b1136bf48c38 199 /* ******************************************* */
sayzyas 0:b1136bf48c38 200 /* Add setting data set operation in this area */
sayzyas 0:b1136bf48c38 201 /* ******************************************* */
sayzyas 2:c62dc496b79a 202 if( buf[I2C_CP_COMMAND_R] == 'Z'){
sayzyas 0:b1136bf48c38 203 wheel.reset();
sayzyas 3:85eb7e954bfa 204 position_offset = 0;
sayzyas 0:b1136bf48c38 205 }
sayzyas 3:85eb7e954bfa 206 // New command 2016.11.17 for customer request
sayzyas 3:85eb7e954bfa 207 else if( buf[I2C_CP_COMMAND_R] == 'S'){
sayzyas 3:85eb7e954bfa 208 wheel.reset();
sayzyas 3:85eb7e954bfa 209 position_offset = 0;
sayzyas 2:c62dc496b79a 210 for( int j = 0; j < NumberOfI2CCommand; j++)
sayzyas 2:c62dc496b79a 211 pc,printf("%02x ", buf[j]);
sayzyas 2:c62dc496b79a 212 pc.printf( "\r\n" );
sayzyas 3:85eb7e954bfa 213 position_offset = ( buf[I2C_CP_PRESET_CPOS_UPPER] << 8 );
sayzyas 3:85eb7e954bfa 214 position_offset |= buf[I2C_CP_PRESET_CPOS_LOWER];
sayzyas 3:85eb7e954bfa 215 pc.printf("POSITION OFFSET = %d\r\n", position_offset);
sayzyas 3:85eb7e954bfa 216 }
sayzyas 3:85eb7e954bfa 217 else if( buf[I2C_CP_COMMAND_R] == 'R'){
sayzyas 3:85eb7e954bfa 218 for( int j = 0; j < NumberOfI2CCommand; j++)
sayzyas 3:85eb7e954bfa 219 pc.printf("%02x ", buf[j]);
sayzyas 3:85eb7e954bfa 220 pc.printf( "\r\n" );
sayzyas 2:c62dc496b79a 221
sayzyas 2:c62dc496b79a 222 dram_diameter = ( buf[I2C_CP_WDRAM_DIA_UPPER] << 8 );
sayzyas 2:c62dc496b79a 223 dram_diameter |= buf[I2C_CP_WDRAM_DIA_LOWER];
sayzyas 2:c62dc496b79a 224 ccable_diameter = ( buf[I2C_CP_CCABLE_DIA_UPPER] << 8 );
sayzyas 2:c62dc496b79a 225 ccable_diameter |= buf[I2C_CP_CCABLE_DIA_LOWER];
sayzyas 2:c62dc496b79a 226 rresolution = buf[I2C_CP_RESOLVER_RESO];
sayzyas 0:b1136bf48c38 227
sayzyas 3:85eb7e954bfa 228 DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
sayzyas 3:85eb7e954bfa 229 DEBUG_PRINT_L1("Bd4> Dram Diameter : %d\t(mm)\r\n", dram_diameter);
sayzyas 3:85eb7e954bfa 230 DEBUG_PRINT_L1("Bd4> CCable Diameter : %d\t(mm)\r\n", ccable_diameter);
sayzyas 3:85eb7e954bfa 231 DEBUG_PRINT_L1("Bd4> Resolver Resolution : %d\t(bit)\r\n", rresolution);
sayzyas 3:85eb7e954bfa 232 DEBUG_PRINT_L1("Bd4> -------------------------------------------\r\n", rresolution);
sayzyas 3:85eb7e954bfa 233 DEBUG_PRINT_L1("Bd4> Real Diameter : %d\t(mm)\r\n", (dram_diameter+ccable_diameter));
sayzyas 3:85eb7e954bfa 234 DEBUG_PRINT_L1("Bd4> Rotation Pulse / 1round : %d\t(pulse)\r\n", (int)(pow(2.0, (double)rresolution)*4));
sayzyas 3:85eb7e954bfa 235 DEBUG_PRINT_L1("Bd4> Distance / 1pulse : %lf\t(mm)\r\n", (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) );
sayzyas 3:85eb7e954bfa 236 DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
sayzyas 0:b1136bf48c38 237
sayzyas 0:b1136bf48c38 238 pulse = wheel.getPulses();
sayzyas 0:b1136bf48c38 239 DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
sayzyas 0:b1136bf48c38 240 // DEBUG_PRINT_L1("Bd4> >DISTANCE: %04d (mm)\n", (int)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND )));
sayzyas 3:85eb7e954bfa 241 DEBUG_PRINT_L1("DISTANCE: %04d (mm)\r\n", (int)( pulse * ( (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) ) );
sayzyas 3:85eb7e954bfa 242 DEBUG_PRINT_L1("Bd4> ---------------------------------------\r\n");
sayzyas 0:b1136bf48c38 243 }
sayzyas 0:b1136bf48c38 244 break;
sayzyas 0:b1136bf48c38 245 }
sayzyas 0:b1136bf48c38 246 Thread::wait(1);
sayzyas 0:b1136bf48c38 247 }
sayzyas 0:b1136bf48c38 248 }
sayzyas 0:b1136bf48c38 249 // ========================================================================
sayzyas 0:b1136bf48c38 250 // Main Function
sayzyas 0:b1136bf48c38 251 // ========================================================================
sayzyas 0:b1136bf48c38 252 int main() {
sayzyas 0:b1136bf48c38 253 int32_t counter = 0;
sayzyas 0:b1136bf48c38 254
sayzyas 0:b1136bf48c38 255 pc.baud(115200);
sayzyas 0:b1136bf48c38 256
sayzyas 0:b1136bf48c38 257 i2c_saddress = Target_IIC_ADDR; // defined at header
sayzyas 0:b1136bf48c38 258
sayzyas 0:b1136bf48c38 259 DEBUG_PRINT_L0("> Board type: Resolver Counter controller(0x%02x)\r\n",i2c_saddress );
sayzyas 0:b1136bf48c38 260
sayzyas 1:86705c6e0ab7 261 i2c_saddress = I2C_ADDRESS_RESOLVER;
sayzyas 0:b1136bf48c38 262
sayzyas 0:b1136bf48c38 263 led_demo();
sayzyas 0:b1136bf48c38 264
sayzyas 0:b1136bf48c38 265 DEBUG_PRINT_L0("\r\n");
sayzyas 0:b1136bf48c38 266 DEBUG_PRINT_L0("Bd4> +--------------------------------------\r\n");
sayzyas 0:b1136bf48c38 267 DEBUG_PRINT_L0("Bd4> | B2 CrExp Resolver Pulse Counter Main \r\n");
sayzyas 0:b1136bf48c38 268 DEBUG_PRINT_L0("Bd4> | Letest update: %s\r\n", LatestUpDate);
sayzyas 0:b1136bf48c38 269 DEBUG_PRINT_L0("Bd4> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 0:b1136bf48c38 270 DEBUG_PRINT_L0("Bd4> | Author: %s\r\n", Author);
sayzyas 0:b1136bf48c38 271 DEBUG_PRINT_L0("Bd4> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
sayzyas 0:b1136bf48c38 272 DEBUG_PRINT_L0("Bd4> +--------------------------------------\r\n");
sayzyas 0:b1136bf48c38 273
sayzyas 0:b1136bf48c38 274 Thread resolverControlTask1(MainCtrl_Interface_task, NULL, osPriorityNormal, 128 * 4);
sayzyas 0:b1136bf48c38 275 wheel.reset();
sayzyas 0:b1136bf48c38 276
sayzyas 0:b1136bf48c38 277 while(1){
sayzyas 0:b1136bf48c38 278
sayzyas 0:b1136bf48c38 279 counter++;
sayzyas 0:b1136bf48c38 280 if( counter >= 50 ){
sayzyas 0:b1136bf48c38 281 led3 = !led3; // Blue
sayzyas 0:b1136bf48c38 282 counter = 0;
sayzyas 0:b1136bf48c38 283 }
sayzyas 0:b1136bf48c38 284 Thread::wait(5);
sayzyas 0:b1136bf48c38 285 }
sayzyas 0:b1136bf48c38 286 }