File content as of revision 1:368ba89c2e6b:

#include "mbed.h"
#include "USBHID.h"
#include "USBSerial.h"
#include "rtos.h"
#include "common.h"
#include "TextLCD.h"

//#include "TextLCD.h"
 
//We declare a USBHID device. By default input and output reports are 64 bytes long.
USBHID hid(8, 8);

//USBSerial serial;
Serial pc(USBTX, USBRX); // UART

// I2C i2c(p9, p10);      // I2C SDA, SCL
I2CSlave slave(p9, p10); //I2C SDA SCL
 
//This report will contain data to be sent
HID_REPORT send_report;
HID_REPORT recv_report;
 
DigitalOut led1(LED1); // 1: on, 0: off
DigitalOut led2(LED2); // 1: on, 0: off
DigitalOut led3(LED3); // 1: on, 0: off
DigitalOut led4(LED4); // 1: on, 0: off

// LED indicator
DigitalOut led_ind0(p13);
DigitalOut led_ind1(p14);
DigitalOut led_ind2(p15);
DigitalOut led_ind3(p25);
DigitalOut led_ind4(p26);
DigitalOut led_ind5(p27);
DigitalOut led_ind6(p28);
DigitalOut led_ind7(p8);

// Switch Matrix 3bit SW ON = 0 input
//  bit 2 1 0   Send data 
//   0  1 1 1 : 
//   1  1 1 0 : 0x01 RF Tfm K
//   2  1 0 1 : 0x02 RF Tfm I
//   3  1 0 0 : 0x04 LB Tfm K
//   4  0 1 1 : 0x08 LB Tfm I
//   5  0 1 0 : 0x10 Winch Down
//   6  0 0 1 : 0x20 Winch Up
//   7  0 0 0 : 
DigitalIn sw1(p5); // Bit 0
DigitalIn sw2(p6); // Bit 1
DigitalIn sw3(p7); // Bit 2

// Digital Input 1:OFF, 0:ON
DigitalIn sw_jsmode(p30);        // SW: Joy Stick mode ( 1: Single, 0: Dual )
DigitalIn sw_crexpshape(p29);   // SW: CrExp Shape ( 1: KO, 0: I )

// Analig Joystick input for crawler control
//AnalogIn js_l_ud(p20);  // Crawler L-UD
//AnalogIn js_l_lr(p19);  // Crawler L-LR
//AnalogIn js_r_ud(p17);  // Crawler R-UD
//AnalogIn js_r_lr(p16);  // Crawler R-LR

AnalogIn js_l_ud(p19);  // Crawler L-UD
AnalogIn js_l_lr(p20);  // Crawler L-LR
AnalogIn js_r_ud(p16);  // Crawler R-UD
AnalogIn js_r_lr(p17);  // Crawler R-LR

uint8_t js_center_value_r_ud = 0x80;
uint8_t js_center_value_r_lr = 0x80;
uint8_t js_center_value_l_ud = 0x80;
uint8_t js_center_value_l_lr = 0x80;

void led_demo(void);
uint8_t calc_motor_speed( uint8_t data, int32_t dir );
void lcd_out( int, int, char* );
void calibrate_joystick( int mode, uint8_t data_ud, uint8_t data_lr );

TextLCD lcd(p11, p12, p24, p23, p22, p21); // rs, e, d4-d7

int i2c_saddress = 0;

