File content as of revision 2:d0a5ee4e7bb8:

#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_ikmode(p29);      // SW: i \ k ,pde ( 1: K, 0: I )
DigitalIn sw_validpart(p30);   // SW: CrExp Shape ( 1: Left-Winch. 0: Right-tfmcrwler )

// 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 lcd_out2( int column, int row, char* msg, int cnt);

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;

// 
void led_winch_valid( int counter )
{
    if( counter < 25)
    {
        led_ind0 = 0;
        led_ind1 = 0;
        led_ind2 = 0;
        led_ind3 = 0; 
    }
    else
    {
        led_ind0 = 1;
        led_ind1 = 1;
        led_ind2 = 1;
        led_ind3 = 1; 
    }
}

void led_crawler_valid( int counter )
{
    if( counter < 25)
    {
        led_ind4 = 0;
        led_ind5 = 0;
        led_ind6 = 0;
        led_ind7 = 0; 
    }
    else
    {
        led_ind4 = 1;
        led_ind5 = 1;
        led_ind6 = 1;
        led_ind7 = 1; 
    }
}

// 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(15);
        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(15);
        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(15);
        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(15);
        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[16];
    
    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++){
    for( int i = 0; i < 10; 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_ikmode.read(), sw_validpart.read() );
        
        // ---------------------------------------------
        // Right JoyStick Control
        // ---------------------------------------------
        // Down
    //    if( js_rud_undata > (js_center_value_r_ud + DEAD_ZONE_BAND_WIDTH) ){
        if( js_rud_undata > (js_center_value_r_ud + 5) ){
            // Crawler Forward Run       
            led4 = 1;
            motor_speed = calc_motor_speed( js_rud_undata, 0 );
            send_report.data[3] = 255 - js_rud_undata; // speed
            DEBUG_PRINT_L2("Handy> R-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rud_undata, send_report.data[3], js_center_value_r_ud);
            if ( sw_validpart == 0 ){
                //------------123 45 --------------------
                sprintf(msg, "F%03d", send_report.data[3]);
                lcd_out2(6,1,msg,4);
            }
            led4 = 0;
        }
        // Up
    //    else if( js_rud_undata < (js_center_value_r_ud - DEAD_ZONE_BAND_WIDTH) ){
        else if( js_rud_undata < (js_center_value_r_ud - 5) ){
            // Crawler Reverse Run       
            led4 = 1;
            motor_speed = calc_motor_speed( js_rud_undata, 1 );
            send_report.data[3] = 255 - js_rud_undata; // speed
            DEBUG_PRINT_L2("Handy> R-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rud_undata, send_report.data[3], js_center_value_r_ud);
            if ( sw_validpart == 0 ){
                //-----------123456789 123456--------------------
                sprintf(msg, "B%03d", send_report.data[3]);
                lcd_out2(6,1,msg,4);
            }
            led4 = 0;
        }           
        else{
            send_report.data[3] = 0x80; // speed
            //    calibrate_joystick(js_ud_un);   // Under constraction
            led4 = 0;
        }

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

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

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

        else{
            send_report.data[0] = 0x80; // speed
        //    calibrate_joystick(js_l_ud_un);   // Under constraction
            led4 = 0;
        }

        if( ( send_report.data[0] == 0x80 ) && ( send_report.data[1] == 0x80 ) ){
            lcd_out2(1,1,"    ",4);
        }
        
        // ---------------------------------------------
        // 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;
            led_ind7 = 1;
            //            123456
            if ( sw_validpart == 0 )
                //            0123456789012345
                lcd_out2(10,1,"RF[P] ",6);
        }
        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;
            led_ind7 = 1;
            //            123456
            if ( sw_validpart == 0 )
                //            0123456789012345
                lcd_out2(10,1,"RF[I] ",6);
        }
        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;
            led_ind7 = 1;
            //            123456
            if ( sw_validpart == 0 )
                //            0123456789012345
                lcd_out2(10,1,"LB[P] ",6);
        }
        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;
            led_ind7 = 1;
            //            123456
            if ( sw_validpart == 0 )
                //            0123456789012345
                lcd_out2(10,1,"LB[I] ",6);
        }
        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;
            led_ind3 = 1;
            //            123456
            if ( sw_validpart == 1 )
                //            0123456789012345
                lcd_out2(10,1,"Wch Dn",6);
        }
        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;
            led_ind3 = 1;
            //            123456
            if ( sw_validpart == 1 )
                //            0123456789012345
                lcd_out2(10,1,"Wch Up",6);
        }
        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;
            //            0123456789012345
            lcd_out2(10,1,"      ",6);
        }
        // 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_validpart == 1 ){ // sw off
            send_report.data[5] |= 0x02;    // SW off: Winch valid
        }
        else{ // sw off
            send_report.data[5] &= 0xFD;    // SW on: Tfm. crawler valid
        }
        if( sw_ikmode == 1 ){ // sw off 
            send_report.data[5] |= 0x01;    // off: K mode
        }
        else{ // sw off
            send_report.data[5] &= 0xFE;    // off: I mode
        }
        
        // *************************
        // Send Command to HID host: Hot Plug !!
        // *************************
        hid.send(&send_report); // send command to USB HID host
        
        Thread::wait(5);
    }

}

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); 
}

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

    for( int i = 0; i < cnt; i++ ){
        lcd.locate(column+i,row);
        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_ikmode.mode( PullUp );    // use internal pullup
    sw_validpart.mode( PullUp );    // use internal pullup
    
//    i2c_saddress = Target_IIC_ADDR;     // defined at header 
    i2c_saddress = 0x10;     // defined at header 
    i2c_saddress <<= 1;
    slave.address(i2c_saddress);
    DEBUG_PRINT_L0("Handy> IIC Address: %02x\r\n",i2c_saddress);

    lcd_out2(0,0,"B2DebrisSurveyor",16); 
    lcd_out2(0,1,"Revast Co.,Ltd. ",16); 
    led_demo();
    lcd_out2(0,0,"Rev1.00 20160218",16); 
    lcd_out2(0,1,"System Booting..",16); 
    
    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_out2(0,0,"Booted up !     ",16); 
 
    if(hid.readNB(&recv_report)) {
    //if(hid.read(&recv_report)) {
        for(int i = 1; i < recv_report.length; i++) {
            DEBUG_PRINT_L0("%d ", recv_report.data[i]);
        }
        DEBUG_PRINT_L0("\r\n");
    }
    //           1234567890123456
    lcd_out2(0,0,"B2DebrisProbe   ",16); 
    Thread::wait(25);
    lcd_out2(0,0,"System OK[-]    ",16);
    lcd_out2(0,1,"                ",16);
       
    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");
            
        }
        //  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_validpart == 0 )&&( sw_ikmode == 0 )){
            //            0123456789012345 
        //  lcd_out2(0,0," [O][X] | JS I  ",16);
            lcd_out2(0,0,"Crawler Inline  ",16);
            led_crawler_valid(counter);
            led_ind0 = 0;
            led_ind1 = 0;
            led_ind2 = 0;
            led_ind3 = 0;
        }
        else if (( sw_validpart == 0 )&&( sw_ikmode == 1 )){
            //            0123456789012345 
        //  lcd_out2(0,0," [O][X] | JS K  ",16);
            lcd_out2(0,0,"Crawler Para    ",16);
            led_crawler_valid(counter);
            led_ind0 = 0;
            led_ind1 = 0;
            led_ind2 = 0;
            led_ind3 = 0;
        }
        else if (( sw_validpart == 1 )&&( sw_ikmode == 0 )){
            //            0123456789012345 
        //  lcd_out2(0,0," [X][O] | JS I  ",16);
            lcd_out2(0,0,"Winch           ",16);
            led_winch_valid(counter);
            led_ind4 = 0;
            led_ind5 = 0;
            led_ind6 = 0;
            led_ind7 = 0;
        }        
        else if (( sw_validpart == 1 )&&( sw_ikmode == 1 )){
            //            0123456789012345 
        //  lcd_out2(0,0," [X][O] | JS K  ",16);
            lcd_out2(0,0,"Winch           ",16);
            led_winch_valid(counter);
            led_ind4 = 0;
            led_ind5 = 0;
            led_ind6 = 0;
            led_ind7 = 0;
        }        
        
#ifdef __UNUSE__       
        // ----------------------------------------------------------------
        // i2c slave read
        // ----------------------------------------------------------------
        i = slave.receive();
        slave.read(buf, NumberOfI2CCommand ); /* Read size should be 10bit ?? */
        switch (i) {
            case I2CSlave::NoData:
            //    DEBUG_PRINT_L0("Handy> the slave has not been addressed\r\n");
            //    lcd_out2(0,0,"System OK[-]    ",16);
                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_out2(0,0,"System OK[|]    ",16); // command
                }
                else if( buf[5] == 0x04 ){
             //       lcd_out2(0,0,"ERROR           ",16);
                }
                else{
             //       lcd_out2(0,0,"System OK[/]    ",16); // unknown command
                }
                break;
        }
        for(int i = 0; i < 10; i++) buf[i] = 0;    // Clear buffer
#endif
        counter++;
        if( counter >= 100 ){
        //    led1 = !led1;
            counter = 0;
        }
        Thread::wait(5);
    }
}