z ysaito
2018.07.26
Dependencies: EthernetInterface TextLCD USBDevice USBHost3 mbed
Fork of
USBHostHub_HelloWorld
by 
Samuel Mokrani
Diff: 0_main.cpp
- Revision:
- 10:a2bd7d07c7f8
- Child:
- 11:ff06edc0219c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/0_main.cpp Wed Feb 10 14:58:50 2016 +0000
@@ -0,0 +1,2136 @@
+/***************************************
+ * Project: B2
+ * Title: CrExp B2 Motor Ctrl Main
+ * Target: LPC1768
+ * ------------------------------------
+ *
+ *
+ *
+ * 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
+ * p16 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 "QEI.h"
+#include "common.h"
+#include "stdio.h"
+#include "TextLCD.h"
+#include "com_func.h"
+
+// USBSerial serial setting
+Serial pc(USBTX, USBRX); // UART
+
+// Digital I/O setting
+DigitalOut led1(LED1); // 1: on, 0: off
+DigitalOut led2(LED2); // 1: on, 0: off
+DigitalOut led3(LED3); // 1: on, 0: off
+DigitalOut led4(LED4); // 1: on, 0: off
+
+// I2C setting
+//i2c(p28, p27); // I2C SDA, SCL is not good ???
+I2C i2c(p9, p10); // I2C SDA, SCL is good
+
+// I2C address
+const int32_t i2c_addr_handy = I2C_ADDRESS_HANDY << 1; // Ctrl Board0 : Handy Controller
+const int32_t i2c_addr_winch = I2C_ADDRESS_WINCH << 1; // Ctrl Board1 : Winch
+const int32_t i2c_addr_transform = I2C_ADDRESS_TRANSFORM << 1; // Ctrl Board2 : Transform
+const int32_t i2c_addr_crawler = I2C_ADDRESS_CRAWLER << 1; // Ctrl Board3 : Crawler
+const int32_t i2c_addr_resolver = I2C_ADDRESS_RESOLVER << 1; // Ctrl Board4 : Resolver
+
+// Ethernet
+EthernetInterface eth;
+TCPSocketServer tcp_server; // TCP Server
+TCPSocketConnection tcp_client;
+UDPSocket udp_server; // UDP Server
+Endpoint client;
+// LCD
+TextLCD lcd(p11, p12, p24, p23, p22, p21); // rs, e, d4-d7
+// Local File System
+LocalFileSystem SettingFile("local"); // Create the local filesystem under the name "local"
+
+// Global
+uint32_t flg_gamePad_Connected = 0;
+char PC_cmd[11+1] = "&0100000000";
+setValue_t setValue; // Setting Data
+char dbuffer[128];
+uint8_t motor_speed = 0;
+/* 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 flg_mutex;
+Mutex swbtn_OpeMutex;
+int swbtn_Opeflg = 0;
+
+bool flg_ButtonOn = false;
+
+// Mutex hwbtn_OpeMutex;
+// int hwbtn_Opeflg = 0;
+
+char motor_current_pct;
+int flg_JS_shape_mode = 0;
+int flg_JS_ope_mode = 0;
+uint8_t motor1_current_pct;
+uint8_t motor2_current_pct;
+
+
+/* Prototype */
+void write_Setting_lfs(void);
+int read_Setting_lfs(void);
+void dsp_console_setting_value(void);
+void winchMovingControl( int, char*, int, winchData_t*, int, char* );
+extern void dspSetValue2Console( Serial*, setValue_t * );
+extern void lcd_out( TextLCD* ,int, int, char* );
+
+// ============================================================
+// Read motor current
+// ============================================================
+void read_motorCurrent(
+ int i2c_addr,
+ char* I2C_cmd,
+ char* I2C_res,
+ int NumberOfI2Cdata
+
+){
+ I2C_cmd[1] = 'C';
+ i2c.write(i2c_addr, I2C_cmd, 3); // Send command to motor control board.
+ //Thread::wait(500);
+ i2c.read(i2c_addr, I2C_res, 3); // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+ /*
+ DEBUG_PRINT_L4(" ++++++++++++++++++++++++++++++\n" );
+ DEBUG_PRINT_L4(" Read Motor1 Current [%d]\n", I2C_res[0] );
+ DEBUG_PRINT_L4(" Read Motor2 Current [%d]\n", I2C_res[1] );
+ DEBUG_PRINT_L4(" Read Motor2 Current [%d]\n", I2C_res[2] );
+ DEBUG_PRINT_L4(" ++++++++++++++++++++++++++++++\n" );
+ */
+}
+
+// ============================================================
+// Send stop command to motor specific controller
+// ============================================================
+void stop_motor( int32_t i2c_addr, char* i2c_cmd, int32_t flg ){
+ if (flg == 1 ){
+ i2c_cmd[3] = MOTOR_STP;
+ i2c_cmd[4] = MOTOR_STP;
+ }
+ i2c.write(i2c_addr, i2c_cmd, 6 ); // Send command to motor control board.
+}
+
+// ============================================================
+// 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;
+}
+
+// ============================================================
+// Read winch current position from Resolver.
+// ============================================================
+int16_t ReadWinchCurrentPosition( int32_t i2c_addr )
+{
+ char I2C_data[2];
+ int16_t res_position;
+
+ i2c.read(i2c_addr, I2C_data, 2); // Read
+ // flg_mutex.lock();
+ res_position = (I2C_data[1] << 8) | I2C_data[0];
+ // winchCurrentPosition = res_position;
+ // flg_mutex.unlock();
+ return res_position;
+}
+
+// ============================================================
+// Button control
+// ============================================================
+void onControl(
+ uint8_t btn00, uint8_t btn01, uint8_t btn02, uint8_t btn03,
+ uint8_t btn04, uint8_t btn05, uint8_t btn06, uint8_t btn07,
+ uint8_t btn08, uint8_t btn09, uint8_t btn10, uint8_t btn11,
+ uint8_t btn12, uint8_t btn13, uint8_t btn14, uint8_t btn15,
+ uint16_t gamePadVID, uint16_t gamePadPID
+){
+ /* ** OLD type ***
+ * I2C Command (7byte)
+ * 0: '#' // Preamble
+ * 1: '0' // Target upper
+ * 2: '1' // Target lower
+ * 3: '0' // Command 1
+ * 4: '1/3' // Command 2
+ * 5: '0/1'
+ */
+
+ /* New Type 15.11.06 ~
+ [0] :
+ [1] :
+ [2] :
+ [3] :
+ [4] : motor 1 direction (A=Forward, B=Reverse, F=Stop)
+ [5] : motor 1 speed
+ [6] : motor 2 direction (A=Forward, B=Reverse, F=Stop)
+ [7] : motor 2 speed <-- New added
+ [8] : N/F
+ [9] : N/F
+ */
+ char I2C_cmd[NumberOfI2CCommand+1] = "#010000";
+ char I2C_res[NumberOfI2CCommand+1] = "\0";
+
+ uint8_t btnStatus_RFK = 0;
+ uint8_t btnStatus_RFI = 0;
+ uint8_t btnStatus_LBK = 0;
+ uint8_t btnStatus_LBI = 0;
+ uint8_t btnStatus_WUP = 0;
+ uint8_t btnStatus_WDN = 0;
+ uint8_t btnStatus_RJSFwdRvs = 0; // R-JS Fwd/Rvs
+ uint8_t btnStatus_RJSLftRgt = 0; // R-JS Left/Light
+ uint8_t btnStatus_LJSFwdRvs = 0; // L-JS Fwd/Rvs
+ uint8_t btnStatus_LJSLftRgt = 0; // L-Js Left/Right
+
+ uint8_t btnStatus_Start = 0; // Guide button status for ELECOM GamePad
+ uint8_t btnStatus_CrossUp = 0;
+ uint8_t btnStatus_CrossDn = 0;
+ uint8_t btnStatus_CrossRt = 0;
+ uint8_t btnStatus_CrossLt = 0;
+
+ // For JS Ope mode B
+ uint8_t btnID_RFK = 0;
+ uint8_t btnID_RFI = 0;
+ uint8_t btnID_LBK = 0;
+ uint8_t btnID_LBI = 0;
+ uint8_t btnID_WUP = 0;
+ uint8_t btnID_WDN = 0;
+
+ uint8_t btnID_Start = 0; // Guide button ID for ELECOM GamePad
+ uint8_t btnID_JS_SD = 0; // JS mode Single / Dual
+ uint8_t btnID_JD_IK = 0; // JS mode I-Shape / KO-Shape
+
+ uint8_t btnID_CrossUp = 0;
+ uint8_t btnID_CrossDn = 0;
+ uint8_t btnID_CrossRt = 0;
+ uint8_t btnID_CrossLt = 0;
+
+ if( gamePadVID == GAMEPAD_VID_RSTHANDY ){ // NRst own original controller
+ DEBUG_PRINT_L4("Bd0> [Rst HDY] ");
+ btnID_WDN = 0x10;
+ btnID_WUP = 0x20;
+ btnID_RFK = 0x01;
+ btnID_RFI = 0x02;
+ btnID_LBK = 0x04;
+ btnID_LBI = 0x08;
+ btnID_CrossUp = 0;
+ btnID_CrossDn = 4;
+ btnID_CrossRt = 2;
+ btnID_CrossLt = 6;
+ btnID_JS_SD = 1; // JS mode Single or Dual
+ btnID_JD_IK = 2; // JS mode I-Shape KO-Shape
+ if ( gamePadPID == GAMEPAD_PID_RSTHANDY ){
+ btnStatus_WUP = btn04;
+ btnStatus_WDN = btn04;
+ btnStatus_RFK = btn04;
+ btnStatus_RFI = btn04;
+ btnStatus_LBK = btn04;
+ btnStatus_LBI = btn04;
+ btnStatus_RJSFwdRvs = btn03; // Assign analog js to this sw input by LPC1768mbed
+ btnStatus_RJSLftRgt = btn02; // Assign analog js to this sw input by LPC1768mbed
+ btnStatus_LJSFwdRvs = btn01; // Assign analog js to this sw input by LPC1768mbed
+ btnStatus_LJSLftRgt = btn00; // Assign analog js to this sw input by LPC1768mbed
+ btnStatus_Start = btn05; //
+ btnStatus_CrossUp = btn06;
+ btnStatus_CrossDn = btn06;
+ btnStatus_CrossRt = btn06;
+ btnStatus_CrossLt = btn06;
+ }
+ }
+ else if (gamePadVID == GAMEPAD_VID_ELECOM ){
+ DEBUG_PRINT_L4("Bd0> [ELECOM] ");
+ btnID_WUP = 2;
+ btnID_WDN = 4;
+ btnID_RFK = 32;
+ btnID_RFI = 128;
+ btnID_LBK = 16;
+ btnID_LBI = 64;
+ btnID_Start = 8; // Guide button ID for ELECOM GamePad
+ btnID_CrossUp = 0;
+ btnID_CrossDn = 4;
+ btnID_CrossRt = 2;
+ btnID_CrossLt = 6;
+ if ( gamePadPID == GAMEPAD_PID_ELECOM_JCU3613M ){
+ btnStatus_WUP = btn04;
+ btnStatus_WDN = btn04;
+ btnStatus_RFK = btn04;
+ btnStatus_RFI = btn04;
+ btnStatus_LBK = btn04;
+ btnStatus_LBI = btn04;
+ btnStatus_RJSFwdRvs = btn03;
+ btnStatus_RJSLftRgt = btn02;
+ btnStatus_LJSFwdRvs = btn01;
+ btnStatus_LJSLftRgt = btn00;
+ btnStatus_Start = btn05; // Guide button status for ELECOM GamePad
+ btnStatus_CrossUp = btn06;
+ btnStatus_CrossDn = btn06;
+ btnStatus_CrossRt = btn06;
+ btnStatus_CrossLt = btn06;
+ }
+ }
+ else if( gamePadVID == GAMEPAD_VID_LOGICOOL ){
+ btnID_WUP = 136;
+ btnID_WDN = 40;
+ btnID_RFK = 2;
+ btnID_RFI = 8;
+ btnID_LBK = 1;
+ btnID_LBI = 4;
+ btnID_Start = 32; // Guide button ID for ELECOM GamePad
+ btnID_CrossUp = 0;
+ btnID_CrossDn = 4;
+ btnID_CrossRt = 2;
+ btnID_CrossLt = 6;
+ if ( gamePadPID == GAMEPAD_PID_LOGICOOL_F710 ){
+ DEBUG_PRINT_L4("Bd0> [LOGI F710] ");
+ btnStatus_WUP = btn05;
+ btnStatus_WDN = btn05;
+ btnStatus_RFK = btn06;
+ btnStatus_RFI = btn06;
+ btnStatus_LBK = btn06;
+ btnStatus_LBI = btn06;
+ }
+ else if ( gamePadPID == GAMEPAD_PID_LOGICOOL_F310 ){
+ DEBUG_PRINT_L4("Bd0> [LOGI F310] ");
+ btnStatus_WUP = btn04;
+ btnStatus_WDN = btn04;
+ btnStatus_RFK = btn05;
+ btnStatus_RFI = btn05;
+ btnStatus_LBK = btn05;
+ btnStatus_LBI = btn05;
+ btnStatus_RJSFwdRvs = btn03;
+ btnStatus_RJSLftRgt = btn02;
+ btnStatus_LJSFwdRvs = btn01;
+ btnStatus_LJSLftRgt = btn00;
+ btnStatus_RJSFwdRvs = btn03;
+ btnStatus_RJSLftRgt = btn02;
+ btnStatus_LJSFwdRvs = btn01;
+ btnStatus_LJSLftRgt = btn00;
+ btnStatus_Start = btn05; // Guide button status for ELECOM GamePad
+ btnStatus_CrossUp = btn04;
+ btnStatus_CrossDn = btn04;
+ btnStatus_CrossRt = btn04;
+ btnStatus_CrossLt = btn04;
+ }
+ }
+ else if ( gamePadVID == GAMEPAD_VID_SANWA){
+ DEBUG_PRINT_L4("Bd0> [SANWA] ");
+ btnID_WUP = 4;
+ btnID_WDN = 2;
+ btnID_RFK = 2;
+ btnID_RFI = 1;
+ btnID_LBK = 128;
+ btnID_LBI = 64;
+ if ( gamePadPID == GAMEPAD_PID_SANWA_JYP70US ){
+ btnStatus_WUP = btn05;
+ btnStatus_WDN = btn05;
+ btnStatus_RFK = btn06;
+ btnStatus_RFI = btn06;
+ btnStatus_LBK = btn05;
+ btnStatus_LBI = btn05;
+ }
+ }
+
+#ifdef __DISP_GAMAPAD_STATUS_ALL__ // For Debug
+// DEBUG_PRINT_BTN(" Btn 00:%d, 01:%d, 02:%d, 03:%d, 04:%d, 05:%d, 06:%d, 07:%d, 08:%d | VID=0x%04x, PID=0x%04x\r\n",
+// btn00,btn01,btn02,btn03,btn04,btn05,btn06,btn07,btn08,
+// gamePadVID, gamePadPID);
+ DEBUG_PRINT_L4(" Btn 00:%d, 01:%d, 02:%d, 03:%d, 04:%d, 05:%d, 06:%d, 07:%d, 08:%d\r\n",
+ btn00,btn01,btn02,btn03,btn04,btn05,btn06,btn07,btn08);
+#endif
+
+ I2C_cmd[4] = '\0';
+ I2C_cmd[5] = '\0';
+ I2C_cmd[6] = '\0';
+ I2C_cmd[7] = '\0';
+ I2C_cmd[8] = '\0';
+ I2C_cmd[9] = '\0';
+
+ int tmpSpeed = 0;
+
+ if (swbtn_Opeflg == 1){
+ Thread::wait(1);
+ }
+ else{
+ if( gamePadVID == GAMEPAD_VID_RSTHANDY ){ // NRst own original controller
+ flg_exp_status &= 0xFFFFFFF0;
+ if(!( btnStatus_Start & 0x01 )){ // I-Shape
+ setValue.operation.sv_JS_ShapeMode = 0;
+ setValue.operation.sv_JS_OpeMode = 0;
+ flg_exp_status |= 0x00000001;
+ }
+ else{ // KO-Shape
+ setValue.operation.sv_JS_ShapeMode = 1;
+ flg_exp_status |= 0x00000002;
+ }
+
+ if(!(btnStatus_Start & 0x02 )&&( btnStatus_Start & 0x01 )){ // Dual
+ setValue.operation.sv_JS_OpeMode = 1;
+ flg_exp_status |= 0x00000004;
+ }
+ else{ // Single
+ setValue.operation.sv_JS_OpeMode = 0;
+ flg_exp_status |= 0x00000008;
+ }
+ DEBUG_PRINT_L4( "-----------------------------\n" );
+ DEBUG_PRINT_L4( "%d : %d\n",btnStatus_Start, flg_exp_status );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\n", setValue.operation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " JS operation mode change: %d\n", setValue.operation.sv_JS_OpeMode );
+ DEBUG_PRINT_L4( "-----------------------------\n" );
+ }
+ else{
+ /*
+ *
+ * GamePad software switch
+ * [Guide] + [B] : JS shape mode toggle: I <--> KO
+ * [Guide] + [X} : JS Operation mode toggle: Dual <--> Single
+ * 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+
+ * |x|x|x|x|x|x|x|o| 1: I-Shape JSmode, 2: K-Shape JSmode, 4: Single JSmode, 8: Dual JSmode
+ * +-+-+-+-+-+-+-+-+
+ */
+ if( btnStatus_Start == btnID_Start ){
+ if ( btnStatus_CrossUp == btnID_CrossUp ){ // I Shape
+ setValue.operation.sv_JS_ShapeMode = 0;
+ setValue.operation.sv_JS_OpeMode = 0;
+ DEBUG_PRINT_L4( "--------------------------------\n" );
+ DEBUG_PRINT_L4( " I\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\n", setValue.operation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " JS operation mode change: %d\n", setValue.operation.sv_JS_OpeMode );
+ DEBUG_PRINT_L4( "--------------------------------\n" );
+ flg_exp_status |= 0x00000001;
+ }
+ else if( btnStatus_CrossDn == btnID_CrossDn ){ // KO Shape
+ setValue.operation.sv_JS_ShapeMode = 1;
+ DEBUG_PRINT_L4( "-------------------------\n" );
+ DEBUG_PRINT_L4( " KO\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\n", setValue.operation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " JS operation mode change: %d\n", setValue.operation.sv_JS_OpeMode );
+ DEBUG_PRINT_L4( "-------------------------\n" );
+ flg_exp_status |= 0x00000002;
+ }
+ else if( btnStatus_CrossRt == btnID_CrossRt ){ // Single JS
+ setValue.operation.sv_JS_OpeMode = 0;
+ DEBUG_PRINT_L4( "-----------------------------\n" );
+ DEBUG_PRINT_L4( " Single\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\n", setValue.operation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " JS operation mode change: %d\n", setValue.operation.sv_JS_OpeMode );
+ DEBUG_PRINT_L4( "-----------------------------\n" );
+ flg_exp_status |= 0x00000004;
+ }
+ else if( btnStatus_CrossLt == btnID_CrossLt ){ // Dual JS
+ if( setValue.operation.sv_JS_ShapeMode == 1 ){ // KO Shape
+ setValue.operation.sv_JS_OpeMode = 1;
+ DEBUG_PRINT_L4( "-----------------------------\n" );
+ DEBUG_PRINT_L4( " Dual\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\n", setValue.operation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " JS operation mode change: %d\n", setValue.operation.sv_JS_OpeMode );
+ DEBUG_PRINT_L4( "-----------------------------\n" );
+ flg_exp_status |= 0x00000008;
+ }
+ }
+ }
+ else{
+ flg_exp_status &= 0xFFFFFFF0;
+ }
+ }
+ /*/ ====================================
+ // Crawler Moving Control
+ // ====================================
+ // JoyStick mode 1: Dual JoyStick mode
+ //
+ // *** ***
+ // * L * * R *
+ // *** ***
+ // F 4 1
+ //
+ // R 8 2
+ //
+ // Forward move 5
+ // Reverce move a
+ // Right rotation 6
+ // Left rotation 9
+ * 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+
+ * |x|x|o|x|x|x|x|x| 1: R Fwd, 2: R Rvs, 4: L Fwd, 8: L Rvs
+ * +-+-+-+-+-+-+-+-+
+ */
+ if( setValue.operation.sv_JS_OpeMode == 1 ){
+ if( btnStatus_LJSFwdRvs < setValue.crawlerCtrl.sv_LBCM_dzc - setValue.crawlerCtrl.sv_LBCM_dzl ){
+ flg_ButtonOn = true;
+ led3 = 1;
+ I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
+ tmpSpeed = ( setValue.crawlerCtrl.sv_LBCM_dzc+1 - btnStatus_LJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc * setValue.crawlerCtrl.sv_LBCM_sli_F / 100; // Speed
+ I2C_cmd[7] = (char)tmpSpeed;
+ DEBUG_PRINT_L3( "Bd0> ******************************\n");
+ DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Fwd (Speed=%d)\n", tmpSpeed);
+ DEBUG_PRINT_L3( "Bd0> ******************************\n");
+ flg_exp_status |= 0x00400000;
+
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+ else if( btnStatus_LJSFwdRvs > setValue.crawlerCtrl.sv_LBCM_dzc + setValue.crawlerCtrl.sv_LBCM_dzu ){
+ flg_ButtonOn = true;
+ led3 = 1;
+ I2C_cmd[6] = MOTOR_RVS; // Motor2 RVS
+ tmpSpeed = ( btnStatus_LJSFwdRvs - setValue.crawlerCtrl.sv_LBCM_dzc ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc+1 * setValue.crawlerCtrl.sv_LBCM_sli_F / 100; // Speed I2C_cmd[5] = (char)tmpSpeed;
+ I2C_cmd[7] = (char)tmpSpeed;
+ DEBUG_PRINT_L3( "Bd0> ******************************\n");
+ DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Rvs (Speed=%d)\n", tmpSpeed);
+ DEBUG_PRINT_L3( "Bd0> ******************************\n");
+ flg_exp_status |= 0x00800000;
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+ else{
+ I2C_cmd[6] = MOTOR_STP; // Stop
+ I2C_cmd[7] = 0; // Speed=0
+ flg_exp_status &= 0xFF3F000F;
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ }
+
+ if( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl ){
+ flg_ButtonOn = true;
+ led3 = 1;
+ I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
+ tmpSpeed = (( 128 - btnStatus_RJSFwdRvs ) * 100 / 127 ) * setValue.crawlerCtrl.sv_RFCM_sli_R / 100;
+ I2C_cmd[3] = (char)tmpSpeed;
+ DEBUG_PRINT_L3( "Bd0> ******************************\n");
+ DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Fwd (Speed=%d)\n", tmpSpeed);
+ DEBUG_PRINT_L3( "Bd0> ******************************\n");
+ flg_exp_status |= 0x00100000;
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+ else if( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu ){
+ flg_ButtonOn = true;
+ led3 = 1;
+ I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
+ tmpSpeed = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc+1 * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
+ I2C_cmd[3] = (char)tmpSpeed;
+ DEBUG_PRINT_L3( "Bd0> ******************************\n");
+ DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Rvs (Speed=%d)\n", tmpSpeed);
+ DEBUG_PRINT_L3( "Bd0> ******************************\n");
+ flg_exp_status |= 0x00200000;
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+ else{
+ I2C_cmd[2] = MOTOR_STP; // Stop
+ I2C_cmd[3] = 0; // Speed=0
+ flg_exp_status &= 0xFFCFFFFF;
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ }
+ // I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ // I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ // I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ // I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ // i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ // i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ led3 = 0;
+ }
+ /* JoyStick mode 0: Single JoyStick mode
+ //
+ // *** ****
+ // * X * * LR *
+ // *** ****
+ // F 4 1
+ //
+ // R 8 2
+ //
+ // Forward move 5
+ // Reverce move a
+ // Right rotation 6
+ // Left rotation 9
+ * 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+
+ * |x|x|o|x|x|x|x|x| 1: R Fwd, 2: R Rvs, 4: L Fwd, 8: L Rvs
+ * +-+-+-+-+-+-+-+-+
+ */
+ else{ // Single JoyStick mode
+ if(( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl ) && ( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu) && ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)){
+ flg_ButtonOn = true;
+ led3 = 1;
+ I2C_cmd[2] = MOTOR_FWD; // Motor1 Fwd
+ I2C_cmd[3] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
+ I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
+ I2C_cmd[7] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
+ DEBUG_PRINT_L4( "Bd0> *************************************\n");
+ DEBUG_PRINT_L4( "Bd0> Single Mode: Dir1 (Speed: %d, %d)\n", I2C_cmd[5], I2C_cmd[7]);
+ DEBUG_PRINT_L4( "Bd0> *************************************\n");
+ flg_exp_status |= 0x00100000;
+ flg_exp_status |= 0x00400000; // 0x00500000
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+ else if(( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu ) && ( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu) && ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)){
+ flg_ButtonOn = true;
+ led3 = 1;
+ I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
+ I2C_cmd[3] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
+ I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
+ I2C_cmd[7] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
+ DEBUG_PRINT_L4( "Bd0> *************************************\n");
+ DEBUG_PRINT_L4( "Bd0> Single Mode: Dir2 (Speed: %d, %d)\n", I2C_cmd[5], I2C_cmd[7]);
+ DEBUG_PRINT_L4( "Bd0> *************************************\n");
+ flg_exp_status |= 0x00200000;
+ flg_exp_status |= 0x00400000; // 0x00600000
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+ else if(( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu ) && ( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu) && ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)){
+ flg_ButtonOn = true;
+ led3 = 1;
+ I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
+ I2C_cmd[3] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
+ I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
+ I2C_cmd[7] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
+ DEBUG_PRINT_L4( "Bd0> *************************************\n");
+ DEBUG_PRINT_L4( "Bd0> Single Mode: Dir3 (Speed: %d, %d)\n", I2C_cmd[5], I2C_cmd[7]);
+ DEBUG_PRINT_L4( "Bd0> *************************************\n");
+ flg_exp_status |= 0x00200000;
+ flg_exp_status |= 0x00800000; // 0x00A00000
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+ else if(( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzu ) && ( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu) && ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)){
+ flg_ButtonOn = true;
+ led3 = 1;
+ I2C_cmd[2] = MOTOR_FWD; // Motor1 Fwd
+ I2C_cmd[3] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
+ I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
+ I2C_cmd[7] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
+ DEBUG_PRINT_L4( "Bd0> *************************************\n");
+ DEBUG_PRINT_L4( "Bd0> Single Mode: Dir4 (Speed: %d, %d)\n", I2C_cmd[5], I2C_cmd[7]);
+ DEBUG_PRINT_L4( "Bd0> *************************************\n");
+ flg_exp_status |= 0x00100000;
+ flg_exp_status |= 0x00800000; // 0x00900000
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+ // ====================================
+ // ALL motor off commmand packet send
+ // ====================================
+ else {
+ led3 = 0;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[2] = MOTOR_STP; // Motor1 Fwd
+ I2C_cmd[3] = 0; // Speed=0
+ I2C_cmd[6] = MOTOR_STP; // Motor2 Rvs
+ I2C_cmd[7] = 0; // Speed=0
+ Thread::wait(5);
+ #endif
+ flg_exp_status &= 0xFF0FFFFF;
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+
+ }
+ // I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ // I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ // I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ // I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ // i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ // i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ }
+ /*
+ * ====================================
+ * Winch Motor Control
+ *
+ * 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+
+ * |x|o|x|x|x|x|x|x| 1: W Down, 2: W Up, 4: -, 8: -
+ * +-+-+-+-+-+-+-+-+
+ */
+ if ( btnStatus_WDN == btnID_WDN ) { // Winch Down (FWD)
+ flg_ButtonOn = true;
+
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 1;
+ // hwbtn_OpeMutex.unlock();
+
+ DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ DEBUG_PRINT_L3( "Bd0> BTN W-DN(Fwd)\n" );
+ DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
+ I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_F; // Speed
+ // I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
+ // I2C_cmd[7] = setValue.winchCtrl.sv_WRM_sli_F; // Speed
+ #endif
+ //// Are these part necessary ?? ////
+ //// = ReadWinchCurrentPosition(i2c_addr_resolver);
+ // ReadCurrentPositon(i2c_addr_resolver, &winchData, sizeof(winchData), 0x00 );
+ // winchCurrentPosition = winchData.dt_WinchCntPosition;
+ ////DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\n", winchCurrentPosition );
+
+ flg_exp_status |= 0x01000000;
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 0;
+ // hwbtn_OpeMutex.unlock();
+
+ I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
+ I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
+ I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
+ I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+
+ read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_res, NumberOfI2CCommand );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ motor_current_pct = motor1_current_pct;
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+
+ flg_ButtonOn = false;
+ }
+ else if ( btnStatus_WUP == btnID_WUP ) { // Winch Up (Rvs)
+ flg_ButtonOn = true;
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 1;
+ // hwbtn_OpeMutex.unlock();
+ DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ DEBUG_PRINT_L3( "Bd0> BTN W-UP(Rvs)\n" );
+ DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
+ I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_R; // Speed
+ // I2C_cmd[6] = MOTOR_RVS; // Motor2 RVS
+ // I2C_cmd[7] = setValue.winchCtrl.sv_WRM_sli_R; // Speed
+ #endif
+ //// Are these part necessary ?? ////
+ ////winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
+ // ReadCurrentPositon(i2c_addr_resolver, &winchData, sizeof(winchData), 0x00 );
+ // winchCurrentPosition = winchData.dt_WinchCntPosition;
+ ////DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\n", winchCurrentPosition );
+
+ flg_exp_status |= 0x02000000;
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 0;
+ // hwbtn_OpeMutex.unlock();
+ I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
+ I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
+ I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
+ I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+
+ read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_res, NumberOfI2CCommand );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ motor_current_pct = motor1_current_pct;
+ motor_current_pct = I2C_res[0];
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+
+ flg_ButtonOn = false;
+ }
+ // ====================================
+ // ALL motor off commmand packet send
+ // ====================================
+ else {
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 1;
+ // hwbtn_OpeMutex.unlock();
+ led3 = 0;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[2] = MOTOR_STP;
+ I2C_cmd[3] = 0;
+ I2C_cmd[6] = MOTOR_STP;
+ I2C_cmd[7] = 0;
+ Thread::wait(5);
+ #endif
+ flg_exp_status &= 0xF0FFFFFF;
+ I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
+ I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
+ I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
+ I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 0;
+ // hwbtn_OpeMutex.unlock();
+
+ }
+// I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
+// I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
+// I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
+// I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
+// i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+// i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+// motor1_current_pct = I2C_res[0];
+// motor2_current_pct = I2C_res[1];
+// motor_current_pct = motor1_current_pct;
+// DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+
+ //// Are these part necessary ?? ////
+ ////winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
+ //ReadCurrentPositon(i2c_addr_resolver, &winchData, sizeof(winchData), 0x00 );
+ //winchCurrentPosition = winchData.dt_WinchCntPosition;
+ ////DEBUG_PRINT_L3("Bd0> Winch Current Position (button) : %04d\n", winchCurrentPosition );
+
+ /*
+ // ====================================
+ // TRANSFORM Motor Control
+ // ====================================
+ * 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+
+ * |o|x|x|x|x|x|x|x| 1: RF-I, 2: RF-K, 4: LB-I, 8: LB-K
+ * +-+-+-+-+-+-+-+-+
+ */
+
+ if ( btnStatus_RFK == btnID_RFK ) { // RF KO
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ DEBUG_PRINT_L3( "Bd0> BTN RF-K\n" );
+ DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 1;
+ // hwbtn_OpeMutex.unlock();
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
+ I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_F; // Speed
+ #endif
+ flg_exp_status |= 0x10000000;
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 0;
+ // hwbtn_OpeMutex.unlock();
+ }
+ else if ( btnStatus_RFI == btnID_RFI ) { // RF I
+ DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ DEBUG_PRINT_L3( "Bd0> BTN RF-I\n" );
+ DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 1;
+ // hwbtn_OpeMutex.unlock();
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
+ I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_R; // Speed
+ #endif
+ flg_exp_status |= 0x20000000;
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 0;
+ // hwbtn_OpeMutex.unlock();
+
+ I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
+ I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
+ I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
+ I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+ else if ( btnStatus_LBK == btnID_LBK ) { // LB KO
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ DEBUG_PRINT_L3( "Bd0> BTN LB-K\n" );
+ DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 1;
+ // hwbtn_OpeMutex.unlock();
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
+ I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_F; // Speed
+ #endif
+ flg_exp_status |= 0x40000000;
+
+ I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
+ I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
+ I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
+ I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 0;
+ // hwbtn_OpeMutex.unlock();
+ flg_ButtonOn = false;
+ }
+ else if ( btnStatus_LBI == btnID_LBI ) { // LB I
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ DEBUG_PRINT_L3( "Bd0> BTN LB-I\n" );
+ DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 1;
+ // hwbtn_OpeMutex.unlock();
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[6] = MOTOR_RVS; // Motor1 RVS
+ I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_R; // Speed
+ #endif
+ flg_exp_status |= 0x80000000;
+
+ I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
+ I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
+ I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
+ I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 0;
+ // hwbtn_OpeMutex.unlock();
+ flg_ButtonOn = false;
+ }
+ // ====================================
+ // ALL motor off commmand packet send
+ // ====================================
+ else {
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 1;
+ // hwbtn_OpeMutex.unlock();
+ led3 = 0;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[2] = MOTOR_STP;
+ I2C_cmd[3] = 0;
+ I2C_cmd[6] = MOTOR_STP;
+ I2C_cmd[7] = 0;
+ Thread::wait(5);
+ #endif
+ flg_exp_status &= 0x0FFFFFFF;
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 0;
+ // hwbtn_OpeMutex.unlock();
+ I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
+ I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
+ I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
+ I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+
+ }
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 1;
+ // hwbtn_OpeMutex.unlock();
+// I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
+// I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
+// I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
+// I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ // Send Command PAcket here !
+// i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+// i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board\]
+ // hwbtn_OpeMutex.lock();
+ // hwbtn_Opeflg = 0;
+ // hwbtn_OpeMutex.unlock();
+ }
+}
+
+uint32_t getc_fromHost( uint8_t *c ){
+
+ uint32_t rts = 0;
+
+ if(pc.readable()){
+ *c = pc.getc();
+ rts = 0;
+ }
+ else{
+ rts = 1;
+ }
+ return rts;
+}
+
+// **************************************************************
+// TASK: Hoat Interface Task
+//
+// **************************************************************
+void clientPC_interface_task(void const *)
+{
+ int rcv_data_cnt;
+ //winchData_t winchData;
+
+ char I2C_cmd[NumberOfI2CCommand+1] = "#010000";
+ char I2C_res[NumberOfI2CCommand+1] = "\0";
+
+// winchData_t winchData;
+ int16_t winchCurrentPosition;
+ char motor_current_pct;
+
+ while(1){
+
+ // DEBUG_PRINT("\nWaiting for UDP packet...\n");
+ rcv_data_cnt = udp_server.receiveFrom(client, dbuffer, sizeof(dbuffer));
+ Thread::wait(10);
+ if( rcv_data_cnt < 0 ){
+ DEBUG_PRINT_L0("Bd0> ## Receive packet fail ##\n");
+ }
+ else{
+ dbuffer[rcv_data_cnt] = '\0';
+ led4 = 1;
+
+ if(!strcmp( dbuffer, "Hello Z\n" )){
+ DEBUG_PRINT_L2("Bd0> Hello Z Packet received from client by UDP\n");
+ char snd_data[] = "Hello I'm CrExoB2";
+ udp_server.sendTo(client, snd_data, sizeof(snd_data));
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ else if(!strcmp( dbuffer, "status\n")){
+ DEBUG_PRINT_L2("Bd0> Hello Status req received from client by UDP\n");
+ strcpy(dbuffer,"XXXX\n");
+ udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ else if(!strcmp( dbuffer, "Hello")){
+ DEBUG_PRINT_L2( "Bd0> Hello Packet received from [ 0x%02x ]\n", flg_exp_status );
+
+ /* ***************************************** */
+ /* Read Winch Current Position from Resolver */
+ /* ***************************************** */
+ winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
+ Thread::wait(10);
+
+ // -------------------------------------
+ // Crawler Moving
+ // -------------------------------------
+ if( flg_exp_status & 0x00F00000 ){
+ // Forward move 5
+ // Reverce move a
+ // Right rotation 6
+ // Left rotation 9
+ if((flg_exp_status & 0x00F00000) == 0x00500000 ){
+ sprintf( dbuffer, "WCFW %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ // Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00a00000 ){
+ sprintf( dbuffer, "WCRV %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ // Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00600000 ){
+ sprintf( dbuffer, "WCRR %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ // Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00900000 ){
+ sprintf( dbuffer, "WCLR %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ // Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00800000 ){
+ sprintf( dbuffer, "LCRV %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ // Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00400000 ){
+ sprintf( dbuffer, "LCFW %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ // Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00200000 ){
+ sprintf( dbuffer, "RCRV %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ // Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00100000 ){
+ sprintf( dbuffer, "RCFW %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ // Thread::wait(10);
+ }
+ DEBUG_PRINT_L2("Bd0> >>>> Crawler >>>>>>>>>>>>>>>>>>>>\n");
+ DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
+ DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ // -------------------------------------
+
+ // Transform
+ // -------------------------------------
+ else if( flg_exp_status & 0x20000000 ){
+#ifdef __READ_CURRENT_AT_CIF_TASK__
+ read_motorCurrent( i2c_addr_transform, I2C_cmd, I2C_res, NumberOfI2CCommand );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ motor_current_pct = motor1_current_pct;
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+#endif // __READ_CURRENT_AT_CIF_TASK__
+ sprintf(dbuffer,"RF2I %03d %04d\0", motor_current_pct, winchCurrentPosition ); // RF Crawler Tfm I
+ DEBUG_PRINT_L2("Bd0> >>>> Transform >>>>>>>>>>>>>>>>>>\n");
+ DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
+ DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ else if( flg_exp_status & 0x10000000 ){
+#ifdef __READ_CURRENT_AT_CIF_TASK__
+ read_motorCurrent( i2c_addr_transform, I2C_cmd, I2C_res, NumberOfI2CCommand );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ motor_current_pct = motor1_current_pct;
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+#endif // __READ_CURRENT_AT_CIF_TASK__
+ sprintf(dbuffer,"RF2K %03d %04d\0", motor_current_pct, winchCurrentPosition ); // RF Crawler Tfm K
+ DEBUG_PRINT_L2("Bd0> >>>> Transform >>>>>>>>>>>>>>>>>>\n");
+ DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
+ DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ else if( flg_exp_status & 0x80000000 ){
+#ifdef __READ_CURRENT_AT_CIF_TASK__
+ read_motorCurrent( i2c_addr_transform, I2C_cmd, I2C_res, NumberOfI2CCommand );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ motor_current_pct = motor1_current_pct;
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+#endif // __READ_CURRENT_AT_CIF_TASK__
+ sprintf(dbuffer,"LB2I %03d %04d\0", motor_current_pct, winchCurrentPosition ); // LB Crawler Tfm I
+ DEBUG_PRINT_L2("Bd0> >>>> Transform >>>>>>>>>>>>>>>>>>\n");
+ DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
+ DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ else if( flg_exp_status & 0x40000000 ){
+#ifdef __READ_CURRENT_AT_CIF_TASK__
+ read_motorCurrent( i2c_addr_transform, I2C_cmd, I2C_res, NumberOfI2CCommand );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ motor_current_pct = motor1_current_pct;
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+#endif // __READ_CURRENT_AT_CIF_TASK__
+ sprintf(dbuffer,"LB2K %03d %04d\0", motor_current_pct, winchCurrentPosition ); // LB Crawler Tfm K
+ DEBUG_PRINT_L2("Bd0> >>>> Transform >>>>>>>>>>>>>>>>>>\n");
+ DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
+ DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ // -------------------------------------
+ // Winch Moving
+ // -------------------------------------
+ else if( flg_exp_status & 0x01000000 ){ // Wincd down (FWD)
+#ifdef __READ_CURRENT_AT_CIF_TASK__
+ read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_res, NumberOfI2CCommand );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ motor_current_pct = motor1_current_pct;
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+#endif // __READ_CURRENT_AT_CIF_TASK__
+ sprintf(dbuffer,"WCDN %03d %04d\0", motor_current_pct, winchCurrentPosition ); // Winch down
+ DEBUG_PRINT_L2("Bd0> >>>> Winch >>>>>>>>>>>>>>>>>>>>>>\n");
+ DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
+ DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ else if( flg_exp_status & 0x02000000 ){ // Winch up (RVS)
+#ifdef __READ_CURRENT_AT_CIF_TASK__
+ read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_res, NumberOfI2CCommand );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ motor_current_pct = motor1_current_pct;
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+#endif // __READ_CURRENT_AT_CIF_TASK__
+ sprintf(dbuffer,"WCUP %03d %04d\0", motor_current_pct, winchCurrentPosition ); // Winch down
+ DEBUG_PRINT_L2("Bd0> >>>> Winch >>>>>>>>>>>>>>>>>>>>>>\n");
+ DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
+ DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ else if( flg_exp_status & 0x0000000f ){
+#ifdef __READ_CURRENT_AT_CIF_TASK__
+ read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_res, NumberOfI2CCommand );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ motor_current_pct = motor1_current_pct;
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+#endif // __READ_CURRENT_AT_CIF_TASK__
+ sprintf( dbuffer,"JSMD %03d %04d\0", setValue.operation.sv_JS_ShapeMode, setValue.operation.sv_JS_OpeMode );
+ DEBUG_PRINT_L2("Bd0> >>>> Winch >>>>>>>>>>>>>>>>>>>>>>\n");
+ DEBUG_PRINT_L2("Bd0> Send JSMode: %s\n", dbuffer);
+ DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ else{
+#ifdef __READ_CURRENT_AT_CIF_TASK__
+ read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_res, NumberOfI2CCommand );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ motor_current_pct = motor1_current_pct;
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+ DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+#endif // __READ_CURRENT_AT_CIF_TASK__
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ // sprintf( dbuffer,"OFF_ %03d %04d\0", motor_current_pct, winchCurrentPosition ); // Winch down
+ }
+ /* Send data to client PC */
+ DEBUG_PRINT_L2(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ DEBUG_PRINT_L2(">>>> Send data 2 client PC >>>>\n");
+ DEBUG_PRINT_L2(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
+ Thread::wait(10);
+ }
+ }
+ Thread::wait(100); // When this period seto to short time, gamepad command can not get. 50msec OK
+ led4 = 0;
+ Thread::wait(100); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+}
+
+// **************************************************************
+// TASK: GamaPad Task
+//
+// **************************************************************
+void gamepad_task(void const *)
+{
+
+ char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
+
+// int counter = 0;
+ // USB HOST GAMEPAD
+ USBHostGamepad gamepad;
+
+ led1 = 1;
+
+ while(1) {
+
+ // try to connect a USB Gamepad
+ while(!gamepad.connect()) {
+ flg_gamePad_Connected = 0;
+ led2 = OFF;
+ // led1 = 1;
+ Thread::wait(500);
+ }
+
+ // when connected, attach handler called on gamepad event
+ gamepad.attachEvent(onControl);
+
+ // wait until the Gamepad is disconnected
+ while(gamepad.connected()) {
+ flg_gamePad_Connected = 1;
+ led2 = !led2;
+ led2 = ON;
+ // led1 = 1;
+
+ I2C_cmd[4] = 0x00;
+ I2C_cmd[5] = 0x01;
+ i2c.write(i2c_addr_handy, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ DEBUG_PRINT_L2("Bd0>Send Handy Controller(%02x) >>>>>>>>>>\n",i2c_addr_handy );
+
+ Thread::wait(500);
+ }
+ }
+}
+
+// ======================================================================
+// Write setting value from lpcal file system of mbed
+// ======================================================================
+void write_Setting_lfs( void )
+{
+ FILE *wfp;
+
+ DEBUG_PRINT_L3("\nBd0> Local file system write ... ");
+ wfp = fopen("/local/set.dat", "wb"); // Open local file "set.txt" for writing
+ if(!wfp){
+ DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\n");
+ }
+ else{
+ Thread::wait(200);
+ fwrite( &setValue, sizeof( int8_t ),sizeof(setValue), wfp );
+ Thread::wait(200);
+ fclose(wfp);
+
+ DEBUG_PRINT_L3("file writen\n");
+ }
+}
+
+// ======================================================================
+// Read setting value from lpcal file system of mbed
+// ======================================================================
+int read_Setting_lfs( void )
+{
+ FILE *rfp;
+
+ DEBUG_PRINT_L3("\nBd0> Read Setting data from local file system \n");
+ DEBUG_PRINT_L3( "Bd0> ---------------------------------------- \n");
+ rfp = fopen("/local/set.dat", "rb"); // Open local file "set.txt" for writing
+ if(!rfp){
+ DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\n");
+ return _FAIL_;
+ }
+ else{
+ Thread::wait(500);
+
+ fread( (int8_t*)&setValue, sizeof(int8_t), sizeof(setValue), rfp );
+ dspSetValue2Console(&pc, &setValue);
+
+ Thread::wait(500);
+ fclose(rfp);
+ DEBUG_PRINT_L3("\n");
+
+ return _OK_;
+ }
+}
+
+// ======================================================================
+// Read Network setting value from lpcal file system of mbed
+// ======================================================================
+int read_NetSetting_lfs( char* ip_address, char* subnet_mask, char* gateway )
+{
+ FILE *rfp;
+// char ip_address[20];
+// char subnet_mask[20];
+// char gateway[20];
+
+ DEBUG_PRINT_L3("\nBd0> Read Network Setting data from local file system \n");
+ DEBUG_PRINT_L3( "Bd0> nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn \n");
+ rfp = fopen("/local/net.dat", "r"); // Open local file "set.txt" for writing
+ if(!rfp){
+ DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\n");
+ return _FAIL_;
+ }
+ else{
+ Thread::wait(500);
+
+ fscanf(rfp, "%s", ip_address);
+ fscanf(rfp, "%s", subnet_mask);
+ fscanf(rfp, "%s", gateway);
+
+ DEBUG_PRINT_L3("Bd0> nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn\n");
+ DEBUG_PRINT_L3("Bd0> ip address : %s\n", ip_address);
+ DEBUG_PRINT_L3("Bd0> subnet mask : %s\n", subnet_mask);
+ DEBUG_PRINT_L3("Bd0> default gateway: %s\n", gateway);
+ DEBUG_PRINT_L3("Bd0> nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn\n");
+
+ Thread::wait(500);
+ fclose(rfp);
+ DEBUG_PRINT_L3("\n");
+
+ return _OK_;
+ }
+}
+// ======================================================================
+// Winch control function
+// ======================================================================
+void winchMovingControl(
+ int mode, // Operationg mode: Relative / Abslute
+ char* dbufferP, // Date buffer pointer
+ int dbuffer_s, // Date buffer size
+ winchData_t* winchDataP, // Winch data structure pointer
+ int winchData_s, // Winch data structure size
+ char* I2C_cmd
+){
+ int rcv_data_cnt;
+// int moving_data;
+ int man_speed;
+
+ bool flg_stop_operation = false;
+
+ uint16_t winchCurrentPosition;
+
+ char I2C_read[NumberOfI2CCommand+1];
+ char I2C_resdata[2]; // Resolver read data
+
+// if (hwbtn_Opeflg == 1){
+// Thread::wait(1);
+// }
+// else{
+
+
+ if( flg_ButtonOn == true ) {Thread::wait(2);}
+
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+
+ if( mode == WINCH_POSITION_CLEAR ){
+ led3 = ON;
+ DEBUG_PRINT_L3("Bd0> === WINCH_POSITION_CLEAR ===\n");
+ rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\n", rcv_data_cnt );
+ if( rcv_data_cnt < 0 ){
+ DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\n" );
+ // tcp_client.send( (char*)winchDataP, winchData_s);
+ // break;
+ }
+ else{
+ DEBUG_PRINT_L3("Bd0> %s\n", dbuffer );
+ // if( !strcmp( dbuffer, "WinchStepDnOf" ) ){
+ // break;
+ // }
+ }
+
+ I2C_cmd[1] = 'Z'; // Zero clear
+ i2c.write(i2c_addr_resolver, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ led3 = OFF;
+ }
+ else if (( mode == WINCH_MMODE_RELATIVE ) || ( mode == WINCH_MMODE_ABSOLUTE )){
+ if ( mode == WINCH_MMODE_RELATIVE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_RELATIVE === \n");
+ if ( mode == WINCH_MMODE_ABSOLUTE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_ABSOLUTE === \n");
+
+ rcv_data_cnt = tcp_client.receive( dbuffer, dbuffer_s);
+
+ *(dbufferP+rcv_data_cnt) = '\0';
+ winchDataP->operation = '\n';
+
+ DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\n", rcv_data_cnt );
+ // Copy received data to Winch data structure.
+ memcpy( winchDataP, (winchData_t *)dbuffer, winchData_s);
+ //// ##########################
+ ////zzz winchDataP->dt_WinchCntPosition = res_position; // Current Position is winch data. But currently this value will be clear after reset MC.
+ //// ##########################
+ // winchDataP->dt_WinchDstPosition += winchDataP->dt_WinchCntPosition;
+ DEBUG_PRINT_L3("Bd0> Winch Rtv Move [ From %04d >>> To %04d ]\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
+
+ swbtn_OpeMutex.lock();
+ swbtn_Opeflg = 1;
+ swbtn_OpeMutex.unlock();
+
+ while( true ){
+ while( true ){
+ led3 = ON;
+ ////winchDataP->dt_WinchCntPosition = res_position; // Current position.
+
+ DEBUG_PRINT_L3("Bd0> == Winch Position ==============\n");
+ DEBUG_PRINT_L3("Bd0> CURRENT : %d\n", winchDataP->dt_WinchCntPosition );
+ DEBUG_PRINT_L3("Bd0> DESTINATION: %d\n", winchDataP->dt_WinchDstPosition );
+ DEBUG_PRINT_L3("Bd0> ================================\n");
+
+ tcp_client.send( (char*)winchDataP, winchData_s);
+ DEBUG_PRINT_L3("Bd0> Send Winch Rtv data [ %04d >>>> %04d ]\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
+
+ rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\n", rcv_data_cnt );
+ if( rcv_data_cnt < 0 ){
+ DEBUG_PRINT_L3("##ERROR## Data receive\n" );
+
+ // tcp_client.send( (char*)winchDataP, winchData_s);
+ break;
+ }
+ else{
+ DEBUG_PRINT_L3("Bd0> %s\n", dbuffer );
+ if( !strcmp( dbuffer, "WinchRtvStop" ) ){
+ flg_stop_operation = true;
+ break;
+ }
+ }
+ if( winchDataP->dt_WinchCntPosition < winchDataP->dt_WinchDstPosition ){
+ I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
+ if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
+ I2C_cmd[3] = setValue.winchCtrl.sv_WDM_mst; // Speed
+ }
+ else{
+ I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_F; // Speed
+ }
+ }
+ else{
+ I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
+ if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
+ I2C_cmd[3] = setValue.winchCtrl.sv_WDM_mrt;; // Speed
+ }
+ else{
+ I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_R; // Speed
+ }
+ }
+
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ // Read winch current position from Resolver.
+ winchCurrentPosition = ReadWinchCurrentPosition( i2c_addr_resolver );
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ winchDataP->operation = 0x00;
+ //i2c.read(i2c_addr_resolver, I2C_resdata, 2); // Read
+ //res_position = (I2C_resdata[1] << 8) | I2C_resdata[0];
+ DEBUG_PRINT_L3("Bd0> DISTANCE from [ 0x%2x : %04x (%02x %02x) ]\n", i2c_addr_resolver, winchCurrentPosition, I2C_resdata[1], I2C_resdata[0]);
+ // --------------------------------------
+ // Read motor current
+ // --------------------------------------
+ read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_read, NumberOfI2CCommand );
+ winchDataP->dt_WinchMotor1Current = I2C_read[0]; // Motor current set
+ winchDataP->dt_WinchMotor2Current = I2C_read[1]; // Motor current set
+ winchDataP->operation = I2C_read[2];
+ DEBUG_PRINT_L3( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L3( "Bd0> ReadMotorCurrent (%d)%\n", winchDataP->dt_WinchMotor1Current );
+ DEBUG_PRINT_L3( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\n", i2c_addr_winch, I2C_read[0]);
+ led3 = OFF;
+ if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) <= 0 ){
+ break;
+ }
+ Thread::wait(10); // Time interval for program debugging
+ }
+ I2C_cmd[2] = MOTOR_STP; // Motor1 STOP
+ I2C_cmd[3] = 0; // Speed
+ I2C_cmd[6] = MOTOR_STP; // Motor2 STOP
+ I2C_cmd[7] = 0; // Speed
+
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ Thread::wait(500); // Time interval for program debugging
+
+ winchCurrentPosition = ReadWinchCurrentPosition( i2c_addr_resolver );
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ winchDataP->operation = 0x00;
+ //winchCurrentPosition = res_position; // Set curretn winch position that send for Host PC here when auto move mode.
+ DEBUG_PRINT_L3("Bd0> Check destination is same to setting point or not: %d\n", winchCurrentPosition);
+ if( winchDataP->dt_WinchDstPosition == winchDataP->dt_WinchCntPosition ){
+ DEBUG_PRINT_L3("Bd0> Destination is same to setting point, then stop operation\n" );
+ break; // When final destination == set point , then break. else adjust position again.
+ }
+ if( flg_stop_operation == true ){
+ DEBUG_PRINT_L3("Bd0> Winch auto operation force stop\n" );
+ flg_stop_operation = false;
+ break;
+ }
+ }
+
+ // Thread::wait(500); // Time interval for program debugging
+
+ winchCurrentPosition = ReadWinchCurrentPosition( i2c_addr_resolver );
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ winchDataP->operation = 0x77;
+ DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\n", winchCurrentPosition);
+
+ tcp_client.send( (char*)winchDataP, winchData_s);
+
+ swbtn_OpeMutex.lock();
+ swbtn_Opeflg = 0;
+ swbtn_OpeMutex.unlock();
+
+ led3 = OFF;
+ }
+
+ // ----------------------------------------------------------
+ // In case of commad received from PC by TCP connection.
+ // In case of hard ware button pushed is by gamepad task
+ // ----------------------------------------------------------
+ else if (( mode == WINCH_STEPDOWN_BTN_ON )||( mode == WINCH_U_STEPDOWN_BTN_ON )) {
+
+ if ( mode == WINCH_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPDOWN_BTN_ON ===\n" );
+ if ( mode == WINCH_U_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPDOWN_BTN_ON ===\n" );
+
+ swbtn_OpeMutex.lock();
+ swbtn_Opeflg = 1;
+ swbtn_OpeMutex.unlock();
+ man_speed = 50;
+ while( 1 ){
+ led3 = ON;
+ DEBUG_PRINT_L3("Bd0> WINCH_STEPDOWN_BTN_ON\n");
+ rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\n", rcv_data_cnt );
+ if( rcv_data_cnt < 0 ){
+ DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\n" );
+ // tcp_client.send( (char*)winchDataP, winchData_s);
+ break;
+ }
+ else{
+ DEBUG_PRINT_L3("Bd0> %s\n", dbuffer );
+ if(( !strcmp( dbuffer, "WinchStepDnOf" ))||( !strcmp( dbuffer, "WinchuStepDnOf" )) ){
+ break;
+ }
+ }
+
+ if ( mode == WINCH_U_STEPDOWN_BTN_ON ) man_speed = 50;
+ else man_speed = 100;
+
+ I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
+ I2C_cmd[3] = man_speed; // Speed
+
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ led3 = OFF;
+
+ read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_read, NumberOfI2CCommand );
+ winchDataP->dt_WinchMotor1Current = I2C_read[0];
+ winchDataP->dt_WinchMotor2Current = I2C_read[1];
+ winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
+ DEBUG_PRINT_L3( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L3( "Bd0> ReadMotorCurrent (%d)%\n", winchDataP->dt_WinchMotor1Current );
+ DEBUG_PRINT_L3( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ if( winchDataP->operation == 0x88 ){
+ winchDataP->dt_WinchMotor1Current = 0xFF;
+ }
+
+ DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\n", i2c_addr_winch, I2C_read[0]);
+ winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition;
+ winchDataP->operation = 0x00;
+ // ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x00 );
+ tcp_client.send( (char*)winchDataP, winchData_s);
+ DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\n", winchCurrentPosition );
+
+ Thread::wait(10); // Time interval for program debugging
+ }
+ DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\n" );
+ I2C_cmd[2] = MOTOR_STP; // Motor1 FWD
+ I2C_cmd[3] = 0; // Speed
+ I2C_cmd[6] = MOTOR_STP; // Motor2 FWD
+ I2C_cmd[7] = 0; // Speed
+
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\n" );
+ winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition;
+ winchDataP->operation = 0x77;
+ //ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x77 );
+ tcp_client.send( (char*)winchDataP, winchData_s);
+
+ swbtn_OpeMutex.lock();
+ swbtn_Opeflg = 0;
+ swbtn_OpeMutex.unlock();
+ led3 = OFF;
+ }
+ // ----------------------------------------------------------
+ // In case of commad received from PC by TCP connection.
+ // In case of hard ware button pushed is by gamepad task
+ // ----------------------------------------------------------
+ else if (( mode == WINCH_STEPUP_BTN_ON )||( mode == WINCH_U_STEPUP_BTN_ON )) {
+
+ if ( mode == WINCH_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPUP_BTN_ON ===\n" );
+ if ( mode == WINCH_U_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPUP_BTN_ON ===\n" );
+
+ swbtn_OpeMutex.lock();
+ swbtn_Opeflg = 1;
+ swbtn_OpeMutex.unlock();
+ man_speed = 1;
+ while( 1 ){
+ led3 = ON;
+ DEBUG_PRINT_L3("Bd0> WINCH_STEPUP_BTN_ON\n");
+ rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\n", rcv_data_cnt );
+ if( rcv_data_cnt < 0 ){
+ DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\n" );
+ // tcp_client.send( (char*)winchDataP, winchData_s);
+ break;
+ }
+ else{
+ DEBUG_PRINT_L3("Bd0> %s\n", dbuffer );
+ if(
+ ( !strcmp( dbuffer, "WinchStepUpOf" ))
+ ||
+ (!strcmp( dbuffer, "WinchuStepUpOf" ))
+ ){
+ break;
+ }
+ }
+
+ if ( mode == WINCH_U_STEPUP_BTN_ON ) man_speed = 50;
+ else man_speed = 100;
+
+ I2C_cmd[2] = MOTOR_RVS; // Motor1 FWD
+ I2C_cmd[3] = man_speed; // Speed
+
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ led3 = OFF;
+
+ read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_read, NumberOfI2CCommand );
+ winchDataP->dt_WinchMotor1Current = I2C_read[0];
+ winchDataP->dt_WinchMotor2Current = I2C_read[1];
+ winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
+ DEBUG_PRINT_L3( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L3( "Bd0> ReadMotorCurrent (%d)%\n", winchDataP->dt_WinchMotor1Current );
+ DEBUG_PRINT_L3( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ if( winchDataP->operation == 0x88 ){
+ winchDataP->dt_WinchMotor1Current = 0xFF;
+ }
+
+ DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\n", i2c_addr_winch, I2C_read[0]);
+ winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition;
+ winchDataP->operation = 0x00;
+ // ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x00 );
+ tcp_client.send( (char*)winchDataP, winchData_s);
+ DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\n", winchCurrentPosition );
+
+ Thread::wait(10); // Time interval for program debugging
+ }
+ DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\n" );
+ I2C_cmd[2] = MOTOR_STP; // Motor1 FWD
+ I2C_cmd[3] = 0; // Speed
+ I2C_cmd[6] = MOTOR_STP; // Motor2 FWD
+ I2C_cmd[7] = 0; // Speed
+
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+
+ DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\n" );
+ winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition;
+ winchDataP->operation = 0x77;
+ // ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x77 );
+ tcp_client.send( (char*)winchDataP, winchData_s);
+
+ swbtn_OpeMutex.lock();
+ swbtn_Opeflg = 0;
+ swbtn_OpeMutex.unlock();
+
+ led3 = OFF;
+ }
+
+ else {
+ DEBUG_PRINT_L3("STEPSTEPSTEPSTEPSTEP\n");
+ }
+// }
+}
+
+// **********************************************************************
+//
+// Main Function of this program
+//
+// **********************************************************************
+int main()
+{
+ winchData_t winchData;
+
+ char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
+// char I2C_res[NumberOfI2CCommand+1] = "\0";
+
+ char ip_address[20];;
+ char subnet_mask[20];
+ char gateway[20];
+
+ int ret;
+ int try_cnt;
+ int rcv_data_cnt;
+
+ bool flg_ethernet = false;
+
+ char ttt[20];
+
+ // Set UART(USB) baudrate
+ pc.baud(115200);
+
+ cf_led_demo( &led1, &led2, &led3, &led4, 15, 35 );
+
+ DEBUG_PRINT_L0("\r\n");
+ DEBUG_PRINT_L0("Bd0> +--------------------------------------------------------------\n");
+ DEBUG_PRINT_L0("Bd0> | Project: B2 Crawler Explorer for 1F-1 PCV internal inspection\n");
+ DEBUG_PRINT_L0("Bd0> |---------\n");
+ DEBUG_PRINT_L0("Bd0> | This is: Main Control Program of Main Controller\n");
+ DEBUG_PRINT_L0("Bd0> | Target MCU: mbed LPC1768\r\n");
+ DEBUG_PRINT_L0("Bd0> | Letest update: %s\r\n", LatestUpDate);
+ DEBUG_PRINT_L0("Bd0> | Program Revision: %s\r\n", ProgramRevision);
+ DEBUG_PRINT_L0("Bd0> | Author: %s\r\n", Author);
+ DEBUG_PRINT_L0("Bd0> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
+ DEBUG_PRINT_L0("Bd0> ---------------------------------------------------------------\n");
+ sprintf( ttt, "%s", ProgramRevision );
+
+ DEBUG_PRINT_L0("Bd0> Start ststem initializing ...\n");
+ DEBUG_PRINT_L0("Bd0> ------------------------\n");
+ DEBUG_PRINT_L0("Bd0> Initalizing Ethernet ...\n");
+ DEBUG_PRINT_L0("Bd0> ------------------------\n");
+
+ read_NetSetting_lfs( ip_address, subnet_mask, gateway );
+
+ DEBUG_PRINT_L0("Bd0> nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn\n");
+ DEBUG_PRINT_L0("Bd0> ip address : %s\n", ip_address);
+ DEBUG_PRINT_L0("Bd0> subnet mask : %s\n", subnet_mask);
+ DEBUG_PRINT_L0("Bd0> default gateway: %s\n", gateway);
+ DEBUG_PRINT_L0("Bd0> nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn\n");
+
+#ifdef __ETERNET_DHCP__
+ ret = eth.init(); // Use DHCP
+#else // __ETERNET_DHCP__
+ ret = eth.init(
+ ip_address, // const char* ip,
+ subnet_mask, // const char* mask,
+ gateway // const char* gateway
+ );
+#endif // __ETERNET_DHCP__
+ if( ret == 0 ){
+ DEBUG_PRINT_L0("Bd0> Eternet init ... OK\n");
+ ret = eth.connect();
+ if( ret == 0 ){
+ cf_led_onoff( &led1,&led2,&led3,&led4, false, false, false, true );
+ DEBUG_PRINT_L0("Bd0> Eternat connect ... OK\n");
+ DEBUG_PRINT_L0("Bd0> [ IP Address : %s ]\n", eth.getIPAddress());
+ udp_server.bind(UDP_SERVER_PORT);
+ tcp_server.bind(TCP_SERVER_PORT);
+ tcp_server.listen();
+ flg_ethernet = true;
+ }
+ else{
+ cf_led_error( &led1,&led2,&led3,&led4 );
+ DEBUG_PRINT_L0("Bd0> ***ERROR*** Eternat connect Fali\n");
+ }
+ }
+ else{
+ DEBUG_PRINT_L0("Bd0> *** ERROR*** Eternat init Fail\n");
+ }
+
+ Thread::wait(500);
+
+ //---------------------------------------------------
+ // Read CrExp setting value from Local File System
+ // setting file "SET.DAT".
+ // When error occured, LED1 will be blinking shortly.
+ //---------------------------------------------------
+ DEBUG_PRINT_L0("Bd0> ---------------------------\n");
+ DEBUG_PRINT_L0("Bd0> Read setting value from LFS\n");
+ DEBUG_PRINT_L0("Bd0> ---------------------------\n");
+ try_cnt = LFS_READ_COUNT;
+ while( 1 ){
+ if( read_Setting_lfs() == _OK_ ) break;
+ else try_cnt -= 1;
+ if( try_cnt == 0 ){
+ DEBUG_PRINT_L0("Bd0> ***ERROR*** LFS read error\n");
+ while(1){
+ led1 = !led1;
+ Thread::wait(30);
+ }
+ }
+ }
+ DEBUG_PRINT_L0("Bd0> LFS read OK\n");
+
+ //---------------------------------------------------
+ // Checking Targer LCXpresso824-MAX board here.
+ // Send Hello Packet and waiting reply message from
+ // target.
+ // When error occured, LED1 will blinking slowly.
+ //---------------------------------------------------
+ DEBUG_PRINT_L0("--------------------------\n");
+ DEBUG_PRINT_L0("Check the target controler\n");
+ DEBUG_PRINT_L0("--------------------------\n");
+/*
+ try_cnt = TARGET_CHECK_COUNT;
+ while(1){
+ // **********************************************************************
+ // **********************************************************************
+ // Check each target motor control 824 board here
+ // currently only one target checked for debugging ...
+ // **********************************************************************
+ // **********************************************************************
+ I2C_cmd[1] = MOTOR_FWD;
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(100);
+ i2c.read(i2c_addr_winch, I2C_res, NumberOfI2CCommand);
+ DEBUG_PRINT_L0("Bd0> Try count down: %d/%d\n", try_cnt, TARGET_CHECK_COUNT );
+ DEBUG_PRINT_L0("Bd0> Return from [ i2c address = 0x%2x : %s ]\n", i2c_addr_winch>>1, I2C_res);
+ DEBUG_PRINT_L0("Bd0> MCTR1[Winch motor controller] found\n");
+ if( I2C_res[0] == 'S' ){
+ break;
+ }
+ else try_cnt -= 1;
+ if( try_cnt == 0 ){
+ DEBUG_PRINT_L0("Bd0> ##ERROR##\n");
+ led1 = OFF;
+ while(1){
+ led1 = !led1; // ON
+ Thread::wait(80);
+ led1 = !led1; // OFF
+ Thread::wait(80);
+ led1 = !led1; // ON
+ Thread::wait(80);
+ led1 = !led1; // OFF
+ Thread::wait(80);
+ led1 = !led1; // ON
+ Thread::wait(80);
+ led1 = !led1; // OFF
+ Thread::wait(80);
+ led1 = !led1; // ON
+ Thread::wait(80);
+ led1 = !led1; // OFF
+ Thread::wait(500);
+ }
+ }
+ }
+ DEBUG_PRINT_L0("Bd0> Target system found\n");
+*/
+ /*
+ **************************************************
+ Send Calculation Data to Resolver Controller
+ **************************************************
+ */
+ DEBUG_PRINT_L0("Bd0> Send Calculation Data to Resolver Controller");
+ I2C_cmd[2] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100>>8)&0xFF); // Dram diameter upper
+ I2C_cmd[3] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100)&0xFF); // Dram diameter lower
+ I2C_cmd[4] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100>>8)&0xFF); // Cable diameter upper
+ I2C_cmd[5] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100)&0xFF); // Cable diameter lower
+ I2C_cmd[6] = setValue.winchCtrl.sv_WRS_RResolution; // Resolver resolution
+ i2c.write(i2c_addr_resolver, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ DEBUG_PRINT_L0(" ... done\n");
+
+ DEBUG_PRINT_L0("Bd0> ----------------------------------------\n");
+ /* Max thread count is (may be ..) 2, How can I increase this , I don't know ?? */
+ DEBUG_PRINT_L0("Bd0> Start host interface task ... ");
+ Thread thread_hif(clientPC_interface_task, NULL, osPriorityHigh, 256 * 2);
+ DEBUG_PRINT_L0("\nBd0> Start host gamepad task ... ");
+ Thread thread_gpd(gamepad_task, NULL, osPriorityHigh, 256 * 2);
+ DEBUG_PRINT_L0("\nBd0> ----------------------------------------\n");
+
+ DEBUG_PRINT_L0( "Bd0> ------------------------\n");
+ DEBUG_PRINT_L0( "Bd0> Initializing completed !\n");
+ DEBUG_PRINT_L0( "Bd0> ------------------------\n");
+
+ led1 = ON; // Initializing is OK then Power Indicator LED ON
+
+ I2C_cmd[4] = 0x00;
+ I2C_cmd[5] = 0x01;
+ i2c.write(i2c_addr_handy, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ while( true ) {
+ Thread::wait(10);
+
+ // -----------------------------------------------------------------
+ // Communicate with client PC program.
+ // TCP connection:
+ // -----------------------------------------------------------------
+ if( flg_ethernet == true ) // in case of Ethernet OK
+ {
+ tcp_server.accept(tcp_client);
+ tcp_client.set_blocking(false, 3500); // Timeout after (300) msec
+ DEBUG_PRINT_L3("Bd0> ----------------------------\n");
+ DEBUG_PRINT_L3("Bd0> TCP Connection from: %s\n", tcp_client.get_address());
+ DEBUG_PRINT_L3("Bd0> ----------------------------\n");
+
+ while(true){
+
+ rcv_data_cnt = tcp_client.receive(dbuffer, sizeof(dbuffer));
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\n", rcv_data_cnt );
+ if( rcv_data_cnt < 0 ){
+ // DEBUG_PRINT("## TCP Receive packet fail ##\n");
+ break;
+ }
+ else{
+ if( !strcmp( dbuffer, "WinchPositionClear" ) ){
+ winchMovingControl(
+ WINCH_POSITION_CLEAR,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ }
+
+ else if( !strcmp( dbuffer, "WinchRtvStart" ) ){
+ winchMovingControl(
+ WINCH_MMODE_RELATIVE,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ }
+ else if( !strcmp( dbuffer, "WinchAbsStart" ) ){
+ winchMovingControl(
+ WINCH_MMODE_ABSOLUTE,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ }
+ else if( !strcmp( dbuffer, "WinchStepUpOn" ) ){
+ winchMovingControl(
+ WINCH_STEPUP_BTN_ON,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ }
+ else if( !strcmp( dbuffer, "WinchStepUpOf" ) ){
+ winchMovingControl(
+ WINCH_STEPUP_BTN_OFF,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ }
+ else if( !strcmp( dbuffer, "WinchStepDnOn" ) ){
+ winchMovingControl(
+ WINCH_STEPDOWN_BTN_ON,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ }
+ else if( !strcmp( dbuffer, "WinchStepDnOf" ) ){
+ winchMovingControl(
+ WINCH_STEPDOWN_BTN_OFF,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ }
+
+ else if( !strcmp( dbuffer, "WinchuStepUpOn" ) ){
+ winchMovingControl(
+ WINCH_U_STEPUP_BTN_ON,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ }
+ else if( !strcmp( dbuffer, "WinchuStepUpOf" ) ){
+ winchMovingControl(
+ WINCH_U_STEPUP_BTN_OFF,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ }
+ else if( !strcmp( dbuffer, "WinchuStepDnOn" ) ){
+ winchMovingControl(
+ WINCH_U_STEPDOWN_BTN_ON,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ }
+ else if( !strcmp( dbuffer, "WinchuStepDnOf" ) ){
+ winchMovingControl(
+ WINCH_U_STEPDOWN_BTN_OFF,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ }
+
+ else if( !strcmp( dbuffer, "SetValue" ) ){
+ DEBUG_PRINT_L3("Bd0> SetValue Request from client\n");
+ rcv_data_cnt = tcp_client.receive( dbuffer, sizeof(dbuffer));
+ DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\n", rcv_data_cnt );
+ memcpy( &setValue, (setValue_t *)dbuffer, sizeof( setValue)) ;
+ // Display setting value list to consol
+ dspSetValue2Console( &pc, &setValue );
+ // Write setting data to local file system
+ // Thread::wait(100);
+ write_Setting_lfs();
+ // Thread::wait(500);
+ // Read setting data from local file system
+ if ( read_Setting_lfs() == _FAIL_ ){
+ DEBUG_PRINT_L0("Bd0> ### ERROR### Can't read out setting data from lfs\n");
+ }
+ }
+ else if(!strcmp( dbuffer, "GetValue" )){
+ DEBUG_PRINT_L3("Bd0> GetValue request from TCP client <-- send");
+ read_Setting_lfs();
+ // Display setting value list to consol
+ dspSetValue2Console( &pc, &setValue );
+ tcp_client.send_all( (char*)&setValue, sizeof(setValue));
+ DEBUG_PRINT_L2("(%d)\n", sizeof(setValue));
+ }
+ }
+ if( rcv_data_cnt <= 0 ) break;
+ }
+ tcp_client.close();
+ }
+ }
+}