z ysaito
2018.07.26
Dependencies: EthernetInterface TextLCD USBDevice USBHost2 mbed
Diff: P1_2Y_handy.cpp
- Revision:
- 0:2cb1be240f50
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/P1_2Y_handy.cpp Thu Jul 26 00:19:33 2018 +0000
@@ -0,0 +1,2032 @@
+/*
+////////////////////////////////////////////////////////////////////////////
+ Project: B1/B2 DebrisProbe Demonstration model
+ Title: Debris Prober demo HandyCtrl Main
+ Target: mbed LPC1768
+ Data: 2017.06.30
+ Author: sayzyas as ZNR
+ ------------------------------------------------
+ The Final Project of this company ! .... may be.
+ ------------------------------------------------
+
+ ## ##### ## #####
+ ## ## ## ## ## ##
+ ## ## ## ##### #####
+ ## ## ## ## ## ##
+ ## ## ##### #####
+
+ mbed LPC1768
+ +-------------USB-----------+
+ | GND VOUT(3.3V) |
+ | VIN VU(5.0V OUT)|
+ | VB IF- |
+ | mR IF+ |
+ | p5 mosi Ether RD- |
+ | p6 miso Ether RD+ |
+ | p7 sck Ether TD- |
+ | p8 Ether TD+ |
+ | p9 tx sdi USB D- |
+ | p10 rx scl USB D+ |
+ | p11 mosi CAN rd p30 |
+ | p12 miso CAN td p29 |
+ | p13 tx sck sda tx p28 |
+ | p14 rx scl rx P27 |
+ | p15 AIn PWM P26 |
+ | p16 AIn PWM P25 |
+ | p17 AIn PWM p24 |
+ | p18 AIn AOut PWM p23 |
+ | p19 AIn PWM p22 |
+ | p20 AIn PWM p21 |
+ +---------------------------+
+
+////////////////////////////////////////////////////////////////////////////
+ */
+
+#include "mbed.h"
+#include "USBHostGamepad.h"
+#include "USBSerial.h"
+#include "rtos.h"
+#include "EthernetInterface.h"
+#include "common.h"
+#include "stdio.h"
+//#include "TextLCD.h"
+#include "com_func.h"
+#include "USBHID.h"
+#include "TextLCD.h"
+
+
+// USBSerial serial setting
+Serial pc(USBTX, USBRX); // UART
+
+// Digital I/O setting
+DigitalOut led1(LED1); // 1:on,0:off System is OK then ON.
+DigitalOut led2(LED2); // 1:on,0:off GamePad is connected.
+DigitalOut led3(LED3); // 1:on,0:off When got the GamePas switch input then ON
+DigitalOut led4(LED4); // 1:on,0:off Access indicator with PC
+
+// LED indicator
+DigitalOut led_ind_ptwc(p14); // LED indicator pan/tilt, winch, crawler limit
+DigitalOut led_ind_tfm(p15); // LED indicator tfm motor current limit
+DigitalOut led_valid_part1(p25); // LED transform, crawler valid
+DigitalOut led_valid_part2(p26); // LED pan/tilt, winch valid
+
+int16_t led_ind_cnt = 0;
+int16_t led_ind_cnt_t = 0;
+
+// 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_JS_ShapeMode(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 Rjsin_UD(p16); // Crawler R-UD
+AnalogIn Rjsin_LR(p17); // Crawler R-LR
+//AnalogIn Ljsin_UD(p19); // Crawler L-UD
+//AnalogIn Ljsin_LR(p20); // Crawler L-LR
+
+const char* TCP_SERVER_ADDRESS = "192.168.0.24";
+// Ethernet
+EthernetInterface eth;
+// TCP
+TCPSocketConnection tsocket; // TCP client
+// UDP
+UDPSocket usocket; // UDP client
+Endpoint client;
+
+// Local File System
+LocalFileSystem local("local"); // Create the local filesystem under the name "local"
+
+TextLCD lcd(p11, p12, p24, p23, p22, p21); // rs, e, d4-d7
+
+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;
+
+// Global
+uint32_t flg_gamePad_Connected = 0;
+char PC_cmd[11+1] = "&0100000000";
+basic_operation_t baseOperation;
+char dbuffer[128];
+
+// Global Parameter of setting
+setValue_t setValue; // Setting Data
+
+/* Status flag */
+/*
+ 0000 0000 : button LI LK RI RK PCW PCCW TU TD
+ 0000 0000 : limit switch
+ 0000 0000 : res
+ 0000 0000 : res
+*/
+uint32_t flg_exp_status = 0;
+Mutex lcdMutex;
+Mutex flg_mutex;
+int swbtn_Opeflg = 0;
+
+int16_t winchCurrentPosition;
+int16_t winchOffsetValue = 0; // 2016.10.07 added
+int16_t winchDramDiameter = 5985; // 2017.01.06 added
+
+Mutex mtx_wcp;
+
+bool flg_ButtonOn = false;
+bool flg_lsw_valid = false;
+
+int flg_JS_shape_mode = 0;
+int flg_JS_ope_mode = 0;
+int motor1_current_pct;
+int motor2_current_pct;
+uint8_t limitSw_Sts = 0;
+char motorLock_sts = '\0';
+
+void led_winch_valid( int counter )
+{
+ if( counter < 25)
+ {
+ led_valid_part2 = 0;
+ }
+ else
+ {
+ led_valid_part2 = 1;
+ }
+}
+
+void led_crawler_valid( int counter )
+{
+ if( counter < 25)
+ {
+ led_valid_part2 = 1;
+ }
+ else
+ {
+ led_valid_part2 = 0;
+ }
+}
+
+void lcd_dsp( int column, int row, char* msg, int cnt)
+{
+ for( int i = 0; i < cnt; i++ )
+ {
+ lcd.locate(column+i,row);
+ lcd.putc(*msg++);
+ }
+ Thread::wait(10);
+}
+
+// ============================================================
+// Send Status to PC
+// ============================================================
+void sendStatus2PC( char *cmd, int32_t numberOfCmd ){
+ int i;
+ led4 = 1;
+ for ( i = 0; i < numberOfCmd; i++ ) {
+ pc.putc(*cmd++);
+ }
+ led4 = 0;
+}
+
+uint8_t adj_crawlerSpeed( uint8_t in )
+{
+ uint8_t out;
+/*
+ pc.printf( "#### %d ####\r\n", in );
+
+ if( ( in > 0 ) && ( in < 98 ))
+ {
+ out = (uint8_t)((float)in * 0.8);
+ }
+ else
+ {
+ out = 100;
+ }
+*/
+ out = in;
+ return out;
+}
+
+
+bool cmd_snd2serverZ(
+ char* cmd, // command send to server
+ int cmdSize
+){
+ tsocket.send(cmd, cmdSize);
+ DEBUG_PRINT_L0("Snd cmd 2 server [ %s (%d) ]\r\n", cmd, cmdSize );
+ // Clean up
+
+ return true;
+}
+
+
+bool flg_mc_tfmcrw = false;
+bool flg_mc_winch = false;
+char I2C_res[NumberOfI2CCommand+1] = "\0";
+
+// *****************************************************************
+// calibration joystick data
+// *****************************************************************
+bool calibrate_joystick( int js, uint8_t data_ud, uint8_t data_lr ){
+ uint16_t tmp;
+
+ if( js == 0 ) // R-JS
+ {
+ 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;
+
+ if
+ (
+ (((127-10) < js_center_value_r_ud )&&(js_center_value_r_ud < (127+10))) &&
+ (((127-10) < js_center_value_r_lr )&&(js_center_value_r_lr < (127+10)))
+ )
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+ else // Other
+ {
+ 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;
+
+ if
+ (
+ (((127-10) < js_center_value_l_ud )&&(js_center_value_l_ud < (127+10))) &&
+ (((127-10) < js_center_value_l_lr )&&(js_center_value_l_lr < (127+10)))
+ ) {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+
+ }
+}
+
+// LED current indicator blink control when
+// each motor is moving.
+void led_ind_run
+(
+ int led_number,
+ int count,
+ int period
+){
+ if(( count >= 0 )&&( count < period ))
+ {
+ if( led_number == 1 )
+ {
+ led_ind_ptwc = ON;
+ }
+ else
+ {
+ led_ind_tfm = ON;
+ }
+ }
+ else if(( count >= period )&&( count < period*2 ))
+ {
+ if( led_number == 1 )
+ {
+ led_ind_ptwc = OFF;
+ }
+ else
+ {
+ led_ind_tfm = OFF;
+ }
+ }
+ else
+ {
+ led_ind_ptwc = OFF;
+ led_ind_tfm = OFF;
+ }
+}
+
+
+void led_error_blink( int no, int times )
+{
+ for( int i = 0; i < times; i++ ) {
+ if( no == 1 )
+ {
+ led_ind_ptwc = 1;
+ }
+ else
+ {
+ led_ind_tfm = 1;
+ }
+ Thread::wait(50);
+ if( no == 1 )
+ {
+ led_ind_ptwc = 0;
+ }
+ else
+ {
+ led_ind_tfm = 0;
+ }
+ Thread::wait(50);
+ }
+}
+
+bool flg_B2 = false;
+bool flg_system_ok = true;
+
+// *****************************************************************
+// switch control task
+// ----------------------
+//
+// Up: center -> 255
+// Down: 002 -> center
+// Right: center -> 255
+// Left : 0 -> center
+//
+// *****************************************************************
+int16_t tmpPosition;
+void sw_task( void const *){
+
+ char msg[16] = "";
+
+ uint16_t jsr_ud_data;
+ uint16_t jsr_lr_data;
+ uint8_t jsr_ud_undata;
+ uint8_t jsr_lr_undata;
+
+ uint8_t m1_speed;
+ uint8_t m2_speed;
+ char sbuf[128] = ""; // TCP send buffer
+ char rbuf[128]; // TCP receive buffer
+ int flg_tfmcrwpart_1st = 0;
+ int flg_wchcptpart_1st = 0;
+
+ int rts;
+ bool jsjg = true;
+
+ bool led_tfm_lock = false;
+ bool led_ptwc_lock = false;
+
+ int runcount = 0;
+
+ DEBUG_PRINT_L0("Handy(Bd2)>> Start ststem initializing ...\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)>> =============================================================\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)>> Initalizing Ethernet ...\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)>> =============================================================\r\n");
+
+ const char* ip_address = "192.168.0.31";
+ const char* subnet_mask = "255.255.255.0";
+ const char* gateway = "192.168.0.0";
+
+ DEBUG_PRINT_L0("Handy(Bd2)> --------------------------------------\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)> ip address : %s\r\n", ip_address);
+ DEBUG_PRINT_L0("Handy(Bd2)> subnet mask : %s\r\n", subnet_mask);
+ DEBUG_PRINT_L0("Handy(Bd2)> default gateway: %s\r\n", gateway);
+ DEBUG_PRINT_L0("Handy(Bd2)> --------------------------------------\r\n");
+
+ char sss[24];
+
+ int ret = eth.init(
+ ip_address, // const char* ip,
+ subnet_mask, // const char* mask,
+ gateway // const char* gateway
+ );
+ if( ret == 0 ){
+
+ DEBUG_PRINT_L0("Handy(Bd2)> Eternet init ... OK\r\n");
+ ret = eth.connect(7000); // Connection time out: 7000msec
+
+ if( ret == 0 ){
+ cf_led_onoff( &led1,&led2,&led3,&led4, false, false, false, true );
+ DEBUG_PRINT_L0("Handy(Bd2)> [ IP Address : %s ]\r\n", eth.getIPAddress());
+ while( true )
+ {
+ if (tsocket.connect(TCP_SERVER_ADDRESS, TCP_SERVER_PORT) < 0) {
+ DEBUG_PRINT_L0("Handy(Bd2)> ###ERROR### : Unable to connect to (%s) on port (%d)\r\n", TCP_SERVER_ADDRESS, TCP_SERVER_PORT);
+ flg_system_ok = false;
+ wait(0.5);
+ break;
+ }
+ flg_system_ok = true;
+ sprintf(sss, "IP:%s", ip_address );
+ lcdMutex.lock();
+ lcd_dsp(0,0,"Network .. OK ",16);
+ sss[15] = ' ';
+ lcd_dsp(0,1,sss,16);
+ lcdMutex.unlock();
+ sprintf( sbuf, "Hello" );
+ DEBUG_PRINT_L0( "Handy(Bd2)> Send [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ wait_ms(5);
+ // Receive winch position from host
+ tsocket.receive( rbuf, sizeof(rbuf));
+
+ //
+ // Check target (B1 or B2) here !
+ //
+ if( !strncmp( rbuf, "B2Demo", 6 ) )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> Echo back [%s]\r\n", rbuf );
+ flg_B2 = true;
+ }
+ wait_ms(300);
+
+ DEBUG_PRINT_L1("Calibrating joystick ... ");
+ for( int i = 0; i < CALIBRATION_COUNT; i++){
+ jsr_ud_data = Rjsin_UD.read_u16();
+ jsr_lr_data = Rjsin_LR.read_u16();
+ jsr_ud_undata = (uint8_t)(jsr_ud_data >> 8);
+ jsr_lr_undata = (uint8_t)(jsr_lr_data >> 8);
+ jsjg = calibrate_joystick( 0, jsr_ud_undata, jsr_lr_undata); // Under constraction
+ }
+
+ if( jsjg == false ){
+ flg_system_ok = false;
+ wait(0.5);
+ break;
+ }
+
+ DEBUG_PRINT_L1("done\r\n");
+
+ lcdMutex.lock();
+ wait_ms(500);
+ lcd_dsp(0,0,"System Boot up ",16);
+
+ if( flg_B2 == true )
+ {
+ sprintf( msg, "B2Wch [**** mm] "); // B2
+ }
+ else
+ {
+ sprintf( msg, "------ B1 ------"); // B1
+ }
+ lcdMutex.lock();
+ lcd_dsp(0,0," ",16);
+ lcd_dsp(0,1,msg,16);
+ lcdMutex.unlock();
+
+ while(1){
+ jsr_ud_data = Rjsin_UD.read_u16();
+ jsr_lr_data = Rjsin_LR.read_u16();
+
+ jsr_ud_undata = (uint8_t)(jsr_ud_data >> 8);
+ jsr_lr_undata = (uint8_t)(jsr_lr_data >> 8);
+
+ DEBUG_PRINT_SW("Handy SW> ");
+ DEBUG_PRINT_SW("R:%03d/%3d,%03d/%3d ", jsr_ud_undata, js_center_value_r_ud, jsr_lr_undata, js_center_value_r_ud );
+ DEBUG_PRINT_SW("SW: %d %d %d ", sw1.read(), sw2.read(), sw3.read() );
+ DEBUG_PRINT_SW("JS: %d %d\r\n", sw_JS_ShapeMode.read(), sw_validpart.read() );
+
+ // **********************************************************
+ // Transform and crawler control part
+ // **********************************************************
+ if( sw_validpart == 0 ){ // sw on
+ flg_wchcptpart_1st = 0;
+ led_valid_part2 = 0;
+ led_valid_part1 = 1;
+ if( flg_tfmcrwpart_1st == 0 )
+ {
+ sprintf( sbuf, "OX_PART_000_0000" );
+ DEBUG_PRINT_L0( "Handy(Bd2)> VALID PART [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ Thread::wait(5);
+ flg_tfmcrwpart_1st = 1;
+ }
+ else
+ {
+ // ---------------------------------------------
+ // Right JoyStick Control
+ // ---------------------------------------------
+ if( sw_JS_ShapeMode == 1 ){ // OFF: KO-Shape
+
+ if( // Dead zone
+ ( jsr_ud_undata >= (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
+ ( jsr_ud_undata <= (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
+ ( jsr_lr_undata >= (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
+ ( jsr_lr_undata <= (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
+ ){
+ sprintf( sbuf, "XX_CLRF_002_0000" ); // M1 Stop
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ sprintf( sbuf, "XX_CLLB_002_0000" ); // M2 Stop
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ lcdMutex.lock();
+ if( runcount < 15 )
+ {
+ lcd_dsp(0,0,"OX [^] ",16);
+ }
+ else if ( runcount < 30 )
+ {
+ lcd_dsp(0,0,"OX [>] ",16);
+ }
+ else if(runcount < 45 )
+ {
+ lcd_dsp(0,0,"OX [v] ",16);
+ }
+ else{
+ lcd_dsp(0,0,"OX [<] ",16);
+ }
+ if( runcount >= 60 )
+ {
+ runcount = 0;
+ }
+ else
+ {
+ runcount++;
+ }
+ lcdMutex.unlock();
+ // Receive winch position from host
+ // rts = tsocket.receive( rbuf, sizeof(rbuf));
+ // if( rts < 0 ) break;
+ led4 = 0;
+ led_ind_ptwc = 0;
+ led_ind_cnt = 0;
+ Thread::wait(15); // Don't delete, important!
+ }
+ else if( // Forward zone
+ ( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
+ ( jsr_lr_undata > (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
+ ( jsr_lr_undata < (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
+ ){
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ led4 = ON;
+ m1_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
+ m2_speed = m1_speed;
+ sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ DEBUG_PRINT_L2("Handy> R-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
+ sprintf(msg, "OX Cr-F:%03d", (255 - jsr_ud_undata));
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+
+ /*
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
+ led_error_blink( 1, 4 );
+ }
+ */
+ led4 = OFF;
+ Thread::wait(15); // Don't delete, important!
+ }
+ else if( // Reverse zone
+ ( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
+ ( jsr_lr_undata > (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
+ ( jsr_lr_undata < (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
+ ){
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ led4 = ON;
+ m1_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
+ m2_speed = m1_speed;
+ sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ DEBUG_PRINT_L2("Handy> R-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
+ sprintf(msg, "OX Cr-R:%03d", (255 - jsr_ud_undata));
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+ /*
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
+ led_error_blink( 1, 4 );
+ }
+ */
+ led4 = OFF;
+ Thread::wait(15); // Don't delete, important!
+ }
+
+ else if( // Right turn zone
+ ( jsr_lr_undata > (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone)) &&
+ ( jsr_ud_undata > (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
+ ( jsr_ud_undata < (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone))
+ ){
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ led4 = ON;
+ m1_speed = ( jsr_lr_undata - js_center_value_r_lr ) * 100 / js_center_value_r_lr; // Speed ratio
+ m2_speed = m1_speed; // Speed
+ sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ DEBUG_PRINT_L2("Handy> R-JS Right: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_lr_undata, send_report.data[2], js_center_value_r_ud);
+ sprintf(msg,"OX Cr->:%03d", jsr_lr_undata);
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+ /*
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
+ led_error_blink( 1, 4 );
+ }
+ */
+ led4 = OFF;
+ Thread::wait(15); // Don't delete, important!
+ }
+ else if( // Left turn zone
+ ( jsr_lr_undata < (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
+ ( jsr_ud_undata > (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
+ ( jsr_ud_undata < (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone))
+ ){
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ led4 = ON;
+ m1_speed = ( js_center_value_r_lr - jsr_lr_undata ) * 100 / js_center_value_r_lr; // Speed ratio
+ m2_speed = m1_speed;
+ sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ DEBUG_PRINT_L2("Handy> R-JS Left: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_lr_undata, send_report.data[2], js_center_value_r_ud);
+ sprintf(msg,"OX Cr<-:%03d", jsr_lr_undata);
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+ /*
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
+ led_error_blink( 1, 4 );
+ }
+ */
+ led4 = OFF;
+ Thread::wait(15); // Don't delete, important!
+ }
+
+ else if( // Right-upper zone
+ ( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
+ ( jsr_lr_undata > (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
+ ){
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ led4 = ON;
+ if( jsr_ud_undata > jsr_lr_undata )
+ {
+ m2_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
+ m1_speed = 100 - (( jsr_lr_undata - js_center_value_r_lr ) * 100 / js_center_value_r_lr);
+ sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ /*
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
+ led_error_blink( 1, 4 );
+ }
+ */
+ }
+ DEBUG_PRINT_L2("Handy> R-JS R-U zone: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
+ sprintf(msg, "OX Cr-FRZ ");
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+ led4 = OFF;
+ Thread::wait(15); // Don't delete, important!
+ }
+
+ else if( // Left-upper zone
+ ( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
+ ( jsr_lr_undata < (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone))
+ ){
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ led4 = ON;
+ if( (jsr_ud_undata - js_center_value_r_ud) > (js_center_value_r_ud-jsr_lr_undata) )
+ {
+ m1_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
+ m2_speed = 100 - (( js_center_value_r_lr - jsr_lr_undata ) * 100 / js_center_value_r_lr);
+ sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ /*
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
+ led_error_blink( 1, 4 );
+ }
+ */
+ }
+ DEBUG_PRINT_L2("Handy> R-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
+ sprintf(msg, "OX Cr-FLZ ");
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+ led4 = OFF;
+ Thread::wait(15); // Don't delete, important!
+ }
+
+ else if( // Right-lower zone
+ ( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
+ ( jsr_lr_undata > (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
+ ){
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ led4 = ON;
+ if( (js_center_value_r_ud - jsr_ud_undata) > (jsr_lr_undata - js_center_value_r_lr) )
+ {
+ m2_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
+ m1_speed = 100 - (( jsr_lr_undata - js_center_value_r_lr ) * 100 / js_center_value_r_lr);
+ sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ /*
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
+ led_error_blink( 1, 4 );
+ }
+ */
+ }
+ DEBUG_PRINT_L2("Handy> R-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
+ sprintf(msg, "OX Cr-RRZ ");
+ //sprintf(msg, "OX C BK RT ");
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+ led4 = OFF;
+ Thread::wait(15); // Don't delete, important!
+ }
+
+ else if( // Left-lower zone
+ ( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
+ ( jsr_lr_undata < (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone))
+ ){
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ led4 = ON;
+ if( (js_center_value_r_ud - jsr_ud_undata) > ( js_center_value_r_lr - jsr_lr_undata) )
+ {
+ m1_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
+ m2_speed = 100 - (( js_center_value_r_lr - jsr_lr_undata ) * 100 / js_center_value_r_lr);
+ sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+
+ if( rts < 0 ) break;
+ sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ /*
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
+ led_error_blink( 1, 4 );
+ }
+ */
+ }
+ DEBUG_PRINT_L2("Handy> R-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
+ sprintf(msg, "OX Cr-RLZ ");
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+ led4 = OFF;
+ Thread::wait(15); // Don't delete, important!
+ }
+ }
+ else // ON: I-Shape
+ {
+ // Right Joystic < Down
+ if( jsr_ud_undata > (js_center_value_r_ud + + setValue.jsCtrl.rjs_upper_dzone) ){
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ led4 = ON;
+ m1_speed = adj_crawlerSpeed(( js_center_value_r_ud+1 - (255 - jsr_ud_undata) ) * 100 / js_center_value_r_ud ); // Speed
+ m2_speed = m1_speed; // Speed
+
+ sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ DEBUG_PRINT_L2("Handy> R-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
+ sprintf(msg, "OX C FW:%03d", (255 - jsr_ud_undata));
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+ /*
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
+ led_error_blink( 1, 4 );
+ }
+ */
+ led4 = OFF;
+ Thread::wait(15); // Don't delete, important!
+ }
+ // Right Joystick < Up >
+ else if( jsr_ud_undata < (js_center_value_r_ud - + setValue.jsCtrl.rjs_upper_dzone) ){
+ // Crawler reverse Run
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ led4 = ON;
+ m1_speed = adj_crawlerSpeed(( (255 - jsr_ud_undata) - js_center_value_r_ud ) * 100 / js_center_value_r_ud ); // Speed
+ m2_speed = m1_speed; // Speed
+ sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ DEBUG_PRINT_L2("Handy> R-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
+ sprintf(msg, "OX C BK:%03d", (255 - jsr_ud_undata));
+
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+ /*
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
+ led_error_blink( 1, 4 );
+ }
+ */
+ led4 = OFF;
+ Thread::wait(15); // Don't delete, important!
+ }
+ else{
+ sprintf( sbuf, "XX_CLRF_002_0000" ); // M1 Stop
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ sprintf( sbuf, "XX_CLLB_002_0000" ); // M2 Stop
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ /*
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+
+ lcdMutex.lock();
+ lcd_dsp(0,0,"OX ",11);
+ lcdMutex.unlock();
+ */
+ led4 = OFF;
+ led_ind_ptwc = OFF;
+ }
+ }
+ // ---------------------------------------------
+ // Other Switch Control: Pass
+ // ---------------------------------------------
+ if ((sw1 == 0)&&(sw2 == 1)&&(sw3==0)){ // Limit Switch valid
+ if( flg_lsw_valid == false )
+ {
+ flg_lsw_valid = true;
+ //printf("lsw valid\r\n");
+ if( flg_B2 == false ) //B1
+ {
+ lcd_dsp(0,1,"------ B1 -----*",16);
+ }
+ else
+ {
+ sprintf(msg,"B2Wch [%04d mm]*", tmpPosition);
+ lcdMutex.lock();
+ lcd_dsp(0,1,msg,16);
+ lcdMutex.unlock();
+ }
+ Thread::wait(700);
+ }
+ else
+ {
+ flg_lsw_valid = false;
+ //printf("lsw invalid\r\n");
+ if( flg_B2 == false ) //B1
+ {
+ lcd_dsp(0,1,"------ B1 ------",16);
+ }
+ else
+ {
+ sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
+ lcdMutex.lock();
+ lcd_dsp(0,1,msg,16);
+ lcdMutex.unlock();
+ }
+ Thread::wait(700);
+ }
+ }
+ else if ((sw1 == 0)&&(sw2 == 1)&&(sw3==1)){ // RF transform K
+ led_ind_run(0, led_ind_cnt_t, LED_IND_BLINK_PERIOD2);
+ if( led_ind_cnt_t > LED_IND_BLINK_PERIOD2*2 )
+ {
+ led_ind_cnt_t = 0;
+ }
+ else
+ {
+ led_ind_cnt_t += 1;
+ }
+ if( flg_lsw_valid == true )
+ {
+ sprintf( sbuf, "XX_TFRF_100_0100" );
+ }
+ else
+ {
+ sprintf( sbuf, "XX_TFRF_000_0100" );
+ }
+
+ sprintf(msg, "OX Tfm-RFK ");
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L5( "Handy(Bd2)> ### TRANSFORM LOCK DETECTION ###\r\n" );
+ led_tfm_lock = true;
+ }
+ if( led_tfm_lock == true )
+ {
+ led_error_blink( 0, 4 );
+ }
+ led3 = 1;
+ Thread::wait(15);
+ }
+ else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==1)){ // RF transform I
+ led_ind_run(0, led_ind_cnt_t, LED_IND_BLINK_PERIOD2);
+ if( led_ind_cnt_t > LED_IND_BLINK_PERIOD2*2 )
+ {
+ led_ind_cnt_t = 0;
+ }
+ else
+ {
+ led_ind_cnt_t += 1;
+ }
+ if( flg_lsw_valid == true )
+ {
+ sprintf( sbuf, "XX_TFRF_101_0100" );
+ }
+ else
+ {
+ sprintf( sbuf, "XX_TFRF_001_0100" );
+ }
+
+ sprintf(msg, "OX Tfm-RFI ");
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L5( "Handy(Bd2)> ### TRANSFORM LOCK DETECTION ###\r\n" );
+ led_tfm_lock = true;
+ }
+ if( led_tfm_lock == true )
+ {
+ led_error_blink( 0, 4 );
+ }
+ led3 = 1;
+ Thread::wait(15);
+ }
+ else if ((sw1 == 0)&&(sw2 == 0)&&(sw3==1)){ // LB transform K
+ led_ind_run(0, led_ind_cnt_t, LED_IND_BLINK_PERIOD2);
+ if( led_ind_cnt_t > LED_IND_BLINK_PERIOD2*2 )
+ {
+ led_ind_cnt_t = 0;
+ }
+ else
+ {
+ led_ind_cnt_t += 1;
+ }
+ if( flg_lsw_valid == true )
+ {
+ sprintf( sbuf, "XX_TFLB_100_0100" );
+ }
+ else
+ {
+ sprintf( sbuf, "XX_TFLB_000_0100" );
+ }
+
+ sprintf(msg, "OX Tfm-LBK ");
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L5( "Handy(Bd2)> ### TRANSFORM LOCK DETECTION ###\r\n" );
+ led_tfm_lock = true;
+ }
+ if( led_tfm_lock == true )
+ {
+ led_error_blink( 0, 4 );
+ }
+ led3 = 1;
+ Thread::wait(15);
+ }
+ else if ((sw1 == 1)&&(sw2 == 1)&&(sw3==0)){ // LB transform I
+ led_ind_run(0, led_ind_cnt_t, LED_IND_BLINK_PERIOD2);
+ if( led_ind_cnt_t > LED_IND_BLINK_PERIOD2*2 )
+ {
+ led_ind_cnt_t = 0;
+ }
+ else
+ {
+ led_ind_cnt_t += 1;
+ }
+ if( flg_lsw_valid == true )
+ {
+ sprintf( sbuf, "XX_TFLB_101_0100" );
+ }
+ else
+ {
+ sprintf( sbuf, "XX_TFLB_001_0100" );
+ }
+
+ sprintf(msg, "OX Tfm-LBI ");
+ lcdMutex.lock();
+ lcd_dsp(0,0,msg,11);
+ lcdMutex.unlock();
+
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L5( "Handy(Bd2)> ### TRANSFORM LOCK DETECTION ###\r\n" );
+ led_tfm_lock = true;
+ }
+ if( led_tfm_lock == true )
+ {
+ led_error_blink( 0, 4 );
+ }
+ led3 = 1;
+ Thread::wait(15);
+ }
+ else
+ {
+ sprintf( sbuf, "XX_TFRF_002_0000" );
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ // Receive winch position from host
+ // rts = tsocket.receive( rbuf, sizeof(rbuf));
+ // if( rts < 0 ) break;
+ sprintf( sbuf, "XX_TFLB_002_0000" );
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ // Receive winch position from host
+ // rts = tsocket.receive( rbuf, sizeof(rbuf));
+ // if( rts < 0 ) break;
+ led3 = OFF;
+ led_tfm_lock = false;
+ led_ind_tfm = OFF;
+ }
+ }
+ }
+ // **********************************************************
+ // B1: Camera pan/tile, B2: Winch control part
+ // **********************************************************
+ else{
+ flg_tfmcrwpart_1st = 0;
+ led_valid_part2 = 1;
+ led_valid_part1 = 0;
+ if( flg_wchcptpart_1st == 0 )
+ {
+ sprintf( sbuf, "XO_PART_000_0000" );
+ DEBUG_PRINT_L0( "Handy(Bd2)> VALID PART [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ Thread::wait(5);
+ flg_wchcptpart_1st = 1;
+ }
+ else
+ {
+ if((sw1 == 0)&&(sw2 == 1)&&(sw3==0)){ // Winch Position Clear
+ sprintf( sbuf, "OO_WCLR_000_0100" );
+ DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ Thread::wait(10);
+ }
+ if( // Dead zone
+ ( jsr_ud_undata >= (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
+ ( jsr_ud_undata <= (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
+ ( jsr_lr_undata >= (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
+ ( jsr_lr_undata <= (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
+ ){
+ if( flg_B2 == false ) // B1
+ {
+
+ sprintf( sbuf, "XX_CPAN_002_0000" ); // M2 Stop
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ sprintf( sbuf, "XX_CTLT_002_0000" ); // M2 Stop
+ DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ lcdMutex.lock();
+ if( runcount < 15 )
+ {
+ lcd_dsp(0,0,"XO [^] ",16);
+ }
+ else if ( runcount < 30 )
+ {
+ lcd_dsp(0,0,"XO [>] ",16);
+ }
+ else if(runcount < 45 )
+ {
+ lcd_dsp(0,0,"XO [v] ",16);
+ }
+ else{
+ lcd_dsp(0,0,"XO [<] ",16);
+ }
+ if( runcount >= 60 )
+ {
+ runcount = 0;
+ }
+ else
+ {
+ runcount++;
+ };
+ lcdMutex.unlock();
+ Thread::wait(2);
+ led_ind_ptwc = OFF;
+ }
+ else // B2
+ {
+ sprintf( sbuf, "XX_WICH_002_0000" );
+ DEBUG_PRINT_L0( "Handy(Bd2)> BTN WCH STOP [%s]\r\n", sbuf );
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ lcdMutex.lock();
+ //lcd_dsp(0,0,"XO ",11);
+
+ lcdMutex.lock();
+ if( runcount < 15 )
+ {
+ lcd_dsp(0,0,"XO [^] ",16);
+ }
+ else if ( runcount < 30 )
+ {
+ lcd_dsp(0,0,"XO [>] ",16);
+ }
+ else if(runcount < 45 )
+ {
+ lcd_dsp(0,0,"XO [v] ",16);
+ }
+ else{
+ lcd_dsp(0,0,"XO [<] ",16);
+ }
+ if( runcount >= 60 )
+ {
+ runcount = 0;
+ }
+ else
+ {
+ runcount++;
+ };
+
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ DEBUG_PRINT_L0( "Handy(Bd2)> Winch Position [%04d]\r\n", tmpPosition );
+ if( tmpPosition != 9999 )
+ {
+ if( flg_lsw_valid == true )
+ {
+ sprintf(msg,"B2Wch [%04d mm]*", tmpPosition);
+ }
+ else
+ {
+ sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
+ }
+ }
+ else
+ {
+ if( flg_lsw_valid == true )
+ {
+ sprintf(msg,"B2Wch [**** mm]*");
+ }
+ else
+ {
+ sprintf(msg,"B2Wch [**** mm] ");
+ }
+ }
+ lcdMutex.lock();
+ lcd_dsp(0,1,msg,16);
+ lcdMutex.unlock();
+ led_ind_ptwc = OFF;
+ }
+ led2 = 0;
+ led3 = 0;
+ led_ind_ptwc = 0;
+ led_ptwc_lock = false;
+ }
+ else if( // PAN zone
+ ( jsr_lr_undata >= (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone)) &&
+ ( jsr_ud_undata >= (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone*2)) &&
+ ( jsr_ud_undata <= (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone*2)) &&
+ ( flg_B2 == false )
+ ){
+ led2 = 1;
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ m1_speed = ( jsr_lr_undata - js_center_value_r_lr ) * 100 / js_center_value_r_lr; // Speed ratio
+ sprintf( sbuf, "XX_CPAN_000_%04d", m1_speed );
+ DEBUG_PRINT_L0( "Handy(Bd2)> BTN PAN CW [%s]\r\n", sbuf );
+ // Send command to host.
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CCAMERA TILT LOCK DETECTION ###\r\n" );
+ led_ptwc_lock = true;
+ }
+ else
+ {
+ lcdMutex.lock();
+ lcd_dsp(0,0,"XO PAN CW ",11);
+ lcd_dsp(0,1,"------ B1 ------",16);
+ lcdMutex.unlock();
+ }
+ if( led_ptwc_lock == true )
+ {
+ led_error_blink( 1, 4 );
+ }
+ }
+ else if( // Left turn zone
+ ( jsr_lr_undata <= (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
+ ( jsr_ud_undata >= (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone*2)) &&
+ ( jsr_ud_undata <= (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone*2)) &&
+ ( flg_B2 == false )
+ ){
+ led2 = 1;
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ m1_speed = ( js_center_value_r_lr - jsr_lr_undata ) * 100 / js_center_value_r_lr; // Speed ratio
+ sprintf( sbuf, "XX_CPAN_001_%04d", m1_speed );
+ DEBUG_PRINT_L0( "Handy(Bd2)> BTN PAN CCW[%s]\r\n", sbuf );
+ // Send command to host.
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CCAMERA TILT LOCK DETECTION ###\r\n" );
+ led_ptwc_lock = true;
+ }
+ else
+ {
+ lcdMutex.lock();
+ lcd_dsp(0,0,"XO PAN CCW ",11);
+ lcd_dsp(0,1,"------ B1 ------",16);
+ lcdMutex.unlock();
+ }
+ if( led_ptwc_lock == true )
+ {
+ led_error_blink( 1, 4 );
+ }
+ }
+ // Down
+ else if( // Down Motor forward
+ ( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
+ ( jsr_lr_undata > (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone*2)) &&
+ ( jsr_lr_undata < (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone*2)) &&
+ ( flg_B2 == false)
+ ){
+ led2 = ON;
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ m2_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
+ //m2_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
+ sprintf( sbuf, "XX_CTLT_000_%04d", m2_speed );
+ DEBUG_PRINT_L0( "Handy(Bd2)> BTN TILT CW [%s]\r\n", sbuf );
+ // Send command to host.
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CCAMERA TILT LOCK DETECTION ###\r\n" );
+ led_ptwc_lock = true;
+ }
+ else
+ {
+ lcdMutex.lock();
+ lcd_dsp(0,0,"XO TLT Down",11);
+ lcd_dsp(0,1,"------ B1 ------",16);
+ lcdMutex.unlock();
+ }
+ if( led_ptwc_lock == true )
+ {
+ led_error_blink( 1, 4 );
+ }
+ }
+ else if( // Up motor reverse
+ ( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
+ ( jsr_lr_undata > (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone*2)) &&
+ ( jsr_lr_undata < (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone*2)) &&
+ ( flg_B2 == false)
+ ){
+ led2 = 1;
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ m2_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
+ // m2_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
+ sprintf( sbuf, "XX_CTLT_001_%04d", m2_speed );
+ DEBUG_PRINT_L0( "Handy(Bd2)> BTN TLT CCW[%s]\r\n", sbuf );
+ // Send command to host.
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### CCAMERA TILT LOCK DETECTION ###\r\n" );
+ led_ptwc_lock = true;
+ }
+ else
+ {
+ lcdMutex.lock();
+ lcd_dsp(0,0,"XO TILT Up ",11);
+ lcd_dsp(0,1,"------ B1 ------",16);
+ lcdMutex.unlock();
+ }
+ if( led_ptwc_lock == true )
+ {
+ led_error_blink( 1, 4 );
+ }
+ }
+ // Down Motor forward
+ else if(
+ ( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
+
+ ( jsr_lr_undata > (js_center_value_r_lr - 25 * setValue.jsCtrl.rjs_left_dzone)) &&
+ ( jsr_lr_undata < (js_center_value_r_lr + 25 * setValue.jsCtrl.rjs_right_dzone)) &&
+ ( flg_B2 == true )
+ ){
+ led2 = ON;
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ m1_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
+ sprintf( sbuf, "XX_WICH_000_%04d", m1_speed );
+ DEBUG_PRINT_L0( "Handy(Bd2)> BTN WCH DN [%s]\r\n", sbuf );
+ // Send command to host.
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ lcdMutex.lock();
+ lcd_dsp(0,0,"XO W Down ",11);
+ lcdMutex.unlock();
+ // Receive winch position from host.
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### WINCH LOCK DETECTION ###\r\n" );
+ led_ptwc_lock = true;
+ }
+ else
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> Winch Position [%04d]\r\n", tmpPosition );
+ sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
+ if( tmpPosition != 9999 )
+ {
+ if( flg_lsw_valid == true )
+ {
+ sprintf(msg,"B2Wch [%04d mm]*", tmpPosition);
+ }
+ else
+ {
+ sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
+ }
+ }
+ else
+ {
+ if( flg_lsw_valid == true )
+ {
+ sprintf(msg,"B2Wch [**** mm]*");
+ }
+ else
+ {
+ sprintf(msg,"B2Wch [**** mm] ");
+ }
+ }
+ lcdMutex.lock();
+ lcd_dsp(0,1,msg,16);
+ lcdMutex.unlock();
+ }
+ if( led_ptwc_lock == true )
+ {
+ led_error_blink( 1, 4 );
+ }
+ }
+ // Up motor reverse
+ else if(
+ ( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
+ ( jsr_lr_undata > (js_center_value_r_lr - 25 * setValue.jsCtrl.rjs_left_dzone)) &&
+ ( jsr_lr_undata < (js_center_value_r_lr + 25 * setValue.jsCtrl.rjs_right_dzone)) &&
+ ( flg_B2 == true )
+ ){
+ led2 = 1;
+ led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
+ if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
+ {
+ led_ind_cnt = 0;
+ }
+ else
+ {
+ led_ind_cnt += 1;
+ }
+ m1_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
+ sprintf( sbuf, "XX_WICH_001_%04d", m1_speed );
+ DEBUG_PRINT_L0( "Handy(Bd2)> BTN WCH UP [%s]\r\n", sbuf );
+ // Send command to host
+ rts = tsocket.send_all(sbuf, sizeof(sbuf));
+ if( rts < 0 ) break;
+ lcdMutex.lock();
+ lcd_dsp(0,0,"XO W Up ",11);
+ lcdMutex.unlock();
+ // Receive winch position from host
+ rts = tsocket.receive( rbuf, sizeof(rbuf));
+ if( rts < 0 ) break;
+ rbuf[4] = '\0';
+ tmpPosition = atoi( rbuf );
+ if( tmpPosition == 9999 )
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> ### WWINCH LOCK DETECTION ###\r\n" );
+ led_ptwc_lock = true;
+ }
+ else
+ {
+ DEBUG_PRINT_L0( "Handy(Bd2)> Winch Position [%04d]\r\n", tmpPosition );
+ sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
+ if( tmpPosition != 9999 )
+ {
+ if( flg_lsw_valid == true )
+ {
+ sprintf(msg,"B2Wch [%04d mm]*", tmpPosition);
+ }
+ else
+ {
+ sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
+ }
+ }
+ else
+ {
+ if( flg_lsw_valid == true )
+ {
+ sprintf(msg,"B2Wch [**** mm]*");
+ }
+ else
+ {
+ sprintf(msg,"B2Wch [**** mm] ");
+ }
+ }
+ lcdMutex.lock();
+ lcd_dsp(0,1,msg,16);
+ lcdMutex.unlock();
+ }
+ if( led_ptwc_lock == true )
+ {
+ led_error_blink( 1, 4 );
+ }
+ }
+ }
+ }
+ Thread::wait(5);
+ }
+ lcd_dsp(0,0,"!!Eth error",11);
+ tsocket.close();
+ }
+
+ }
+ else{
+ cf_led_error( &led1,&led2,&led3,&led4 );
+ DEBUG_PRINT_L0("Handy(Bd2)> ###ERROR### Eternet connect Fali\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)> This programis booting in Stand alone mode.\r\n");
+ flg_system_ok = false;
+ }
+ }
+ else{
+ DEBUG_PRINT_L0("Handy(Bd2)> ###ERROR### Eternet init Fail\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)> This programis booting in Stand alone mode.\r\n");
+ flg_system_ok = false;
+ }
+}
+
+// ======================================================================
+// Read setting value from lpcal file system of mbed
+// ======================================================================
+bool read_LFS( setValue_t* setValue ){
+ FILE *fp;
+ char *fname = "/local/jsset.txt";
+ char s[150];
+ int c;
+ int data;
+ bool rts;
+
+ flg_mutex.lock();
+ fp = fopen(fname, "r");
+ if( fp != NULL ){ // Open "set.txt" on the local file system for writing
+ c = getc(fp);
+ if( c != '#' ){
+ pc.printf( "#### ERROR This is not a setting file ####\r\n");
+ rts = false;
+ }
+ else{
+ fgets( s, 100, fp );
+ pc.printf( "%s", s );
+ fscanf(fp,"%03d",&data);setValue->jsCtrl.rjs_upper_dzone=data; pc.printf("R-JS upper = %03d",setValue->jsCtrl.rjs_upper_dzone); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->jsCtrl.rjs_lower_dzone=data; pc.printf("R-JS lower = %03d",setValue->jsCtrl.rjs_lower_dzone); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->jsCtrl.rjs_right_dzone=data; pc.printf("R-JS upper = %03d",setValue->jsCtrl.rjs_right_dzone); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->jsCtrl.rjs_left_dzone=data; pc.printf("R-JS lower = %03d",setValue->jsCtrl.rjs_left_dzone); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->jsCtrl.reserved_1=data; pc.printf("%03d",setValue->jsCtrl.reserved_1); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->jsCtrl.reserved_2=data; pc.printf("%03d",setValue->jsCtrl.reserved_2); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->jsCtrl.reserved_3=data; pc.printf("%03d",setValue->jsCtrl.reserved_3); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->jsCtrl.reserved_4=data; pc.printf("%03d",setValue->jsCtrl.reserved_4); fgets(s,100,fp); pc.printf("%s",s);
+ }
+ fclose(fp);
+ rts = true;
+ }
+ else{
+ pc.printf( "#### ERROR local file open error ####\r\n");
+ rts = false;
+ }
+ flg_mutex.unlock();
+ return rts;
+}
+
+
+extern "C" {
+#include "rt_TypeDef.h"
+#include "rt_System.h"
+}
+// **********************************************************************
+//
+// Main Function of this program
+//
+// **********************************************************************
+int main()
+{
+ Mutex file_access_mutex;
+// int ret;
+ int try_cnt;
+// bool flg_ethernet = false;
+ char sss[20];
+
+ // Serial baudrate
+ pc.baud(115200);
+
+ sw_validpart.mode( PullUp ); // use internal pullup
+ sw_JS_ShapeMode.mode( PullUp ); // use internal pullup
+
+ led_ind_ptwc = ON; // LED indicator pan/tilt, crawler limit
+ led_ind_tfm = ON; // LED indicator tfm motor current limit
+ led_valid_part1 = ON; // LED transform, crawler valid
+ led_valid_part2 = ON; // LED pan/tilt, winch valid
+
+ lcd_dsp(0,0,"B2 PMORPH Demo ",16);
+ lcd_dsp(0,1,"Revast Co.,Ltd. ",16);
+ lcd_dsp(0,0,"Rev1.00 20170403",16);
+ lcd_dsp(0,1,"System Booting..",16);
+
+ DEBUG_PRINT_L0("\r\n");
+ DEBUG_PRINT_L0("Bd2> HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH\r\n");
+ DEBUG_PRINT_L0("Bd2> | Project: B1/B2 Debris Prover Demonstration machine\r\n");
+ DEBUG_PRINT_L0("Bd2> |-------------------------------------------------------------\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)>> | This is: Handy Control Program of Main Controller\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)>> | Target MCU: mbed LPC1768\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)>> | Letest update: %s\r\n", LatestUpDate);
+ DEBUG_PRINT_L0("Handy(Bd2)>> | Program Revision: %s\r\n", ProgramRevision);
+ DEBUG_PRINT_L0("Handy(Bd2)>> | Author: %s\r\n", Author);
+ DEBUG_PRINT_L0("Handy(Bd2)>> | Copyright(C) 2017 %s Allright Reserved\r\n", Company);
+ DEBUG_PRINT_L0("Handy(Bd2)>> HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH\r\n");
+ sprintf( sss, "%s", ProgramRevision );
+
+// Thread::wait(50);
+
+ //---------------------------------------------------
+ // Read CrExp setting value from Local File System
+ // setting file "SET.DAT".
+ // When error occured, LED1 will be blinking shortly.
+ //---------------------------------------------------
+ DEBUG_PRINT_L0("Handy(Bd2)> =============================================================\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)> 2. Read setting value from LFS\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)> =============================================================\r\n");
+
+#ifdef __CREATE_SETTING_FILE__
+ write_LFS(&setValue); // Create and set setting file.
+#endif // __CREATE_SETTING_FILE__
+
+ // --------------------------------------------------------------------
+ // Read setting from local file system and set to internal structure
+ // --------------------------------------------------------------------
+ lcdMutex.lock();
+ lcd_dsp(0,0,"LFS reading ... ",16);
+ lcdMutex.unlock();
+ try_cnt = LFS_READ_COUNT;
+ while( 1 ){
+ if( read_LFS(&setValue) == true ) break;
+ else try_cnt -= 1;
+ if( try_cnt == 0 ){
+ DEBUG_PRINT_L0("Handy(Bd2)> ***ERROR*** LFS read error\r\n");
+ while(1){
+ led1 = !led1;
+ Thread::wait(30);
+ }
+ }
+ }
+
+ DEBUG_PRINT_L0("Handy(Bd2)> LFS read OK\r\n");
+ lcdMutex.lock();
+ lcd_dsp(0,0,"LFS read OK ",16);
+ lcdMutex.unlock();
+
+ led3 = ON; // Setting Data Read OK
+ led2 = ON; // Check target OK
+
+ /* Set basic function default setting */
+ baseOperation.sv_JS_OpeMode = 0;
+ baseOperation.sv_JS_OpeMode = 0;
+ baseOperation.sv_WinchValid = 0;
+
+ DEBUG_PRINT_L0( "Handy(Bd2)> ----------------------------------\r\n");
+ DEBUG_PRINT_L0( "Handy(Bd2)> >>>> Initializing completed ! <<<<\r\n");
+ DEBUG_PRINT_L0( "Handy(Bd2)> ----------------------------------\r\n");
+
+/*
+ lcdMutex.lock();
+ lcd_dsp(0,0,"Booted up ! ",16);
+ Thread::wait(500);
+ lcd_dsp(0,0,"LFS System OK ",16);
+ lcd_dsp(0,1," ",16);
+ Thread::wait(500);
+ lcd_dsp(0,0," ",16);
+ lcd_dsp(0,1,"B2Wch [3500 mm] ",16);
+ lcdMutex.unlock();
+*/
+
+ lcdMutex.lock();
+ lcd_dsp(0,0,"Start sw task ",16);
+ lcdMutex.unlock();
+ DEBUG_PRINT_L0("Handy(Bd2)> =============================================================\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)>> 5. Start the task\r\n");
+ /* Max thread count is (may be ..) 2, How can I increase this , I don't know ?? */
+
+// DEBUG_PRINT_L0("Handy(Bd2)> Start host gamepad task ... ");
+// Thread thread_gpd(gamepad_task, NULL, osPriorityNormal, 128*4); // GamePad Task isn't necessary.
+ DEBUG_PRINT_L0("Handy(Bd2)> Start switch task ... ");
+ Thread thread_swd(sw_task, NULL, osPriorityNormal, 256 * 4);
+
+ DEBUG_PRINT_L0("\r\n");
+ DEBUG_PRINT_L0("Handy(Bd2)> =============================================================\r\n");
+
+ led4 = OFF;
+ led3 = OFF;
+ led2 = OFF;
+ led1 = ON; // Initializing is OK then Power Indicator LED ON
+
+ wait(10.0);
+
+ if( flg_system_ok == true )
+ {
+ led_ind_ptwc = OFF; // LED indicator pan/tilt, crawler limit
+ led_ind_tfm = OFF; // LED indicator tfm motor current limit
+ led_valid_part1 = OFF; // LED transform, crawler valid
+ led_valid_part2 = OFF; // LED pan/tilt, winch valid
+ }
+
+ while( 1 ) {
+ Thread::wait(3);
+ // -----------------------------------------------------------------
+ // Communicate with client PC program.
+ // TCP connection:
+ // -----------------------------------------------------------------
+ if( flg_system_ok == true )
+ {
+ lcdMutex.lock();
+ lcd_dsp(13,0,"[^] ",4);
+ lcdMutex.unlock();
+ Thread::wait(300);
+ lcdMutex.lock();
+ lcd_dsp(13,0,"[>] ",4);
+ lcdMutex.unlock();
+ Thread::wait(300);
+ lcdMutex.lock();
+ lcd_dsp(13,0,"[v] ",4);
+ lcdMutex.unlock();
+ Thread::wait(300);
+ lcdMutex.lock();
+ lcd_dsp(13,0,"[<] ",4);
+ lcdMutex.unlock();
+ Thread::wait(300);
+ }
+ else
+ {
+ lcdMutex.lock();
+ // 0123456789012345
+ lcd_dsp(0,0,"HandyCtrl Error!",16);
+ lcd_dsp(0,1,"* Check system! * ",16);
+ lcdMutex.unlock();
+ Thread::wait(750);
+ }
+ }
+
+ tsocket.close();
+}
\ No newline at end of file