2_main.cpp

00001 #include "mbed.h"
00002 #include "USBHID.h"
00003 #include "USBSerial.h"
00004 #include "rtos.h"
00005 #include "common.h"
00006 #include "TextLCD.h"
00007 
00008 //#include "TextLCD.h"
00009  
00010 //We declare a USBHID device. By default input and output reports are 64 bytes long.
00011 USBHID hid(8, 8);
00012 
00013 //USBSerial serial;
00014 Serial pc(USBTX, USBRX); // UART
00015 
00016 // I2C i2c(p9, p10);      // I2C SDA, SCL
00017 I2CSlave slave(p9, p10); //I2C SDA SCL
00018  
00019 //This report will contain data to be sent
00020 HID_REPORT send_report;
00021 HID_REPORT recv_report;
00022  
00023 DigitalOut led1(LED1); // 1: on, 0: off
00024 DigitalOut led2(LED2); // 1: on, 0: off
00025 DigitalOut led3(LED3); // 1: on, 0: off
00026 DigitalOut led4(LED4); // 1: on, 0: off
00027 
00028 // LED indicator
00029 DigitalOut led_ind0(p13);
00030 DigitalOut led_ind1(p14);
00031 DigitalOut led_ind2(p15);
00032 DigitalOut led_ind3(p25);
00033 DigitalOut led_ind4(p26);
00034 DigitalOut led_ind5(p27);
00035 DigitalOut led_ind6(p28);
00036 DigitalOut led_ind7(p8);
00037 
00038 // Switch Matrix 3bit SW ON = 0 input
00039 //  bit 2 1 0   Send data 
00040 //   0  1 1 1 : 
00041 //   1  1 1 0 : 0x01 RF Tfm K
00042 //   2  1 0 1 : 0x02 RF Tfm I
00043 //   3  1 0 0 : 0x04 LB Tfm K
00044 //   4  0 1 1 : 0x08 LB Tfm I
00045 //   5  0 1 0 : 0x10 Winch Down
00046 //   6  0 0 1 : 0x20 Winch Up
00047 //   7  0 0 0 : 
00048 DigitalIn sw1(p5); // Bit 0
00049 DigitalIn sw2(p6); // Bit 1
00050 DigitalIn sw3(p7); // Bit 2
00051 
00052 // Digital Input 1:OFF, 0:ON
00053 DigitalIn sw_ikmode(p29);      // SW: i \ k ,pde ( 1: K, 0: I )
00054 DigitalIn sw_validpart(p30);   // SW: CrExp Shape ( 1: Left-Winch. 0: Right-tfmcrwler )
00055 
00056 // Analig Joystick input for crawler control
00057 //AnalogIn js_l_ud(p20);  // Crawler L-UD
00058 //AnalogIn js_l_lr(p19);  // Crawler L-LR
00059 //AnalogIn js_r_ud(p17);  // Crawler R-UD
00060 //AnalogIn js_r_lr(p16);  // Crawler R-LR
00061 
00062 AnalogIn js_l_ud(p19);  // Crawler L-UD
00063 AnalogIn js_l_lr(p20);  // Crawler L-LR
00064 AnalogIn js_r_ud(p16);  // Crawler R-UD
00065 AnalogIn js_r_lr(p17);  // Crawler R-LR
00066 
00067 uint8_t js_center_value_r_ud = 0x80;
00068 uint8_t js_center_value_r_lr = 0x80;
00069 uint8_t js_center_value_l_ud = 0x80;
00070 uint8_t js_center_value_l_lr = 0x80;
00071 
00072 void led_demo(void);
00073 uint8_t calc_motor_speed( uint8_t data, int32_t dir );
00074 void lcd_out( int, int, char* );
00075 void lcd_out2( int column, int row, char* msg, int cnt);
00076 
00077 void calibrate_joystick( int mode, uint8_t data_ud, uint8_t data_lr );
00078 
00079 TextLCD lcd(p11, p12, p24, p23, p22, p21); // rs, e, d4-d7
00080 
00081 int i2c_saddress = 0;
00082 
00083 // 
00084 void led_winch_valid( int counter )
00085 {
00086     if( counter < 25)
00087     {
00088         led_ind0 = 0;
00089         led_ind1 = 0;
00090         led_ind2 = 0;
00091         led_ind3 = 0; 
00092     }
00093     else
00094     {
00095         led_ind0 = 1;
00096         led_ind1 = 1;
00097         led_ind2 = 1;
00098         led_ind3 = 1; 
00099     }
00100 }
00101 
00102 void led_crawler_valid( int counter )
00103 {
00104     if( counter < 25)
00105     {
00106         led_ind4 = 0;
00107         led_ind5 = 0;
00108         led_ind6 = 0;
00109         led_ind7 = 0; 
00110     }
00111     else
00112     {
00113         led_ind4 = 1;
00114         led_ind5 = 1;
00115         led_ind6 = 1;
00116         led_ind7 = 1; 
00117     }
00118 }
00119 
00120 // LED demo
00121 void led_demo()
00122 {
00123     int i;
00124     for( i = 0; i < 7; i++ ) {
00125         led_ind0 = 1;
00126         led_ind1 = 1;
00127         led_ind2 = 1;
00128         led_ind3 = 1;
00129         led_ind4 = 1;
00130         led_ind5 = 1;
00131         led_ind6 = 1;
00132         led_ind7 = 1;
00133         led1 = 1;   // on
00134         led2 = 0;   // off
00135         led3 = 0;   // off
00136         led4 = 0;   // off
00137         Thread::wait(15);
00138         led_ind0 = 0;
00139         led_ind1 = 0;
00140         led_ind2 = 0;
00141         led_ind3 = 0;
00142         led_ind4 = 0;
00143         led_ind5 = 0;
00144         led_ind6 = 0;
00145         led_ind7 = 0;
00146         led1 = 0;   // off
00147         led2 = 1;   // on
00148         led3 = 0;   // off
00149         led4 = 0;   // off
00150         Thread::wait(15);
00151         led_ind0 = 1;
00152         led_ind1 = 1;
00153         led_ind2 = 1;
00154         led_ind3 = 1;
00155         led_ind4 = 1;
00156         led_ind5 = 1;
00157         led_ind6 = 1;
00158         led_ind7 = 1;
00159         led1 = 0;   // off
00160         led2 = 0;   // off
00161         led3 = 1;   // on
00162         led4 = 0;   // off
00163         Thread::wait(15);
00164         led_ind0 = 0;
00165         led_ind1 = 0;
00166         led_ind2 = 0;
00167         led_ind3 = 0;
00168         led_ind4 = 0;
00169         led_ind5 = 0;
00170         led_ind6 = 0;
00171         led_ind7 = 0;
00172         led1 = 0;   // off
00173         led2 = 0;   // off
00174         led3 = 0;   // off
00175         led4 = 1;   // on
00176         Thread::wait(15);
00177         led_ind0 = 1;
00178         led_ind1 = 1;
00179         led_ind2 = 1;
00180         led_ind3 = 1;
00181         led_ind4 = 1;
00182         led_ind5 = 1;
00183         led_ind6 = 1;
00184         led_ind7 = 1;
00185         led1 = 0;   // off
00186         led2 = 0;   // off
00187         led3 = 0;   // off
00188         led4 = 0;   // off
00189     }
00190 }
00191 
00192 // *****************************************************************
00193 //  calibration joystick data
00194 // *****************************************************************
00195 void calibrate_joystick( int mode, uint8_t data_ud, uint8_t data_lr ){
00196     uint16_t tmp;
00197 
00198     if( mode == 0 )
00199     {
00200         tmp = (uint16_t)js_center_value_r_ud;
00201         tmp += (uint16_t)data_ud;
00202         tmp /= 2;
00203         js_center_value_r_ud = (uint8_t)tmp;
00204         
00205         tmp = (uint16_t)js_center_value_r_lr;
00206         tmp += (uint16_t)data_lr;
00207         tmp /= 2;
00208         js_center_value_r_lr = (uint8_t)tmp;
00209     }
00210     else
00211     {
00212         
00213         tmp = (uint16_t)js_center_value_l_ud;
00214         tmp += (uint16_t)data_ud;
00215         tmp /= 2;
00216         js_center_value_l_ud = (uint8_t)tmp;
00217         
00218         tmp = (uint16_t)js_center_value_l_lr;
00219         tmp += (uint16_t)data_lr;
00220         tmp /= 2;
00221         js_center_value_l_lr = (uint8_t)tmp;
00222     }
00223 }
00224 
00225 // *****************************************************************
00226 //  calcuilation motor speed
00227 // *****************************************************************
00228 uint8_t calc_motor_speed( uint8_t data, int32_t dir ){
00229     uint8_t speed;
00230     if( dir == 0 ){
00231 //        speed = (data - js_center_value) * 100 / js_center_value;
00232         if( speed > 95 ) speed = 100;
00233     }
00234     else{
00235 //        speed = (js_center_value - data) * 100 / js_center_value;
00236         if( speed > 95 ) speed = 100;
00237     }
00238     return speed;
00239 }
00240 
00241 // *****************************************************************
00242 //  switch control task
00243 // ----------------------
00244 //
00245 //    Up:    center -> 255
00246 //    Down:  002 -> center
00247 //    Right: center -> 255
00248 //    Left : 0 -> center 
00249 //
00250 // *****************************************************************
00251 void sw_task( void const *){
00252 
00253     char msg[16];
00254     
00255     uint16_t js_rud_data;
00256     uint16_t js_rlr_data;
00257     uint8_t js_rud_undata;
00258     uint8_t js_rlr_undata;
00259     
00260     uint16_t js_lud_data;
00261     uint16_t js_llr_data;
00262     uint8_t js_lud_undata;
00263     uint8_t js_llr_undata;
00264 
00265     float motor_speed_f = 0;
00266     uint8_t motor_speed = 0;
00267     
00268     send_report.length = 8;
00269         
00270     send_report.data[0] = 0x80; // L JoyStick Left/Right
00271     send_report.data[1] = 0x80; // L JoyStick Up/Down
00272     send_report.data[2] = 0x80; // R JoyStick Left/Right^
00273     send_report.data[3] = 0x80; // R JoyStick Up/Down
00274     send_report.data[4] = 0x00; // Operatiuon Switch
00275     // Data Bytes
00276     //  7 6 5 4   3    2    1    0
00277     // +-+-+-+-+-+----+----+----+----+
00278     // |-|-|-|-|-|LB-I|LB-K|RF-I|RF-K|
00279     // +-+-+-+-+-+----+----+----+----+
00280         
00281     send_report.data[5] = 0x00; // JS mode switch 
00282     // Command Bit
00283     //  7 6 5 4 3 2  1   0
00284     // +-+-+-+-+-+-+---+----+
00285     // |-|-|-|-|-|-|S/D|I/KO|
00286     // +-+-+-+-+-+-+---+----+
00287     //              1:S 1:KO  <--- sw off
00288     //              0:D 0:I   <--- sw on
00289 
00290 
00291    
00292     send_report.data[6] = 0x00; // No Function
00293     send_report.data[7] = 0x00; // No Function
00294     send_report.data[8] = 0x00;
00295     
00296     DEBUG_PRINT_L1("Calibrating joystick ... ");
00297 //    for( int i = 0; i < CALIBRATION_COUNT; i++){
00298     for( int i = 0; i < 10; i++){
00299         js_rud_data = js_r_ud.read_u16();
00300         js_rlr_data = js_r_lr.read_u16();
00301         js_rud_undata = (uint8_t)(js_rud_data >> 8);
00302         js_rlr_undata = (uint8_t)(js_rlr_data >> 8);
00303         calibrate_joystick( 0, js_rud_undata, js_rlr_undata);   // Under constraction
00304         js_lud_data = js_l_ud.read_u16();
00305         js_llr_data = js_l_lr.read_u16();
00306         js_lud_undata = (uint8_t)(js_lud_data >> 8);
00307         js_llr_undata = (uint8_t)(js_llr_data >> 8);
00308         calibrate_joystick( 1, js_lud_undata, js_llr_undata);   // Under constraction
00309     }
00310     DEBUG_PRINT_L1("done\r\n");
00311     
00312     while(1){
00313         js_rud_data = js_r_ud.read_u16();
00314         js_rlr_data = js_r_lr.read_u16();
00315         js_lud_data = js_l_ud.read_u16();
00316         js_llr_data = js_l_lr.read_u16();
00317         
00318         js_rud_undata = (uint8_t)(js_rud_data >> 8);
00319         js_rlr_undata = (uint8_t)(js_rlr_data >> 8);
00320         js_lud_undata = (uint8_t)(js_lud_data >> 8);
00321         js_llr_undata = (uint8_t)(js_llr_data >> 8);
00322         
00323         DEBUG_PRINT_L1("Handy> ");
00324         DEBUG_PRINT_L1("R:%03d/%3d,%03d/%3d  ", js_rud_undata, js_center_value_r_ud, js_rlr_undata, js_center_value_r_ud );
00325         DEBUG_PRINT_L1("L:%03d/%3d,%03d/%3d  ", js_lud_undata, js_center_value_l_ud, js_llr_undata, js_center_value_l_ud );
00326         DEBUG_PRINT_L1("SW: %d %d %d  ", sw1.read(), sw2.read(), sw3.read() );
00327         DEBUG_PRINT_L1("JS: %d %d\r\n", sw_ikmode.read(), sw_validpart.read() );
00328         
00329         // ---------------------------------------------
00330         // Right JoyStick Control
00331         // ---------------------------------------------
00332         // Down
00333     //    if( js_rud_undata > (js_center_value_r_ud + DEAD_ZONE_BAND_WIDTH) ){
00334         if( js_rud_undata > (js_center_value_r_ud + 5) ){
00335             // Crawler Forward Run       
00336             led4 = 1;
00337             motor_speed = calc_motor_speed( js_rud_undata, 0 );
00338             send_report.data[3] = 255 - js_rud_undata; // speed
00339             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);
00340             if ( sw_validpart == 0 ){
00341                 //------------123 45 --------------------
00342                 sprintf(msg, "F%03d", send_report.data[3]);
00343                 lcd_out2(6,1,msg,4);
00344             }
00345             led4 = 0;
00346         }
00347         // Up
00348     //    else if( js_rud_undata < (js_center_value_r_ud - DEAD_ZONE_BAND_WIDTH) ){
00349         else if( js_rud_undata < (js_center_value_r_ud - 5) ){
00350             // Crawler Reverse Run       
00351             led4 = 1;
00352             motor_speed = calc_motor_speed( js_rud_undata, 1 );
00353             send_report.data[3] = 255 - js_rud_undata; // speed
00354             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);
00355             if ( sw_validpart == 0 ){
00356                 //-----------123456789 123456--------------------
00357                 sprintf(msg, "B%03d", send_report.data[3]);
00358                 lcd_out2(6,1,msg,4);
00359             }
00360             led4 = 0;
00361         }           
00362         else{
00363             send_report.data[3] = 0x80; // speed
00364             //    calibrate_joystick(js_ud_un);   // Under constraction
00365             led4 = 0;
00366         }
00367 
00368         // Right
00369      //   if( js_rlr_undata > (js_center_value_r_ud + DEAD_ZONE_BAND_WIDTH) ){
00370         if( js_rlr_undata > (js_center_value_r_ud + 5) ){
00371             // Crawler Forward Run       
00372             led4 = 1;
00373             motor_speed = calc_motor_speed( js_rlr_undata, 0 );
00374             send_report.data[2] = js_rlr_undata; // speed
00375             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);
00376             if ( sw_validpart == 0 ){
00377                 //-----------123456789 123456--------------------
00378                 sprintf(msg,"R%03d", send_report.data[2]);
00379                 lcd_out2(6,1,msg,4);
00380             }
00381             led4 = 0;
00382         }
00383         
00384         // Left
00385     //    else if( js_rlr_undata < (js_center_value_r_ud - DEAD_ZONE_BAND_WIDTH) ){
00386         else if( js_rlr_undata < (js_center_value_r_ud - 5) ){
00387             // Crawler Reverse Run       
00388             led4 = 1;
00389             motor_speed = calc_motor_speed( js_rlr_undata, 1 );
00390             send_report.data[2] = js_rlr_undata; // speed
00391             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);
00392             if ( sw_validpart == 0 ){
00393                 //-----------123456789 123456--------------------
00394                 sprintf(msg,"L%03d", send_report.data[2]);
00395                 lcd_out2(6,1,msg,4);
00396             }
00397             led4 = 0;
00398         }
00399 
00400         else{
00401             send_report.data[2] = 0x80; // speed
00402         //    calibrate_joystick(js_ud_un);   // Under constraction
00403             led4 = 0;
00404         }
00405         
00406         if( ( send_report.data[2] == 0x80 ) && ( send_report.data[3] == 0x80 ) ){
00407             lcd_out2(6,1,"    ",4);
00408         }
00409         
00410         // ---------------------------------------------
00411         // Left JoyStick Control
00412         // ---------------------------------------------
00413         // Down
00414    //     if( js_lud_undata > (js_center_value_l_ud + DEAD_ZONE_BAND_WIDTH) ){
00415         if( js_lud_undata > (js_center_value_l_ud + 5) ){
00416             // Crawler Forward Run       
00417             led4 = 1;
00418             motor_speed = calc_motor_speed( js_lud_undata, 0 );
00419             send_report.data[1] = 255 - js_lud_undata; // speed
00420             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);
00421             if ( sw_validpart == 0 ){
00422                 //-----------123456789 123456--------------------
00423                 sprintf(msg,"F%03d", send_report.data[1]);
00424                 lcd_out2(1,1,msg,4);
00425             }
00426             led4 = 0;
00427         }
00428         // Up
00429     //    else if( js_lud_undata < (js_center_value_l_ud - DEAD_ZONE_BAND_WIDTH) ){
00430         else if( js_lud_undata < (js_center_value_l_ud - 5) ){
00431             // Crawler Reverse Run       
00432             led4 = 1;
00433             motor_speed = calc_motor_speed( js_lud_undata, 1 );
00434             send_report.data[1] = 255 - js_lud_undata; // speed
00435             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);
00436             if ( sw_validpart == 0 ){
00437                 //-----------123456789 123456--------------------
00438                 sprintf(msg,"B%03d", send_report.data[1]);
00439                 lcd_out2(1,1,msg,4);
00440             }
00441             led4 = 0;
00442         }           
00443         else{
00444             send_report.data[1] = 0x80; // speed
00445             //    calibrate_joystick(js_l_ud_un);   // Under constraction
00446             led4 = 0;
00447         }
00448 
00449         // Right           
00450 //        if( js_llr_undata > (js_center_value_l_ud + DEAD_ZONE_BAND_WIDTH) ){
00451         if( js_llr_undata > (js_center_value_l_ud + 5) ){
00452             // Crawler Forward Run       
00453             led4 = 1;
00454             motor_speed = calc_motor_speed( js_llr_undata, 0 );
00455             send_report.data[0] = js_llr_undata; // speed
00456             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);
00457             if ( sw_validpart == 0 ){
00458                 //-----------123456789 123456--------------------
00459                 sprintf(msg,"R%03d", send_report.data[0]);
00460                 lcd_out2(1,1,msg,4);
00461             }
00462             led4 = 0;
00463         }
00464         // Left
00465 //        else if( js_llr_undata < (js_center_value_l_ud - DEAD_ZONE_BAND_WIDTH) ){
00466         else if( js_llr_undata < (js_center_value_l_ud - 5) ){
00467             // Crawler Reverse Run       
00468             led4 = 1;
00469             motor_speed = calc_motor_speed( js_llr_undata, 1 );
00470             send_report.data[0] = js_llr_undata; // speed
00471             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);
00472             if ( sw_validpart == 0 ){
00473                 //-----------123456789 123456--------------------
00474                 sprintf(msg,"L%03d", send_report.data[0]);
00475                 lcd_out2(1,1,msg,4);
00476             }
00477             led4 = 0;
00478         }
00479 
00480         else{
00481             send_report.data[0] = 0x80; // speed
00482         //    calibrate_joystick(js_l_ud_un);   // Under constraction
00483             led4 = 0;
00484         }
00485 
00486         if( ( send_report.data[0] == 0x80 ) && ( send_report.data[1] == 0x80 ) ){
00487             lcd_out2(1,1,"    ",4);
00488         }
00489         
00490         // ---------------------------------------------
00491         // Other Switch Control: Pass
00492         // ---------------------------------------------
00493         if ((sw1 == 0)&&(sw2 == 1)&&(sw3==1)){
00494             led3 = 1;
00495             send_report.data[4] = 0x01;     // RF Tfm K (Fwd)
00496             led_ind4 = 1;
00497             led_ind5 = 1;
00498             led_ind6 = 1;
00499             led_ind7 = 1;
00500             //            123456
00501             if ( sw_validpart == 0 )
00502                 //            0123456789012345
00503                 lcd_out2(10,1,"RF[P] ",6);
00504         }
00505         else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==1)){
00506             send_report.data[4] = 0x02;     // RF Tfm I (Rvs)
00507             led3 = 1;
00508             led_ind4 = 1;
00509             led_ind5 = 1;
00510             led_ind6 = 1;
00511             led_ind7 = 1;
00512             //            123456
00513             if ( sw_validpart == 0 )
00514                 //            0123456789012345
00515                 lcd_out2(10,1,"RF[I] ",6);
00516         }
00517         else if ((sw1 == 0)&&(sw2 == 0)&&(sw3==1)){  
00518             send_report.data[4] = 0x04;     // LB Tfm K (Fwd)
00519             led3 = 1;
00520             led_ind4 = 1;
00521             led_ind5 = 1;
00522             led_ind6 = 1;
00523             led_ind7 = 1;
00524             //            123456
00525             if ( sw_validpart == 0 )
00526                 //            0123456789012345
00527                 lcd_out2(10,1,"LB[P] ",6);
00528         }
00529         else if ((sw1 == 1)&&(sw2 == 1)&&(sw3==0)){  
00530             led3 = 1;
00531             send_report.data[4] = 0x08;     // LB Tfm I (Rvs)
00532             led_ind4 = 1;
00533             led_ind5 = 1;
00534             led_ind6 = 1;
00535             led_ind7 = 1;
00536             //            123456
00537             if ( sw_validpart == 0 )
00538                 //            0123456789012345
00539                 lcd_out2(10,1,"LB[I] ",6);
00540         }
00541         else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==0)){  
00542             led2 = 1;
00543             send_report.data[4] = 0x10;     // Winch Down (Fwd)
00544             led_ind0 = 1;
00545             led_ind1 = 1;
00546             led_ind2 = 1;
00547             led_ind3 = 1;
00548             //            123456
00549             if ( sw_validpart == 1 )
00550                 //            0123456789012345
00551                 lcd_out2(10,1,"Wch Dn",6);
00552         }
00553         else if ((sw1 == 0)&&(sw2 == 1)&&(sw3==0)){  
00554             led2 = 1;
00555             send_report.data[4] = 0x20;     // Winch Up (Rvs) 
00556             led_ind0 = 1;
00557             led_ind1 = 1;
00558             led_ind2 = 1;
00559             led_ind3 = 1;
00560             //            123456
00561             if ( sw_validpart == 1 )
00562                 //            0123456789012345
00563                 lcd_out2(10,1,"Wch Up",6);
00564         }
00565         else{
00566             send_report.data[4] = 0;
00567             /*
00568             led_ind0 = 0;
00569             led_ind1 = 0;
00570             led_ind2 = 0;
00571             led_ind3 = 0;
00572             led_ind4 = 0;
00573             led_ind5 = 0;
00574             led_ind6 = 0;
00575             led_ind7 = 0;
00576             */
00577             led2 = 0;
00578             led3 = 0;
00579             //            0123456789012345
00580             lcd_out2(10,1,"      ",6);
00581         }
00582         // Command Bit
00583         //  7 6 5 4 3 2  1   0
00584         // +-+-+-+-+-+-+---+----+
00585         // |-|-|-|-|-|-|S/D|I/KO|
00586         // +-+-+-+-+-+-+---+----+
00587         //              1:S 1:KO  <--- sw off
00588         //              0:D 0:I   <--- sw on
00589         if( sw_validpart == 1 ){ // sw off
00590             send_report.data[5] |= 0x02;    // SW off: Winch valid
00591         }
00592         else{ // sw off
00593             send_report.data[5] &= 0xFD;    // SW on: Tfm. crawler valid
00594         }
00595         if( sw_ikmode == 1 ){ // sw off 
00596             send_report.data[5] |= 0x01;    // off: K mode
00597         }
00598         else{ // sw off
00599             send_report.data[5] &= 0xFE;    // off: I mode
00600         }
00601         
00602         // *************************
00603         // Send Command to HID host: Hot Plug !!
00604         // *************************
00605         hid.send(&send_report); // send command to USB HID host
00606         
00607         Thread::wait(5);
00608     }
00609 
00610 }
00611 
00612 void lcd_out( int column, int row, char* msg ){
00613     
00614 //    lcd.cls();
00615     lcd.locate(column,row);
00616 
00617 /*    
00618     for( int i = 0; i < 16; i++ ){
00619         lcd.putc(*msg++);
00620     }
00621 */    
00622     lcd.printf( msg );
00623     Thread::wait(10); 
00624 }
00625 
00626 void lcd_out2( int column, int row, char* msg, int cnt){
00627     
00628 //    lcd.cls();
00629 
00630     for( int i = 0; i < cnt; i++ ){
00631         lcd.locate(column+i,row);
00632         lcd.putc(*msg++);
00633     }
00634     
00635 //    lcd.printf( msg );
00636     Thread::wait(10); 
00637 }
00638 
00639 // **********************************************************************
00640 //
00641 //    Main Function of this program
00642 //
00643 // **********************************************************************
00644 int main(void) {
00645     
00646     int32_t counter = 0;
00647     
00648     char msg[16];
00649     char buf[11];
00650     int i;
00651 
00652     pc.baud(115200);
00653 
00654     sw_ikmode.mode( PullUp );    // use internal pullup
00655     sw_validpart.mode( PullUp );    // use internal pullup
00656     
00657 //    i2c_saddress = Target_IIC_ADDR;     // defined at header 
00658     i2c_saddress = 0x10;     // defined at header 
00659     i2c_saddress <<= 1;
00660     slave.address(i2c_saddress);
00661     DEBUG_PRINT_L0("Handy> IIC Address: %02x\r\n",i2c_saddress);
00662 
00663     lcd_out2(0,0,"B2DebrisSurveyor",16); 
00664     lcd_out2(0,1,"Revast Co.,Ltd. ",16); 
00665     led_demo();
00666     lcd_out2(0,0,"Rev1.00 20160218",16); 
00667     lcd_out2(0,1,"System Booting..",16); 
00668     
00669     DEBUG_PRINT_L0("\r\n");
00670     DEBUG_PRINT_L0("Handy> +-----------------------------------------------------------\r\n");
00671     DEBUG_PRINT_L0("Handy> | Project: Crawler Explorer for 1F-1 PCV internal inspection\r\n");
00672     DEBUG_PRINT_L0("Handy> |---------\r\n");
00673     DEBUG_PRINT_L0("Handy> | This is: Handy Controller main program over USB-HID \r\n");
00674 //    DEBUG_PRINT_L0("Handy> |   Target MCU: %s\r\n", Targetmbed);
00675     DEBUG_PRINT_L0("Handy> |   Letest update: %s\r\n", LatestUpDate);
00676     DEBUG_PRINT_L0("Handy> |   Program Revision: %s\r\n", ProgramRevision);
00677     DEBUG_PRINT_L0("Handy> |   Author: %s\r\n", Author);
00678     DEBUG_PRINT_L0("Handy> |   Copyright(C) 2015 %s Allright Reserved\r\n", Company);
00679     DEBUG_PRINT_L0("Handy> +-----------------------------------------------------------\r\n");
00680    
00681     Thread task1(sw_task, NULL, osPriorityNormal, 256 * 4);
00682 
00683     //           1234567890123456 
00684     lcd_out2(0,0,"Booted up !     ",16); 
00685  
00686     if(hid.readNB(&recv_report)) {
00687     //if(hid.read(&recv_report)) {
00688         for(int i = 1; i < recv_report.length; i++) {
00689             DEBUG_PRINT_L0("%d ", recv_report.data[i]);
00690         }
00691         DEBUG_PRINT_L0("\r\n");
00692     }
00693     //           1234567890123456
00694     lcd_out2(0,0,"B2DebrisProbe   ",16); 
00695     Thread::wait(25);
00696     lcd_out2(0,0,"System OK[-]    ",16);
00697     lcd_out2(0,1,"                ",16);
00698        
00699     while (1) {
00700        //try to read a msg
00701 
00702         if(hid.readNB(&recv_report)) {
00703             for(int i = 1; i < recv_report.length; i++) {
00704             //    DEBUG_PRINT("%d ", recv_report.data[i]);
00705             //    serial.printf("%d ", recv_report.data[i]);
00706             }
00707             // DEBUG_PRINT("\r\n");
00708             // serial.printf("\r\n");
00709             
00710         }
00711         //  7 6 5 4 3 2  1   0
00712         // +-+-+-+-+-+-+---+----+
00713         // |-|-|-|-|-|-|S/D|I/KO|
00714         // +-+-+-+-+-+-+---+----+
00715         //              1:S 1:KO  <--- sw off
00716         //              0:D 0:I   <--- sw on
00717         if (( sw_validpart == 0 )&&( sw_ikmode == 0 )){
00718             //            0123456789012345 
00719         //  lcd_out2(0,0," [O][X] | JS I  ",16);
00720             lcd_out2(0,0,"Crawler Inline  ",16);
00721             led_crawler_valid(counter);
00722             led_ind0 = 0;
00723             led_ind1 = 0;
00724             led_ind2 = 0;
00725             led_ind3 = 0;
00726         }
00727         else if (( sw_validpart == 0 )&&( sw_ikmode == 1 )){
00728             //            0123456789012345 
00729         //  lcd_out2(0,0," [O][X] | JS K  ",16);
00730             lcd_out2(0,0,"Crawler Para    ",16);
00731             led_crawler_valid(counter);
00732             led_ind0 = 0;
00733             led_ind1 = 0;
00734             led_ind2 = 0;
00735             led_ind3 = 0;
00736         }
00737         else if (( sw_validpart == 1 )&&( sw_ikmode == 0 )){
00738             //            0123456789012345 
00739         //  lcd_out2(0,0," [X][O] | JS I  ",16);
00740             lcd_out2(0,0,"Winch           ",16);
00741             led_winch_valid(counter);
00742             led_ind4 = 0;
00743             led_ind5 = 0;
00744             led_ind6 = 0;
00745             led_ind7 = 0;
00746         }        
00747         else if (( sw_validpart == 1 )&&( sw_ikmode == 1 )){
00748             //            0123456789012345 
00749         //  lcd_out2(0,0," [X][O] | JS K  ",16);
00750             lcd_out2(0,0,"Winch           ",16);
00751             led_winch_valid(counter);
00752             led_ind4 = 0;
00753             led_ind5 = 0;
00754             led_ind6 = 0;
00755             led_ind7 = 0;
00756         }        
00757         
00758 #ifdef __UNUSE__       
00759         // ----------------------------------------------------------------
00760         // i2c slave read
00761         // ----------------------------------------------------------------
00762         i = slave.receive();
00763         slave.read(buf, NumberOfI2CCommand ); /* Read size should be 10bit ?? */
00764         switch (i) {
00765             case I2CSlave::NoData:
00766             //    DEBUG_PRINT_L0("Handy> the slave has not been addressed\r\n");
00767             //    lcd_out2(0,0,"System OK[-]    ",16);
00768                 break;
00769             case I2CSlave::ReadAddressed:
00770                 DEBUG_PRINT_L0("Handy> the master has requested a read from this slave\r\n");
00771                 slave.write(msg, strlen(msg) + 1); // Includes null char
00772                 break;
00773              case I2CSlave::WriteGeneral:
00774                 DEBUG_PRINT_L0("Handy> the master is writing to all slave\r\n"); 
00775                 slave.read(buf, NumberOfI2CCommand);
00776                 DEBUG_PRINT_L0("Handy> Read G: %s\r\n", buf);
00777                 break;
00778              case I2CSlave::WriteAddressed:
00779                 DEBUG_PRINT_L0("Handy> the master is writing to this slave\r\n");
00780                 slave.read(buf, NumberOfI2CCommand);
00781              //   DEBUG_PRINT_L0("Handy> Read A: %s\r\n", buf);
00782                 if( buf[5] == 0x01 ){
00783              //       lcd_out2(0,0,"System OK[|]    ",16); // command
00784                 }
00785                 else if( buf[5] == 0x04 ){
00786              //       lcd_out2(0,0,"ERROR           ",16);
00787                 }
00788                 else{
00789              //       lcd_out2(0,0,"System OK[/]    ",16); // unknown command
00790                 }
00791                 break;
00792         }
00793         for(int i = 0; i < 10; i++) buf[i] = 0;    // Clear buffer
00794 #endif
00795         counter++;
00796         if( counter >= 100 ){
00797         //    led1 = !led1;
00798             counter = 0;
00799         }
00800         Thread::wait(5);
00801     }
00802 }