z ysaito
2018.07.26
Dependencies: TextLCD USBDevice mbed-rtos mbed
Diff: 2_main.cpp
- Revision:
- 2:d0a5ee4e7bb8
- Parent:
- 1:368ba89c2e6b
diff -r 368ba89c2e6b -r d0a5ee4e7bb8 2_main.cpp
--- a/2_main.cpp Mon Mar 28 00:09:18 2016 +0000
+++ b/2_main.cpp Thu Jul 26 00:20:57 2018 +0000
@@ -50,8 +50,8 @@
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 )
+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
@@ -72,12 +72,51 @@
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()
{
@@ -95,7 +134,7 @@
led2 = 0; // off
led3 = 0; // off
led4 = 0; // off
- Thread::wait(50);
+ Thread::wait(15);
led_ind0 = 0;
led_ind1 = 0;
led_ind2 = 0;
@@ -108,7 +147,7 @@
led2 = 1; // on
led3 = 0; // off
led4 = 0; // off
- Thread::wait(50);
+ Thread::wait(15);
led_ind0 = 1;
led_ind1 = 1;
led_ind2 = 1;
@@ -121,7 +160,7 @@
led2 = 0; // off
led3 = 1; // on
led4 = 0; // off
- Thread::wait(50);
+ Thread::wait(15);
led_ind0 = 0;
led_ind1 = 0;
led_ind2 = 0;
@@ -134,7 +173,7 @@
led2 = 0; // off
led3 = 0; // off
led4 = 1; // on
- Thread::wait(50);
+ Thread::wait(15);
led_ind0 = 1;
led_ind1 = 1;
led_ind2 = 1;
@@ -211,7 +250,7 @@
// *****************************************************************
void sw_task( void const *){
- char msg[32];
+ char msg[16];
uint16_t js_rud_data;
uint16_t js_rlr_data;
@@ -255,7 +294,8 @@
send_report.data[8] = 0x00;
DEBUG_PRINT_L1("Calibrating joystick ... ");
- for( int i = 0; i < CALIBRATION_COUNT; i++){
+// 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);
@@ -284,67 +324,76 @@
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() );
+ 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 + 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 );
- 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);
+ 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 - 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 );
- 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);
+ 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
- //-----------123456789 123456--------------------
- // sprintf(msg,"R-JS (%03d) ", send_report.data[3]);
- // lcd_out(0,0,msg);
- // calibrate_joystick(js_ud_un); // Under constraction
+ // 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 + 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 );
- 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);
+ 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 - 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 );
- 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);
+ 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;
}
@@ -354,64 +403,77 @@
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 + 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 );
- 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);
+ 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 - 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 );
- 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);
+ 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
- //-----------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) ){
+// 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 );
- 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);
+ 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 - 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 );
- 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);
+ 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;
}
@@ -420,6 +482,10 @@
// 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
@@ -430,6 +496,11 @@
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)
@@ -437,6 +508,11 @@
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)
@@ -444,6 +520,11 @@
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;
@@ -451,6 +532,11 @@
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;
@@ -458,6 +544,11 @@
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;
@@ -465,9 +556,15 @@
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;
@@ -476,8 +573,11 @@
led_ind5 = 0;
led_ind6 = 0;
led_ind7 = 0;
- led2 = 0;
+ */
+ led2 = 0;
led3 = 0;
+ // 0123456789012345
+ lcd_out2(10,1," ",6);
}
// Command Bit
// 7 6 5 4 3 2 1 0
@@ -486,19 +586,17 @@
// +-+-+-+-+-+-+---+----+
// 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
+ if( sw_validpart == 1 ){ // sw off
+ send_report.data[5] |= 0x02; // SW off: Winch valid
}
else{ // sw off
- send_report.data[5] &= 0xFE; // SW on: I-Shape
+ send_report.data[5] &= 0xFD; // SW on: Tfm. crawler valid
}
-
- if( sw_jsmode == 1 ){ // sw off
- send_report.data[5] |= 0x02; // SW off: Single
-
+ if( sw_ikmode == 1 ){ // sw off
+ send_report.data[5] |= 0x01; // off: K mode
}
else{ // sw off
- send_report.data[5] &= 0xFD; // SW: on Dual
+ send_report.data[5] &= 0xFE; // off: I mode
}
// *************************
@@ -507,7 +605,6 @@
hid.send(&send_report); // send command to USB HID host
Thread::wait(5);
- //lcd_out( 0, 1, "**********" ); // Handy Only
}
}
@@ -526,6 +623,18 @@
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);
+}
// **********************************************************************
//
@@ -542,26 +651,27 @@
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
+ 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_out(0,0,"B2DebrisSurveyor");
- lcd_out(0,1,"Revast Co.,Ltd. ");
+ lcd_out2(0,0,"B2DebrisSurveyor",16);
+ lcd_out2(0,1,"Revast Co.,Ltd. ",16);
led_demo();
- lcd_out(0,0,"Rev1.00 20160218");
- lcd_out(0,1,"System Booting..");
+ 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> | 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);
@@ -571,18 +681,20 @@
Thread task1(sw_task, NULL, osPriorityNormal, 256 * 4);
// 1234567890123456
- lcd_out(0,0,"Booted up ! ");
+ 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");
}
- lcd_out(0,0,"B2DebrisSurveyor");
-
-
- Thread::wait(5);
+ // 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
@@ -596,13 +708,63 @@
// 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 ); /* リードサイズは何故か10じゃないと変になる */
+ 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_out(0,1,"System OK[-] ");
+ // 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");
@@ -618,31 +780,21 @@
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
+ // lcd_out2(0,0,"System OK[|] ",16); // command
}
else if( buf[5] == 0x04 ){
- lcd_out(0,1,"ERROR ");
+ // lcd_out2(0,0,"ERROR ",16);
}
else{
- lcd_out(0,1,"System OK[/] "); // unknown command
+ // 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 >= 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;
- */
+ if( counter >= 100 ){
+ // led1 = !led1;
counter = 0;
}
Thread::wait(5);