// LED demo
void led_demo()
{
    int i;
    for( i = 0; i < 7; i++ ) {
        led_ind0 = 1;
        led_ind1 = 1;
        led_ind2 = 1;
        led_ind3 = 1;
        led_ind4 = 1;
        led_ind5 = 1;
        led_ind6 = 1;
        led_ind7 = 1;
        led1 = 1;   // on
        led2 = 0;   // off
        led3 = 0;   // off
        led4 = 0;   // off
        Thread::wait(50);
        led_ind0 = 0;
        led_ind1 = 0;
        led_ind2 = 0;
        led_ind3 = 0;
        led_ind4 = 0;
        led_ind5 = 0;
        led_ind6 = 0;
        led_ind7 = 0;
        led1 = 0;   // off
        led2 = 1;   // on
        led3 = 0;   // off
        led4 = 0;   // off
        Thread::wait(50);
        led_ind0 = 1;
        led_ind1 = 1;
        led_ind2 = 1;
        led_ind3 = 1;
        led_ind4 = 1;
        led_ind5 = 1;
        led_ind6 = 1;
        led_ind7 = 1;
        led1 = 0;   // off
        led2 = 0;   // off
        led3 = 1;   // on
        led4 = 0;   // off
        Thread::wait(50);
        led_ind0 = 0;
        led_ind1 = 0;
        led_ind2 = 0;
        led_ind3 = 0;
        led_ind4 = 0;
        led_ind5 = 0;
        led_ind6 = 0;
        led_ind7 = 0;
        led1 = 0;   // off
        led2 = 0;   // off
        led3 = 0;   // off
        led4 = 1;   // on
        Thread::wait(50);
        led_ind0 = 1;
        led_ind1 = 1;
        led_ind2 = 1;
        led_ind3 = 1;
        led_ind4 = 1;
        led_ind5 = 1;
        led_ind6 = 1;
        led_ind7 = 1;
        led1 = 0;   // off
        led2 = 0;   // off
        led3 = 0;   // off
        led4 = 0;   // off
    }
}

// *****************************************************************
//  calibration joystick data
// *****************************************************************
void calibrate_joystick( int mode, uint8_t data_ud, uint8_t data_lr ){
    uint16_t tmp;

    if( mode == 0 )
    {
        tmp = (uint16_t)js_center_value_r_ud;
        tmp += (uint16_t)data_ud;
        tmp /= 2;
        js_center_value_r_ud = (uint8_t)tmp;
        
        tmp = (uint16_t)js_center_value_r_lr;
        tmp += (uint16_t)data_lr;
        tmp /= 2;
        js_center_value_r_lr = (uint8_t)tmp;
    }
    else
    {
        
        tmp = (uint16_t)js_center_value_l_ud;
        tmp += (uint16_t)data_ud;
        tmp /= 2;
        js_center_value_l_ud = (uint8_t)tmp;
        
        tmp = (uint16_t)js_center_value_l_lr;
        tmp += (uint16_t)data_lr;
        tmp /= 2;
        js_center_value_l_lr = (uint8_t)tmp;
    }
}

// *****************************************************************
//  calcuilation motor speed
// *****************************************************************
uint8_t calc_motor_speed( uint8_t data, int32_t dir ){
    uint8_t speed;
    if( dir == 0 ){
//        speed = (data - js_center_value) * 100 / js_center_value;
        if( speed > 95 ) speed = 100;
    }
    else{
//        speed = (js_center_value - data) * 100 / js_center_value;
        if( speed > 95 ) speed = 100;
    }
    return speed;
}

