z ysaito
2018.07.26
Dependencies: TextLCD USBDevice mbed-rtos mbed
Revision 2:d0a5ee4e7bb8, committed 2018-07-26
- Comitter:
- sayzyas
- Date:
- Thu Jul 26 00:20:57 2018 +0000
- Parent:
- 1:368ba89c2e6b
- Commit message:
- 2018.07.26
Changed in this revision
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);
diff -r 368ba89c2e6b -r d0a5ee4e7bb8 USBDevice.lib --- a/USBDevice.lib Mon Mar 28 00:09:18 2016 +0000 +++ b/USBDevice.lib Thu Jul 26 00:20:57 2018 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/USBDevice/#2af474687369 +http://mbed.org/users/mbed_official/code/USBDevice/#53949e6131f6
diff -r 368ba89c2e6b -r d0a5ee4e7bb8 common.h
--- a/common.h Mon Mar 28 00:09:18 2016 +0000
+++ b/common.h Thu Jul 26 00:20:57 2018 +0000
@@ -1,17 +1,54 @@
-
+/*
+ DEBUG PRINT MACRO
+ http://qiita.com/saltheads/items/e1b0ab54d3d6029c9593
+ http://tricky-code.net/nicecode/code10.php
+*/
/* Information */
-#define LatestUpDate "2016.02.17"
-#define ProgramRevision "Rev. 0.95"
-#define Author "Y.Saito"
+#define LatestUpDate "2016.11.04"
+#define ProgramRevision "Rev 2.40"
+#define Author "ZisNotRevast"
#define Company "Revast Co.,Ltd."
-#define Targetmbed "mbed LPC1768"
+
+//#define __CREATE_SETTING_FILE__
+//#define __TARGET_BOARD_CHECK__
+
+
+//#define __WInchDebug__ // For debugging
+
+
-/* Debug macro */
+// Read motor current
+#define __READ_TFM_MOTOR_CURRENT__
+
+#define __IIC_COMAMND_SEND__
+
+#define __SET_WINCH_MOTOR_SPEED_BY_VOLUME__
+
+// ======================================================================
+// For Debugging
+// ======================================================================
+//#define __DEBUG_PRINT_SW__ // Display SW Status to console
#define __DEBUG_L0__
-#define __DEBUG_L1__
-#define __DEBUG_L2__
-//#define __DEBUG_L3__
+//#define __DEBUG_L1__
+//#define __DEBUG_L2__
+#define __DEBUG_L3__
//#define __DEBUG_L4__
+//#define __DEBUG_L5__
+
+#define __DEBUG_WINCH_DATA__
+
+
+#ifdef __DEBUG_WINCH_DATA__
+ #define DEBUG_PRINT_WINCH_DATA(...) pc.printf(__VA_ARGS__)
+#else
+ #define DEBUG_PRINT_WINCH_DATA(...)
+#endif
+
+#ifdef __DEBUG_PRINT_SW__
+ #define DEBUG_PRINT_SW(...) pc.printf(__VA_ARGS__)
+#else
+ #define DEBUG_PRINT_SW(...)
+#endif
#ifdef __DEBUG_L0__
#define DEBUG_PRINT_L0(...) pc.printf(__VA_ARGS__)
@@ -38,24 +75,281 @@
#else
#define DEBUG_PRINT_L4(...)
#endif
+#ifdef __DEBUG_L5__
+ #define DEBUG_PRINT_L5(...) pc.printf(__VA_ARGS__)
+#else
+ #define DEBUG_PRINT_L5(...)
+#endif
-/* ***************** */
-/* Target definition */
-/* ***************** */
-#define i2c_addr_handy (0x50); // Handy Controller
-#define i2c_addr_winch (0x40); // Ctrl Board1 : Winch
-#define i2c_addr_tfansform (0x30); // Ctrl Board2 : Transform
-#define i2c_addr_crawler (0x20); // Ctrl Board3 : Crawler
+/* ********************************************* */
+
+// Motor Controller RoboCAN ID definition
+#define MCTR_CANID_PANTILTWCH 2 // Pan/Tilt or Winch motor's ID
+#define MCTR_CANID_TFM 3 // Trabsform motor's ID
+#define MCTR_CANID_CRW 1 // Crawler motor's ID
+
+#define NUMBER_OF_MCCMD 7
+#define WINCH_OFFSET_VALUE 20
+
+/*
+ B1/B2 demo machine controller control command list
+*/
+enum{
+ XX_WICH, // Winch
+ XX_CLRF, // R Crawler
+ XX_CLLB, // L Crawler
+ XX_TFRF, // R Transform
+ XX_TFLB, // R Transform
+ ZTRF_I_1, // RF-I Transform ON
+ ZTRF_I_0, // RF-I Transform OFF
+ ZTRF_K_1, // RF-K Transform ON
+ ZTRF_K_0, // RF-K Transform OFF
+ ZTLB_I_1, // LB-I Transform ON
+ ZTLB_I_0, // LB-I Transform OFF
+ ZTLB_K_1, // LB-K Transform ON
+ ZTLB_K_0, // LB-K Transform OFF
+ ZCP_FW_1, // Camera PAN Forward ON
+ ZCP_FW_0, // Camera PAN Forward OFF
+ ZCP_RS_1, // Camera PAN Reverse ON
+ ZCP_RS_0, // Camera PAN Reverse OFF
+ ZCT_FW_1, // Camera TILT Forward ON
+ ZCT_FW_0, // Camera TILT Forward OFF
+ ZCT_RS_1, // Camera TILT Reverse ON
+ ZCT_RS_0, // Camera TILT Reverse OFF
+ ZESHAPE_I, // Shape I : Switch ON
+ ZESHAPE_K, // Shape K : Switch OFF
+ ZTFCWP_VD, // TFM,CRW Part valid : Switch ON
+ ZWICHP_VD // Winch Part valid : Switch OFF
+};
+
+
+enum{
+ MOTOR_NO0,
+ MOTOR_NO1,
+ MOTOR_NO2,
+ MOTOR_NO3
+};
+
+enum{
+ MOTOR_FORWARD,
+ MOTOR_REVERSE,
+ MOTOR_STOP
+};
+
+#define MC_LOCK_COUNT 5
+
+/* ********************************************* */
+
+#define MOTOR_1 '1'
+#define MOTOR_2 '2'
+
+#define MOTOR_
+
+/* Above is still under construction ..... */
+
+#define __DISP_GAMAPAD_STATUS_ALL__
+#define __DISP_WRITE_VALUE__
+
+#define I2C_ADDRESS_HANDY 0x20
+#define I2C_ADDRESS_WINCH 0x10
+#define I2C_ADDRESS_TRANSFORM 0x08
+#define I2C_ADDRESS_CRAWLER 0x04
+#define I2C_ADDRESS_RESOLVER 0x02
+
+
+#define _OK_ 1
+#define _NG_ 0
+#define _FAIL_ 0
+
+#define ON 1
+#define OFF 0
+
+#define LFS_READ_COUNT 3
+#define TARGET_CHECK_COUNT 10
+
+//#define __DSP_MOTOR_CURRENT_
+
+/* Usually this should be comment out */
+//#define _COMMUNCATE_PC_BY_SERIAL_
+
+// Should validate this definition when you use DHCP.
+//#define __ETERNET_DHCP__
+
+#define NumberOfPcCommand 11
+#define NumberOfI2CCommand 14
+
+/* I2C Command packet to motor controller */
+/* For motor controller */
+enum{
+ I2C_CP_PREAMBLE, // Preamble of command packet
+ I2C_CP_COMMAND, // instruction command
+ I2C_CP_M1_DIR, // motor1 rotation direction
+ I2C_CP_M1_SPEED, // motor1 rotation speed
+ I2C_CP_M1_FWD_CNTTH_U, // motor1 current limit detection threshold upper byte
+ I2C_CP_M1_FWD_CNTTH_L, // motor1 current limit detection threshold lower byte
+ I2C_CP_M1_RVS_CNTTH_U, // motor1 current limit detection threshold upper byte
+ I2C_CP_M1_RVS_CNTTH_L, // motor1 current limit detection threshold lower byte
+ I2C_CP_M2_DIR, // motor2 rotation direction
+ I2C_CP_M2_SPEED, // motor2 rotation speed
+ I2C_CP_M2_FWD_CNTTH_U, // motor2 current limit detection threshold upper byte
+ I2C_CP_M2_FWD_CNTTH_L, // motor2 current limit detection threshold lower byte
+ I2C_CP_M2_RVS_CNTTH_U, // motor2 current limit detection threshold upper byte
+ I2C_CP_M2_RVS_CNTTH_L, // motor2 current limit detection threshold lower byte
+};
+/* For resolver reader controller */
+enum{
+ I2C_CP_PREAMBLE_R, // Preamble of command packet
+ I2C_CP_COMMAND_R, // instruction command
+ I2C_CP_WDRAM_DIA_UPPER, // motor1 rotation direction
+ I2C_CP_WDRAM_DIA_LOWER, // motor1 rotation speed
+ I2C_CP_CCABLE_DIA_UPPER, // motor1 current limit detection threshold upper byte
+ I2C_CP_CCABLE_DIA_LOWER, // motor1 current limit detection threshold lower byte
+ I2C_CP_RESOLVER_RESO, // motor1 current limit detection threshold upper byte
+ I2C_CP_PRESET_CPOS_UPPER, // reserved
+ I2C_CP_PRESET_CPOS_LOWER, // reserved
+ I2C_CP_RES3, // reserved
+ I2C_CP_RES4, // reserved
+ I2C_CP_RES5, // reserved
+ I2C_CP_RES6, // reserved
+ I2C_CP_RES7, // reserved
+};
-#define Target_IIC_ADDR i2c_addr_handy // For Winch motor controller
-//#define Target_IIC_ADDR i2c_addr_tfansform // For Transform controller
-//#define Target_IIC_ADDR i2c_addr_crawler // For Crawler motor controller
+/* Winch Operating mode */
+enum{
+ WINCH_POSITION_CLEAR,
+ WINCH_PRESET_BASEDATA,
+ WINCH_MMODE_RELATIVE,
+ WINCH_MMODE_ABSOLUTE,
+ WINCH_STEPDOWN_BTN_ON,
+ WINCH_STEPDOWN_BTN_OFF,
+ WINCH_STEPUP_BTN_ON,
+ WINCH_STEPUP_BTN_OFF,
+ WINCH_U_STEPDOWN_BTN_ON,
+ WINCH_U_STEPDOWN_BTN_OFF,
+ WINCH_U_STEPUP_BTN_ON,
+ WINCH_U_STEPUP_BTN_OFF,
+ WINCH_PRESET_POSITION,
+};
+
+/* Ether net */
+#define TCP_PCCSERVER_PORT 10004
+#define TCP_CMDSERVER_PORT 10002
+#define UDP_SERVER_PORT 10000
+
+#define MOTOR_FWD 'F' /* Forward Rotation */
+#define MOTOR_RVS 'R' /* Reverse Rotation */
+#define MOTOR_STP 'S' /* Stop */
-#define NumberOfI2CCommand 10
+// =========================================================
+// Setting Values
+// =========================================================
+typedef struct {
+ int8_t sv_JS_ShapeMode; // JS Shape: 0=I, 1=KO
+ int8_t sv_JS_OpeMode; // JS Operation: 0=single, 1=dual
+ int8_t sv_WinchValid; // Winch function valid or not: 0=Tfm, Crawler valid, 1=Winch valid
+ int8_t res;
+} basic_operation_t;
+
+
+
+
+typedef struct {
+ uint16_t drm_mtr_ithd_f; // 2 Current threshold winch dram Motor ( forward )
+ uint16_t drm_mtr_ithd_r; // 2 Current threshold winch dram Motor ( reverse )
+ uint16_t tmp_mtr_ithd_f; // 2 Current threshold winch No2 Motor ( forward )
+ uint16_t tmp_mtr_ithd_r; // 2 Current threshold winch No2 Motor ( reverse )
+ uint8_t drm_mtr_hspd_f; // 1 Motor speed winch dram motor high speed ( forward )
+ uint8_t drm_mtr_hspd_r; // 1 Motor speed winch dram motor high speed ( reverse )
+ uint8_t drm_mtr_lspd_f; // 1 Motor speed winch dram motor low speed ( forward )
+ uint8_t drm_mtr_lspd_r; // 1 Motor speed winch dram motor low speed ( reverse )
+ uint8_t tmp_mtr_hspd_f; // 1 Motor speed winch No2 motor high speed ( forward )
+ uint8_t tmp_mtr_hspd_r; // 1 Motor speed winch No2 motor high speed ( reverse )
+ uint8_t tmp_mtr_lspd_f; // 1 Motor speed winch No2 motor low speed ( forward )
+ uint8_t tmp_mtr_lspd_r; // 1 Motor speed winch No2 motor low speed ( reverse )
+ uint16_t dram_dmtr_x100; // 2 Winch dram diameter x 100
+ uint16_t adj_val_x10000; // 2 Winch adjust value x 100
+ uint8_t res_resolution; // 1 Winch resolver resolution (bit)
+ uint8_t reserved_1; // 1 reserved for future use
+ uint8_t reserved_2; // 1 reserved for future use
+ uint8_t reserved_3; // 1 reserved for future use
+} winch_SetValue_t; // << 24 byte >>
-#define DEAD_ZONE_BAND_WIDTH 5
-#define CALIBRATION_COUNT 300
+typedef struct {
+ uint16_t rf_mtr_ithd_f; // 2 Current threshold RF motor ( forward )
+ uint16_t rf_mtr_ithd_r; // 2 Current threshold RF motor ( reverse )
+ uint16_t lb_mtr_ithd_f; // 2 Current threshold LB motor ( forward )
+ uint16_t lb_mtr_ithd_r; // 2 Current threshold LB motor ( reverse )
+ uint8_t rf_mtr_hspd_f; // 1 Motor speed RF ( forward )
+ uint8_t rf_mtr_hspd_r; // 1 Motor speed RF ( reverse )
+ uint8_t lb_mtr_hspd_f; // 1 Motor speed LB ( forward )
+ uint8_t lb_mtr_hspd_r; // 1 Motor speed LB ( reverse )
+ uint8_t reserved_1; // 1 reserved for future use
+ uint8_t reserved_2; // 1 reserved for future use
+ uint8_t reserved_3; // 1 reserved for future use
+ uint8_t reserved_4; // 1 reserved for future use
+} tfm_SetValue_t; // << 16 byte >>
+
+typedef struct {
+ uint16_t rf_mtr_ithd_f; // 2 Current threshold RF motor ( forward )
+ uint16_t rf_mtr_ithd_r; // 2 Current threshold RF motor ( reverse )
+ uint16_t lb_mtr_ithd_f; // 2 Current threshold LB motor ( forward )
+ uint16_t lb_mtr_ithd_r; // 2 Current threshold LB motor ( reverse )
+ uint8_t rf_mtr_hspd_f; // 1 Motor speed RF ( forward )
+ uint8_t rf_mtr_hspd_r; // 1 Motor speed RF ( reverse )
+ uint8_t lb_mtr_hspd_f; // 1 Motor speed LB ( forward )
+ uint8_t lb_mtr_hspd_r; // 1 Motor speed LB ( reverse )
+ uint8_t reserved_1; // 1 reserved for future use
+ uint8_t reserved_2; // 1 reserved for future use
+ uint8_t reserved_3; // 1 reserved for future use
+ uint8_t reserved_4; // 1 reserved for future use
+} crawler_SetValue_t; // << 16 byte >>
-#define MOTOR_FWD_ROTATION 0
-#define MOTOR_RVS_ROTATION 1
+typedef struct {
+ uint8_t rjs_centervalue; // 1 R JoyStick center value
+ uint8_t rjs_u_dead_zone; // 1 R Joystick upper dead zone width
+ uint8_t rjs_l_dead_zone; // 1 R Joystick lower dead zone width
+ uint8_t ljs_centervalue; // 1 L JoyStick center value
+ uint8_t ljs_u_dead_zone; // 1 L Joystick upper dead zone width
+ uint8_t ljs_l_dead_zone; // 1 L Joystick lower dead zone width
+ uint8_t reserved_1; // 1 reserved for future use
+ uint8_t reserved_2; // 1 reserved for future use
+} js_SetValue_t; // << 8 byte >>
+
+typedef struct SetValue {
+ winch_SetValue_t winchCtrl;
+ tfm_SetValue_t tfmCtrl;
+ crawler_SetValue_t crawlerCtrl;
+ js_SetValue_t jsCtrl;
+} setValue_t;
+
+
+
+#define SLOWDOWN_DISTANCE 23 // <--10 2016.06.01
+#define SLOWDOWN_NEAR_DISTANCE 3
+/*
+typedef struct {
+ int16_t dt_WinchCntPosition; // Current winch posittion
+ int16_t dt_WinchRtvValue; // Winch Moving distance
+ int16_t dt_WinchDstPosition; // Destination Position
+ char operation;
+ int8_t dt_WinchMotorCurrent; // Winch motor current
+} winchData_t;
+*/
+
+typedef struct {
+ int16_t dt_WinchCntPosition; // Current winch posittion
+ int16_t dt_WinchRtvValue; // Winch Moving distance
+ int16_t dt_WinchDstPosition; // Destination Position
+ uint8_t operation;
+ uint8_t dt_WinchMotor1Current; // Winch motor 1 current
+ uint8_t dt_WinchMotor2Current; // Winch motor 2 current
+ uint8_t res;
+} winchData_t;
+
+
+#define ROTATE_PER_RESOLUTION 24
+
+#define __READ_CURRENT_AT_CIF_TASK__
+
+//mv_WinchMvData
diff -r 368ba89c2e6b -r d0a5ee4e7bb8 mbed-rtos.lib --- a/mbed-rtos.lib Mon Mar 28 00:09:18 2016 +0000 +++ b/mbed-rtos.lib Thu Jul 26 00:20:57 2018 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed-rtos/#3d9d2b8b8f17 +http://mbed.org/users/mbed_official/code/mbed-rtos/#5713cbbdb706
diff -r 368ba89c2e6b -r d0a5ee4e7bb8 mbed.bld --- a/mbed.bld Mon Mar 28 00:09:18 2016 +0000 +++ b/mbed.bld Thu Jul 26 00:20:57 2018 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/6f327212ef96 \ No newline at end of file +https://os.mbed.com/users/mbed_official/code/mbed/builds/e7ca05fa8600 \ No newline at end of file