// *****************************************************************
//  switch control task
// ----------------------
//
//    Up:    center -> 255
//    Down:  002 -> center
//    Right: center -> 255
//    Left : 0 -> center 
//
// *****************************************************************
void sw_task( void const *){

    char msg[32];
    
    uint16_t js_rud_data;
    uint16_t js_rlr_data;
    uint8_t js_rud_undata;
    uint8_t js_rlr_undata;
    
    uint16_t js_lud_data;
    uint16_t js_llr_data;
    uint8_t js_lud_undata;
    uint8_t js_llr_undata;

    float motor_speed_f = 0;
    uint8_t motor_speed = 0;
    
    send_report.length = 8;
        
    send_report.data[0] = 0x80; // L JoyStick Left/Right
    send_report.data[1] = 0x80; // L JoyStick Up/Down
    send_report.data[2] = 0x80; // R JoyStick Left/Right^
    send_report.data[3] = 0x80; // R JoyStick Up/Down
    send_report.data[4] = 0x00; // Operatiuon Switch
    // Data Bytes
    //  7 6 5 4   3    2    1    0
    // +-+-+-+-+-+----+----+----+----+
    // |-|-|-|-|-|LB-I|LB-K|RF-I|RF-K|
    // +-+-+-+-+-+----+----+----+----+
        
    send_report.data[5] = 0x00; // JS mode switch 
    // Command Bit
    //  7 6 5 4 3 2  1   0
    // +-+-+-+-+-+-+---+----+
    // |-|-|-|-|-|-|S/D|I/KO|
    // +-+-+-+-+-+-+---+----+
    //              1:S 1:KO  <--- sw off
    //              0:D 0:I   <--- sw on


   
    send_report.data[6] = 0x00; // No Function
    send_report.data[7] = 0x00; // No Function
    send_report.data[8] = 0x00;
    
    DEBUG_PRINT_L1("Calibrating joystick ... ");
    for( int i = 0; i < CALIBRATION_COUNT; i++){
        js_rud_data = js_r_ud.read_u16();
        js_rlr_data = js_r_lr.read_u16();
        js_rud_undata = (uint8_t)(js_rud_data >> 8);
        js_rlr_undata = (uint8_t)(js_rlr_data >> 8);
        calibrate_joystick( 0, js_rud_undata, js_rlr_undata);   // Under constraction
        js_lud_data = js_l_ud.read_u16();
        js_llr_data = js_l_lr.read_u16();
        js_lud_undata = (uint8_t)(js_lud_data >> 8);
        js_llr_undata = (uint8_t)(js_llr_data >> 8);
        calibrate_joystick( 1, js_lud_undata, js_llr_undata);   // Under constraction
    }
    DEBUG_PRINT_L1("done\r\n");
    
    while(1){
        js_rud_data = js_r_ud.read_u16();
        js_rlr_data = js_r_lr.read_u16();
        js_lud_data = js_l_ud.read_u16();
        js_llr_data = js_l_lr.read_u16();
        
        js_rud_undata = (uint8_t)(js_rud_data >> 8);
        js_rlr_undata = (uint8_t)(js_rlr_data >> 8);
        js_lud_undata = (uint8_t)(js_lud_data >> 8);
        js_llr_undata = (uint8_t)(js_llr_data >> 8);
        
        DEBUG_PRINT_L1("Handy> ");
        DEBUG_PRINT_L1("R:%03d/%3d,%03d/%3d  ", js_rud_undata, js_center_value_r_ud, js_rlr_undata, js_center_value_r_ud );
        DEBUG_PRINT_L1("L:%03d/%3d,%03d/%3d  ", js_lud_undata, js_center_value_l_ud, js_llr_undata, js_center_value_l_ud );
        DEBUG_PRINT_L1("SW: %d %d %d  ", sw1.read(), sw2.read(), sw3.read() );
        DEBUG_PRINT_L1("JS: %d %d\r\n", sw_jsmode.read(), sw_crexpshape.read() );
        
        // ---------------------------------------------
        // Right JoyStick Control
        // ---------------------------------------------
        // Down
        if( js_rud_undata > (js_center_value_r_ud + DEAD_ZONE_BAND_WIDTH) ){
            // Crawler Forward Run       
            led4 = 1;
            motor_speed = calc_motor_speed( js_rud_undata, 0 );
            DEBUG_PRINT_L2("Handy> R-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rud_undata, js_rud_undata, js_center_value_r_ud);
            send_report.data[3] = 255 - js_rud_undata; // speed
            //------------123456789 123456--------------------
        //    sprintf(msg, "R-JS Dn (%03d)  ", send_report.data[3]);
        //    lcd_out(0,0,msg);
            led4 = 0;
        }
        // Up
        else if( js_rud_undata < (js_center_value_r_ud - DEAD_ZONE_BAND_WIDTH) ){
            // Crawler Reverse Run       
            led4 = 1;
            motor_speed = calc_motor_speed( js_rud_undata, 1 );
            DEBUG_PRINT_L2("Handy> R-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rud_undata, js_rud_undata, js_center_value_r_ud);
            send_report.data[3] = 255 - js_rud_undata; // speed
            //-----------123456789 123456--------------------
        //    sprintf(msg,"R-JS Up (%03d)  ", send_report.data[3]);
        //    lcd_out(0,0,msg);
            led4 = 0;
        }           
        else{
            send_report.data[3] = 0x80; // speed
            //-----------123456789 123456--------------------
        //    sprintf(msg,"R-JS    (%03d)  ", send_report.data[3]);
        //    lcd_out(0,0,msg);
                    //    calibrate_joystick(js_ud_un);   // Under constraction
            led4 = 0;
        }

        // Right
        if( js_rlr_undata > (js_center_value_r_ud + DEAD_ZONE_BAND_WIDTH) ){
            // Crawler Forward Run       
            led4 = 1;
            motor_speed = calc_motor_speed( js_rlr_undata, 0 );
            DEBUG_PRINT_L2("Handy> R-JS Right: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rlr_undata, js_rlr_undata, js_center_value_r_ud);
            send_report.data[2] = js_rlr_undata; // speed
            //-----------123456789 123456--------------------
        //    sprintf(msg,"R-JS Rt (%03d)  ", send_report.data[2]);
        //    lcd_out(0,0,msg);
            led4 = 0;
        }
        
        // Left
        else if( js_rlr_undata < (js_center_value_r_ud - DEAD_ZONE_BAND_WIDTH) ){
            // Crawler Reverse Run       
            led4 = 1;
            motor_speed = calc_motor_speed( js_rlr_undata, 1 );
            DEBUG_PRINT_L2("Handy> R-JS Left: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rlr_undata, js_rlr_undata, js_center_value_r_ud);
            send_report.data[2] = js_rlr_undata; // speed
            //-----------123456789 123456--------------------
        //    sprintf(msg,"R-JS Lt (%03d)  ", send_report.data[2]);
        //    lcd_out(0,0,msg);
            led4 = 0;
        }

        else{
            send_report.data[2] = 0x80; // speed
        //    calibrate_joystick(js_ud_un);   // Under constraction
            led4 = 0;
        }
        
        // ---------------------------------------------
        // Left JoyStick Control
        // ---------------------------------------------
        // Down
        if( js_lud_undata > (js_center_value_l_ud + DEAD_ZONE_BAND_WIDTH) ){
            // Crawler Forward Run       
            led4 = 1;
            motor_speed = calc_motor_speed( js_lud_undata, 0 );
            DEBUG_PRINT_L2("Handy> L-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_lud_undata, js_lud_undata, js_center_value_l_ud);
            send_report.data[1] = 255 - js_lud_undata; // speed
            //-----------123456789 123456--------------------
        //    sprintf(msg,"L-JS Dn (%03d)  ", send_report.data[1]);
        //    lcd_out(0,1,msg);
            led4 = 0;
        }
        // Up
        else if( js_lud_undata < (js_center_value_l_ud - DEAD_ZONE_BAND_WIDTH) ){
            // Crawler Reverse Run       
            led4 = 1;
            motor_speed = calc_motor_speed( js_lud_undata, 1 );
            DEBUG_PRINT_L2("Handy> L-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_lud_undata, js_lud_undata, js_center_value_l_ud);
            send_report.data[1] = 255 - js_lud_undata; // speed
            //-----------123456789 123456--------------------
        //    sprintf(msg,"L-JS Up (%03d)  ", send_report.data[1]);
        //    lcd_out(0,1,msg);
            led4 = 0;
        }           
        else{
            send_report.data[1] = 0x80; // speed
            //-----------123456789 123456--------------------
        //    sprintf(msg,"L-JS    (%03d)  ", send_report.data[1]);
        //    lcd_out(0,1,msg);
            //    calibrate_joystick(js_l_ud_un);   // Under constraction
            led4 = 0;
        }

        // Right           
        if( js_llr_undata > (js_center_value_l_ud + DEAD_ZONE_BAND_WIDTH) ){
            // Crawler Forward Run       
            led4 = 1;
            motor_speed = calc_motor_speed( js_llr_undata, 0 );
            DEBUG_PRINT_L2("Handy> L-JS Right: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_llr_undata, js_llr_undata, js_center_value_l_ud);
            send_report.data[0] = js_llr_undata; // speed
            //-----------123456789 123456--------------------
        //    sprintf(msg,"L-JS Rt (%03d)  ", send_report.data[0]);
        //    lcd_out(0,1,msg);
            led4 = 0;
        }
        // Left
        else if( js_llr_undata < (js_center_value_l_ud - DEAD_ZONE_BAND_WIDTH) ){
            // Crawler Reverse Run       
            led4 = 1;
            motor_speed = calc_motor_speed( js_llr_undata, 1 );
            DEBUG_PRINT_L2("Handy> L-JS Left: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_llr_undata, js_llr_undata, js_center_value_l_ud);
            send_report.data[0] = js_llr_undata; // speed
            //-----------123456789 123456--------------------
        //    sprintf(msg,"L-JS Lt (%03d)  ", send_report.data[0]);
        //    lcd_out(0,1,msg);
            led4 = 0;
        }

        else{
            send_report.data[0] = 0x80; // speed
        //    calibrate_joystick(js_l_ud_un);   // Under constraction
            led4 = 0;
        }
        
        // ---------------------------------------------
        // Other Switch Control: Pass
        // ---------------------------------------------
        if ((sw1 == 0)&&(sw2 == 1)&&(sw3==1)){
            led3 = 1;
            send_report.data[4] = 0x01;     // RF Tfm K (Fwd)
            led_ind4 = 1;
            led_ind5 = 1;
            led_ind6 = 1;
        }
        else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==1)){
            send_report.data[4] = 0x02;     // RF Tfm I (Rvs)
            led3 = 1;
            led_ind4 = 1;
            led_ind5 = 1;
            led_ind6 = 1;
        }
        else if ((sw1 == 0)&&(sw2 == 0)&&(sw3==1)){  
            send_report.data[4] = 0x04;     // LB Tfm K (Fwd)
            led3 = 1;
            led_ind4 = 1;
            led_ind5 = 1;
            led_ind6 = 1;
        }
        else if ((sw1 == 1)&&(sw2 == 1)&&(sw3==0)){  
            led3 = 1;
            send_report.data[4] = 0x08;     // LB Tfm I (Rvs)
            led_ind4 = 1;
            led_ind5 = 1;
            led_ind6 = 1;
        }
        else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==0)){  
            led2 = 1;
            send_report.data[4] = 0x10;     // Winch Down (Fwd)
            led_ind0 = 1;
            led_ind1 = 1;
            led_ind2 = 1;
        }
        else if ((sw1 == 0)&&(sw2 == 1)&&(sw3==0)){  
            led2 = 1;
            send_report.data[4] = 0x20;     // Winch Up (Rvs) 
            led_ind0 = 1;
            led_ind1 = 1;
            led_ind2 = 1;
        }
        else{
            send_report.data[4] = 0;
            led_ind0 = 0;
            led_ind1 = 0;
            led_ind2 = 0;
            led_ind3 = 0;
            led_ind4 = 0;
            led_ind5 = 0;
            led_ind6 = 0;
            led_ind7 = 0;
            led2 = 0;            
            led3 = 0;
        }
        // Command Bit
        //  7 6 5 4 3 2  1   0
        // +-+-+-+-+-+-+---+----+
        // |-|-|-|-|-|-|S/D|I/KO|
        // +-+-+-+-+-+-+---+----+
        //              1:S 1:KO  <--- sw off
        //              0:D 0:I   <--- sw on
        if( sw_crexpshape == 1 ){ // sw off
            send_report.data[5] |= 0x01;    // SW off: K-Shape
        }
        else{ // sw off
            send_report.data[5] &= 0xFE;    // SW on: I-Shape
        }

        if( sw_jsmode == 1 ){ // sw off 
            send_report.data[5] |= 0x02;    // SW off: Single

        }
        else{ // sw off
            send_report.data[5] &= 0xFD;    // SW: on Dual
        }
        
        // *************************
        // Send Command to HID host: Hot Plug !!
        // *************************
        hid.send(&send_report); // send command to USB HID host
        
        Thread::wait(5);
        //lcd_out( 0, 1, "**********" );    // Handy Only
    }

}

void lcd_out( int column, int row, char* msg ){
    
//    lcd.cls();
    lcd.locate(column,row);

/*    
    for( int i = 0; i < 16; i++ ){
        lcd.putc(*msg++);
    }
*/    
    lcd.printf( msg );
    Thread::wait(10); 
}


// **********************************************************************
//
//    Main Function of this program
//
// **********************************************************************
int main(void) {
    
    int32_t counter = 0;
    
    char msg[16];
    char buf[11];
    int i;

    pc.baud(115200);

    sw_jsmode.mode( PullUp );    // use internal pullup
    sw_crexpshape.mode( PullUp );    // use internal pullup
 
    i2c_saddress = Target_IIC_ADDR;     // defined at header 
    i2c_saddress <<= 1;
    slave.address(i2c_saddress);
    DEBUG_PRINT_L0("Handy> IIC Address: %02x\r\n",i2c_saddress);

    lcd_out(0,0,"B2DebrisSurveyor"); 
    lcd_out(0,1,"Revast Co.,Ltd. "); 
    led_demo();
    lcd_out(0,0,"Rev1.00 20160218"); 
    lcd_out(0,1,"System Booting.."); 
    
    DEBUG_PRINT_L0("\r\n");
    DEBUG_PRINT_L0("Handy> +-----------------------------------------------------------\r\n");
    DEBUG_PRINT_L0("Handy> | Project: Crawler Explorer for 1F-1 PCV internal inspection\r\n");
    DEBUG_PRINT_L0("Handy> |---------\r\n");
    DEBUG_PRINT_L0("Handy> | This is: Handy Controller main program over USB-HID \r\n");
    DEBUG_PRINT_L0("Handy> |   Target MCU: %s\r\n", Targetmbed);
    DEBUG_PRINT_L0("Handy> |   Letest update: %s\r\n", LatestUpDate);
    DEBUG_PRINT_L0("Handy> |   Program Revision: %s\r\n", ProgramRevision);
    DEBUG_PRINT_L0("Handy> |   Author: %s\r\n", Author);
    DEBUG_PRINT_L0("Handy> |   Copyright(C) 2015 %s Allright Reserved\r\n", Company);
    DEBUG_PRINT_L0("Handy> +-----------------------------------------------------------\r\n");
   
    Thread task1(sw_task, NULL, osPriorityNormal, 256 * 4);

    //           1234567890123456 
    lcd_out(0,0,"Booted up !     "); 
 
    if(hid.readNB(&recv_report)) {
        for(int i = 1; i < recv_report.length; i++) {
            DEBUG_PRINT_L0("%d ", recv_report.data[i]);
        }
        DEBUG_PRINT_L0("\r\n");
    }
    lcd_out(0,0,"B2DebrisSurveyor"); 
     
    
    Thread::wait(5);
       
    while (1) {
       //try to read a msg

        if(hid.readNB(&recv_report)) {
            for(int i = 1; i < recv_report.length; i++) {
            //    DEBUG_PRINT("%d ", recv_report.data[i]);
            //    serial.printf("%d ", recv_report.data[i]);
            }
            // DEBUG_PRINT("\r\n");
            // serial.printf("\r\n");
            
        }

        i = slave.receive();
        slave.read(buf, NumberOfI2CCommand ); /* リードサイズは何故か10じゃないと変になる */
        switch (i) {
            case I2CSlave::NoData:
            //    DEBUG_PRINT_L0("Handy> the slave has not been addressed\r\n");
                lcd_out(0,1,"System OK[-]    ");
                break;
            case I2CSlave::ReadAddressed:
                DEBUG_PRINT_L0("Handy> the master has requested a read from this slave\r\n");
                slave.write(msg, strlen(msg) + 1); // Includes null char
                break;
             case I2CSlave::WriteGeneral:
                DEBUG_PRINT_L0("Handy> the master is writing to all slave\r\n"); 
                slave.read(buf, NumberOfI2CCommand);
                DEBUG_PRINT_L0("Handy> Read G: %s\r\n", buf);
                break;
             case I2CSlave::WriteAddressed:
                DEBUG_PRINT_L0("Handy> the master is writing to this slave\r\n");
                slave.read(buf, NumberOfI2CCommand);
             //   DEBUG_PRINT_L0("Handy> Read A: %s\r\n", buf);
                if( buf[5] == 0x01 ){
                    lcd_out(0,1,"System OK[|]    "); // command
                }
                else if( buf[5] == 0x04 ){
                    lcd_out(0,1,"ERROR           ");
                }
                else{
                    lcd_out(0,1,"System OK[/]    "); // unknown command
                }
                break;
        }
        for(int i = 0; i < 10; i++) buf[i] = 0;    // Clear buffer

        counter++;
        if( counter >= 2 ){
            led1 = !led1;
            /*
            led_ind0 = !led_ind0;
            led_ind1 = !led_ind1;
            led_ind2 = !led_ind2;
            led_ind3 = !led_ind3;
            led_ind4 = !led_ind4;
            led_ind5 = !led_ind5;
            led_ind6 = !led_ind6;
            led_ind7 = !led_ind7;
            */
            counter = 0;
        }
        Thread::wait(5);
    }
}