z ysaito
/
Pro_B2_0ZNR_B2DebrisSurvayor_1768LAN
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: EthernetInterface TextLCD USBDevice USBHost3 mbed
Fork of
USBHostHub_HelloWorld
by 
Samuel Mokrani
Revision 15:01680ed6b799, committed 2018-07-26
- Comitter:
- sayzyas
- Date:
- Thu Jul 26 00:20:39 2018 +0000
- Parent:
- 14:3a5ae61ab1f4
- Child:
- 16:1adb3f1e867d
- Commit message:
- 2018.07.26
Changed in this revision
--- a/0_main.cpp Thu Apr 14 10:25:08 2016 +0000
+++ b/0_main.cpp Thu Jul 26 00:20:39 2018 +0000
@@ -1,2753 +1,3075 @@
-
-/***************************************
- * 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 "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 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
-// I2C setting
-I2C i2c_res(p28, p27); // I2C SDA, SCL is not good ???
-I2C i2c(p9, p10); // I2C SDA, SCL is good
-
-// 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 local("local"); // Create the local filesystem under the name "local"
-
-// Global
-uint32_t flg_gamePad_Connected = 0;
-char PC_cmd[11+1] = "&0100000000";
-basic_operation_t baseOperation;
-char dbuffer[128];
-uint8_t motor_speed = 0;
-
-// 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 flg_mutex;
-Mutex swbtn_OpeMutex;
-int swbtn_Opeflg = 0;
-
-int16_t winchCurrentPosition;
-
-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';
-
-
-/* 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_readcmd,
- char* I2C_res,
- int NumberOfI2Cdata
-){
- i2c.read(i2c_addr, I2C_res, NumberOfI2Cdata);
- /*
- DEBUG_PRINT_L0(" ++++++++++++++++++++++++++++++\r\n" );
- DEBUG_PRINT_L0(" Read Motor1 Current [%d]\r\n", I2C_res[0] );
- DEBUG_PRINT_L0(" Read Motor2 Current [%d]\r\n", I2C_res[1] );
- DEBUG_PRINT_L0(" Read [%d]\r\n", I2C_res[2] );
- DEBUG_PRINT_L0(" ++++++++++++++++++++++++++++++\r\n" );
- */
-}
-
-// ============================================================
-// 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 = 0;
- int rts;
-
- rts = i2c.read(i2c_addr, I2C_data, 2); // Read
- mtx_wcp.lock();
- if( rts == 0 ){
- res_position = (I2C_data[1] << 8) | I2C_data[0];
- }
- else{
- res_position = -1;
- }
- // winchCurrentPosition = res_position;
- mtx_wcp.unlock();
- return res_position;
-}
-
-
-char I2C_res[NumberOfI2CCommand+1] = "\0";
-// ============================================================
-// 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] = "#0100000000000";
- char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
-
- 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_RFLBI = 0; // RF-I and LB-I both button on same time
- uint8_t btnID_RFLBK = 0; // RF-K and LB-K both button on same time
- 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;
- // ---- This is temporary setting ------------------------------
- btnID_RFLBI = 192; // RF-I and LB-I both button on same time
- btnID_RFLBK = 48; // RF-K and LB-K both button on same time
- // --------------------------------------------------------------
- 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_WDN = btn04;
- btnStatus_WUP = 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_CrossUp = btn06;
- btnStatus_CrossDn = btn06;
- btnStatus_CrossRt = btn06;
- btnStatus_CrossLt = btn06;
- btnStatus_Start = btn05; //
- }
- }
- else if (gamePadVID == GAMEPAD_VID_ELECOM ){
- DEBUG_PRINT_L4("Bd0> [ELECOM] ");
- btnID_WDN = 4;
- btnID_WUP = 2;
- btnID_RFK = 32;
- btnID_RFI = 128;
- btnID_LBK = 16;
- btnID_LBI = 64;
- // ---------------------
- btnID_RFLBI = 192; // RF-I and LB-I both button on same time
- btnID_RFLBK = 48; // RF-K and LB-K both button on same time
- // ---------------------
- 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_WDN = btn04;
- btnStatus_WUP = 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_WDN = 40;
- btnID_WUP = 136;
- 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_WDN = btn05;
- btnStatus_WUP = 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_WDN = btn04;
- btnStatus_WUP = 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_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_WDN = 2;
- btnID_WUP = 4;
- btnID_RFK = 2;
- btnID_RFI = 1;
- btnID_LBK = 128;
- btnID_LBI = 64;
- // ---------------------
- btnID_RFLBI = 80; // RF-I and LB-I both button on same time
- btnID_RFLBK = 40; // RF-K and LB-K both button on same time
- // ---------------------
- btnID_CrossUp = 0;
- btnID_CrossDn = 255;
- btnID_CrossRt = 0;
- btnID_CrossLt = 255;
- if ( gamePadPID == GAMEPAD_PID_SANWA_JYP70US ){
- btnStatus_WDN = btn05;
- btnStatus_WUP = btn05;
- btnStatus_RFK = btn06;
- btnStatus_RFI = btn06;
- btnStatus_LBK = btn05;
- btnStatus_LBI = btn05;
- 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_CrossUp = btn01;
- btnStatus_CrossDn = btn01;
- btnStatus_CrossRt = btn00;
- btnStatus_CrossLt = btn00;
- }
- }
-
-#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_SW("Bd0> -- Button Status -------------------------------\r\n");
- DEBUG_PRINT_SW("Bd0> 00(%02x) 01(%02x) 02(%02x) 03(%02x)\r\n", btn00,btn01,btn02,btn03);
- DEBUG_PRINT_SW("Bd0> 04(%02x) 05(%02x) 06(%02x) 07(%02x) 08(%02x)\r\n", btn04,btn05,btn06,btn07,btn08);
- DEBUG_PRINT_SW("Bd0> 09(%02x) 10(%02x) 11(%02x) 12(%02x) 13(%02x) 14(%02x) 15(%02x)\r\n", btn09,btn10,btn11,btn12,btn13,btn14,btn15);
- DEBUG_PRINT_SW("Bd0> ------------------------------------------------\r\n");
-#endif
-
- I2C_cmd[I2C_CP_M1_DIR] = '\0';
- I2C_cmd[I2C_CP_M1_SPEED] = '\0';
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = '\0';
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = '\0';
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = '\0';
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = '\0';
- I2C_cmd[I2C_CP_M2_DIR] = '\0';
- I2C_cmd[I2C_CP_M2_SPEED] = '\0';
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = '\0';
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = '\0';
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = '\0';
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = '\0';
-
- int tmpSpeed = 0;
-
- if (swbtn_Opeflg == 1){
- Thread::wait(1);
- }
- else{
- if( flg_lsw_valid == true ){
- I2C_cmd[1] = 'V';
- }
- else{
- I2C_cmd[1] = '0';
- }
- if( gamePadVID == GAMEPAD_VID_RSTHANDY ){ // NRst own original controller
- flg_exp_status &= 0xFFFFFFF0;
- if(!( btnStatus_Start & 0x01 )){ // I-Shape
- flg_mutex.lock();
- baseOperation.sv_JS_ShapeMode = 0;
- baseOperation.sv_WinchValid = 0;
- flg_mutex.unlock();
- flg_exp_status |= 0x00000001;
- }
- else{ // KO-Shape
- flg_mutex.lock();
- baseOperation.sv_JS_ShapeMode = 1;
- flg_mutex.unlock();
- flg_exp_status |= 0x00000002;
- }
-
- if(!(btnStatus_Start & 0x02 )){ // Tfm,crawler part valid
- flg_mutex.lock();
- baseOperation.sv_WinchValid = 0;
- flg_mutex.unlock();
- flg_exp_status |= 0x00000004;
- }
- else{ // Winch part valid
- flg_mutex.lock();
- baseOperation.sv_WinchValid = 1;
- flg_mutex.unlock();
- flg_exp_status |= 0x00000008;
- }
- DEBUG_PRINT_L4( "-----------------------------\r\n" );
- DEBUG_PRINT_L4( "%d : %d\r\n",btnStatus_Start, flg_exp_status );
- DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
- DEBUG_PRINT_L4( "-----------------------------\r\n" );
- }
- else{
- /*
- *
- * GamePad software switch
- * Cross button Up on : JS shape mode I
- * Cross button Down on : JS shape mode KO
- * Cross button Right on: Winch part valid
- * Cross button Left on : Crawlerm, Transform part valid
- * 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: Left part(Crawler, Tfm) part valid, 8: Winch part valid
- * +-+-+-+-+-+-+-+-+
- */
- if ( btnStatus_CrossUp == btnID_CrossUp ){ // I Shape
- flg_mutex.lock();
- baseOperation.sv_JS_ShapeMode = 0;
- flg_mutex.unlock();
- DEBUG_PRINT_L4( "--------------------------------\r\n" );
- DEBUG_PRINT_L4( " I\r\n" );
- DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
- DEBUG_PRINT_L4( "--------------------------------\r\n" );
- flg_exp_status |= 0x00000001;
- }
- else if( btnStatus_CrossDn == btnID_CrossDn ){ // KO Shape
- flg_mutex.lock();
- baseOperation.sv_JS_ShapeMode = 1;
- flg_mutex.unlock();
- DEBUG_PRINT_L4( "-------------------------\r\n" );
- DEBUG_PRINT_L4( " KO\r\n" );
- DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
- DEBUG_PRINT_L4( "-------------------------\r\n" );
- flg_exp_status |= 0x00000002;
- }
- else if( btnStatus_CrossRt == btnID_CrossLt ){ // Valid Part : Crawler (Left)
- flg_mutex.lock();
- baseOperation.sv_WinchValid = 0;
- flg_mutex.unlock();
- DEBUG_PRINT_L4( "-----------------------------\r\n" );
- DEBUG_PRINT_L4( " Tfm, Crawler\r\n" );
- DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
- DEBUG_PRINT_L4( "-----------------------------\r\n" );
- flg_exp_status |= 0x00000004;
- }
- else if( btnStatus_CrossLt == btnID_CrossRt ){ // Valid Part : Winch (Right)
- flg_mutex.lock();
- baseOperation.sv_WinchValid = 1;
- flg_mutex.unlock();
- DEBUG_PRINT_L4( "-----------------------------\r\n" );
- DEBUG_PRINT_L4( " Winch\r\n" );
- DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
- DEBUG_PRINT_L4( "-----------------------------\r\n" );
- flg_exp_status |= 0x00000008;
- }
- else{
- flg_exp_status &= 0xFFFFFFF0;
- }
- }
- /*
- // ====================================
- // 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_RFLBK)&&(baseOperation.sv_WinchValid==0)){ // Both sw on
- flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> BTN RF-K & LB-K\r\n" );
- led3 = 1;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
- I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_F; // Speed
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor1 FWD
- I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_F; // Speed
- #endif
- flg_exp_status |= 0x30000000;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- #ifdef __READ_TFM_MOTOR_CURRENT__
- read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- limitSw_Sts = I2C_res[3];
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
- DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
- #endif // __IIC_COMAMND_SEND__
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
-
- else if ((btnStatus_RFI== btnID_RFLBI)&&(baseOperation.sv_WinchValid==0)){ // Both sw on
- flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> BTN RF-I & LB-I\r\n" );
- led3 = 1;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 FWD
- I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_F; // Speed
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor1 FWD
- I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_F; // Speed
- #endif
- flg_exp_status |= 0x10000000;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- #ifdef __READ_TFM_MOTOR_CURRENT__
- read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- limitSw_Sts = I2C_res[3];
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
- DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
- #endif // __IIC_COMAMND_SEND__
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
-
- else if ((btnStatus_RFK==btnID_RFK)&&(baseOperation.sv_WinchValid==0)){ // RF KO
- flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> BTN RF-K\r\n" );
- led3 = 1;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
- I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_F; // Speed
- #endif
- flg_exp_status |= 0x10000000;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- #ifdef __READ_TFM_MOTOR_CURRENT__
- read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- limitSw_Sts = I2C_res[3];
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
- DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
- #endif // __IIC_COMAMND_SEND__
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
- else if ((btnStatus_RFI==btnID_RFI)&&(baseOperation.sv_WinchValid==0)){ // RF I
- DEBUG_PRINT_L3( "Bd0> BTN RF-I\r\n" );
- led3 = 1;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
- I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_R; // Speed
- #endif
- flg_exp_status |= 0x20000000;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- #ifdef __READ_TFM_MOTOR_CURRENT__
- read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- limitSw_Sts = I2C_res[3];
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
- DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
- #endif // __READ_TFM_MOTOR_CURRENT__
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
-
- else if ((btnStatus_LBK==btnID_LBK)&&(baseOperation.sv_WinchValid==0)){ // LB KO
- flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> BTN LB-K\r\n" );
- led3 = 1;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 FWD
- I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_F; // Speed
- #endif
- flg_exp_status |= 0x40000000;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- #ifdef __READ_TFM_MOTOR_CURRENT__
- read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- limitSw_Sts = I2C_res[3];
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
- DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
- #endif // __READ_TFM_MOTOR_CURRENT__
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
- else if ((btnStatus_LBI==btnID_LBI)&&(baseOperation.sv_WinchValid==0)) { // LB I
- flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> BTN LB-I\r\n" );
- led3 = 1;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor1 RVS
- I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_R; // Speed
- #endif
- flg_exp_status |= 0x80000000;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- #ifdef __READ_TFM_MOTOR_CURRENT__
- read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- limitSw_Sts = I2C_res[3];
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
- DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
- #endif // __READ_TFM_MOTOR_CURRENT__
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
- // ====================================
- // ALL motor off commmand packet send
- // -- This part isn't operated in case of general game controller, because no event. --
- // ====================================
- else if (baseOperation.sv_WinchValid==0){
- led3 = 0;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP;
- I2C_cmd[I2C_CP_M1_SPEED] = 0;
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP;
- I2C_cmd[I2C_CP_M2_SPEED] = 0;
- Thread::wait(5);
- #endif
- flg_exp_status &= 0x0FFFFFFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
-
- #ifdef __READ_TFM_MOTOR_CURRENT__
- read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- limitSw_Sts = I2C_res[3];
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
- DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
- #endif // __READ_TFM_MOTOR_CURRENT__
-
- }
-
- /*/ ====================================
- // Crawler Moving Control
- // ====================================
- // JoyStick mode 1: Independence mode ( 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( baseOperation.sv_JS_OpeMode == 1 ){
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_R & 0xFF );
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_R & 0xFF );
- if ((btnStatus_LJSFwdRvs<setValue.crawlerCtrl.sv_LBCM_dzc-setValue.crawlerCtrl.sv_LBCM_dzl)&&(baseOperation.sv_WinchValid==0)){
- flg_ButtonOn = true;
- led3 = 1;
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 FWD
- tmpSpeed = ( setValue.crawlerCtrl.sv_LBCM_dzc+1 - btnStatus_LJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc * setValue.crawlerCtrl.sv_LBCM_srto_F / 100; // Speed
- I2C_cmd[I2C_CP_M2_SPEED] = (char)tmpSpeed;
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Fwd (Speed=%d)\r\n", tmpSpeed);
- // Read motor current from target
- read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
-
- flg_exp_status |= 0x00400000;
- i2c.write(I2C_ADDRESS_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)&&(baseOperation.sv_WinchValid==0)){
- flg_ButtonOn = true;
- led3 = 1;
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 RVS
- tmpSpeed = ( btnStatus_LJSFwdRvs - setValue.crawlerCtrl.sv_LBCM_dzc ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc+1 * setValue.crawlerCtrl.sv_LBCM_srto_F / 100; // Speed I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = (char)tmpSpeed;
- I2C_cmd[I2C_CP_M2_SPEED] = (char)tmpSpeed;
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Rvs (Speed=%d)\r\n", tmpSpeed);
-
- // Read motor current from target
- read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
-
- flg_exp_status |= 0x00800000;
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
- else if (baseOperation.sv_WinchValid==0) {
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Stop
- I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed=0
- flg_exp_status &= 0xFF3FFFFF;
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- }
-
- if ((btnStatus_RJSFwdRvs<setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)&&(baseOperation.sv_WinchValid==0)){
- flg_ButtonOn = true;
- led3 = 1;
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
- tmpSpeed = (( 128 - btnStatus_RJSFwdRvs ) * 100 / 127 ) * setValue.crawlerCtrl.sv_RFCM_srto_R / 100;
- I2C_cmd[I2C_CP_M1_SPEED] = (char)tmpSpeed;
- DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Fwd (Speed=%d)\r\n", tmpSpeed);
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
-
- // Read motor current from target
- read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
-
- flg_exp_status |= 0x00100000;
- i2c.write(I2C_ADDRESS_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)&&(baseOperation.sv_WinchValid==0)){
- flg_ButtonOn = true;
- led3 = 1;
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
- tmpSpeed = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc+1 * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
- I2C_cmd[I2C_CP_M1_SPEED] = (char)tmpSpeed;
- DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Rvs (Speed=%d)\r\n", tmpSpeed);
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
-
- // Read motor current from target
- read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
-
- flg_exp_status |= 0x00200000;
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
- else if (baseOperation.sv_WinchValid==0) {
- DEBUG_PRINT_L2("***** MOTOR STOP ****\r\n");
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Stop
- I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed=0
- flg_exp_status &= 0xFFCFFFFF;
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- }
- led3 = 0;
- }
- /* JoyStick mode 0: Syncronous mode ( 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
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_R & 0xFF );
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_R & 0xFF );
- 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 + 50) &&
- ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50 ) &&
- ( baseOperation.sv_WinchValid == 0)
- ){
- flg_ButtonOn = true;
- led3 = 1;
- if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Reverse
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
- }
- else{
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Fwd
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
- }
- I2C_cmd[I2C_CP_M1_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
- I2C_cmd[I2C_CP_M2_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- DEBUG_PRINT_L3( "Bd0> Single Mode: Dir1 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_RVS_CNTTH_U], I2C_cmd[I2C_CP_M2_SPEED]);
- flg_exp_status |= 0x00500000;
- //flg_exp_status |= 0x00400000; // 0x00500000
-
- // Read motor current from target
- read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
-
- i2c.write(I2C_ADDRESS_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 + 50 ) &&
- ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
- ( baseOperation.sv_WinchValid == 0)
- ){
- flg_ButtonOn = true;
- led3 = 1;
- if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Reverse
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor1 Reverse
- }
- else{
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Rvs
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
- }
- I2C_cmd[I2C_CP_M1_SPEED] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
- I2C_cmd[I2C_CP_M2_SPEED] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- DEBUG_PRINT_L3( "Bd0> Single Mode: Dir2 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_RVS_CNTTH_U], I2C_cmd[I2C_CP_M2_SPEED]);
- flg_exp_status |= 0x00600000;
- //flg_exp_status |= 0x00400000; // 0x00600000
-
- // Read motor current from target
- read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
-
- i2c.write(I2C_ADDRESS_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 + 50) &&
- ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
- ( baseOperation.sv_WinchValid == 0)
- ){
- flg_ButtonOn = true;
- led3 = 1;
- if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Rvs
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
- }
- else{
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Rvs
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
- }
- I2C_cmd[I2C_CP_M1_SPEED] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
- I2C_cmd[I2C_CP_M2_SPEED] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- DEBUG_PRINT_L3( "Bd0> Single Mode: Dir3 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_RVS_CNTTH_U], I2C_cmd[I2C_CP_M2_SPEED]);
- flg_exp_status |= 0x00A00000;
- //flg_exp_status |= 0x00800000; // 0x00A00000
- // Read motor current from target
-
- read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
-
- i2c.write(I2C_ADDRESS_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 + 50) &&
- ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
- ( baseOperation.sv_WinchValid == 0)
- ){
- flg_ButtonOn = true;
- led3 = 1;
- if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Reverse
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor1 Reverse
- }
- else{
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Fwd
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
- }
- I2C_cmd[I2C_CP_M1_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
- I2C_cmd[I2C_CP_M2_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- DEBUG_PRINT_L3( "Bd0> Single Mode: Dir4 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_RVS_CNTTH_U], I2C_cmd[I2C_CP_M2_SPEED]);
- flg_exp_status |= 0x00900000;
- //flg_exp_status |= 0x00800000; // 0x00900000
-
- // Read motor current from target
- read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
- motorLock_sts = I2C_res[4];
- if( motorLock_sts == '1' ){
- motor1_current_pct = 999;
- }
- else{
- motor1_current_pct = I2C_res[0];
- }
- if( motorLock_sts == '2' ){
- motor2_current_pct = 999;
- }
- else{
- motor2_current_pct = I2C_res[1];
- }
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
-
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
- // ====================================
- // ALL motor off commmand packet send
- // ====================================
- else if (baseOperation.sv_WinchValid==0) {
- led3 = 0;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Fwd
- I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed=0
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 Rvs
- I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed=0
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // Thread::wait(5);
- #endif
- flg_exp_status &= 0xFF0FFFFF;
- // I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- // I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- // I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- // I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- }
- }
-
- /*
- * ====================================
- * 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) && (baseOperation.sv_WinchValid == 1)){ // Winch Down (FWD)
- flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> BTN W-DN(Fwd)\r\n" );
- led3 = 1;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
- I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_F; // Speed
- // I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 FWD
- // I2C_cmd[I2C_CP_M2_SPEED] = setValue.winchCtrl.sv_WRM_hsrto_F; // Speed
- #endif
- flg_exp_status |= 0x01000000;
-
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_R & 0xFF );
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_R & 0xFF );
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
- else if (( btnStatus_WUP == btnID_WUP ) && (baseOperation.sv_WinchValid == 1)) { // Winch Up (Rvs)
- flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> BTN W-UP(Rvs)\r\n" );
- led3 = 1;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
- I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_R; // Speed
- // I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 RVS
- // I2C_cmd[I2C_CP_M2_SPEED] = setValue.winchCtrl.sv_WRM_hsrto_R; // Speed
- #endif
- flg_exp_status |= 0x02000000;
-
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_R & 0xFF );
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_R & 0xFF );
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
- // ====================================
- // ALL motor off commmand packet send
- // ====================================
- else if (baseOperation.sv_WinchValid == 1){
- led3 = 0;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP;
- I2C_cmd[I2C_CP_M1_SPEED] = 0;
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP;
- I2C_cmd[I2C_CP_M2_SPEED] = 0;
- Thread::wait(5);
- #endif
- flg_exp_status &= 0xF0FFFFFF;
-
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_R & 0xFF );
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_F >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_F & 0xFF );
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_R >> 8 ) & 0xFF;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_R & 0xFF );
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- }
- }
-}
-
-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
-//
-// **************************************************************
-int first_counter = 0;
-void clientPC_interface_task(void const *)
-{
- int rcv_data_cnt;
- //winchData_t winchData;
-
- char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
-
-// winchData_t winchData;
-// int16_t winchCurrentPosition;
- int16_t winchTempPosition;
-
- int cnt = 0;
-
- while(1){
-
- // DEBUG_PRINT("\r\nWaiting for UDP packet...\r\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 ##\r\n");
- }
- else{
- dbuffer[rcv_data_cnt] = '\0';
- led4 = 1;
-
- if(!strcmp( dbuffer, "Hello Z\r\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\r\n")){
- DEBUG_PRINT_L2("Bd0> Hello Status req received from client by UDP\r\n");
- strcpy(dbuffer,"XXXX\r\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, "set_jss")){ // Single JS mode
- baseOperation.sv_JS_OpeMode = 0;
- }
- else if(!strcmp( dbuffer, "set_jsd")){ // Dual JS mode
- baseOperation.sv_JS_OpeMode = 1;
- }
- else if(!strcmp( dbuffer, "lsw_valid")){ // Limit swich detection valid
- flg_lsw_valid = true;
- }
- else if(!strcmp( dbuffer, "lsw_invalid")){ // Limit swich detection invalid
- flg_lsw_valid = false;
- }
- else if(!strcmp( dbuffer, "Hello")){
- DEBUG_PRINT_L2( "Bd0> Hello Packet received from [ 0x%02x ]\r\n", flg_exp_status );
-
- /* ***************************************** */
- /* Read Winch Current Position from Resolver */
- /* ***************************************** */
- if (baseOperation.sv_WinchValid == 1){ // read winch current position operation is valid in case of winch part is valid.
- winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
- if( winchTempPosition != -1 ){
- mtx_wcp.lock();
- winchCurrentPosition = winchTempPosition;
- mtx_wcp.unlock();
- }
- }
- Thread::wait(10);
-
- first_counter++;
- if( first_counter > 10 ) {
- sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts); // Winch down
- first_counter = 10;
- }
- else{
- sprintf( dbuffer,"JSMD %03d %03d 0000 000\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid );
- }
- // -------------------------------------
- // 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 ){
- // 01234 5678 9012 34569
- sprintf( dbuffer, "BCFW %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
- Thread::wait(10);
- }
- else if((flg_exp_status & 0x00F00000) == 0x00a00000 ){
- sprintf( dbuffer, "BCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
- Thread::wait(10);
- }
- else if((flg_exp_status & 0x00F00000) == 0x00600000 ){
- sprintf( dbuffer, "BCRR %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
- Thread::wait(10);
- }
- else if((flg_exp_status & 0x00F00000) == 0x00900000 ){
- sprintf( dbuffer, "BCLR %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
- Thread::wait(10);
- }
- else if((flg_exp_status & 0x00F00000) == 0x00800000 ){
- sprintf( dbuffer, "LCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
- Thread::wait(10);
- }
- else if((flg_exp_status & 0x00F00000) == 0x00400000 ){
- sprintf( dbuffer, "LCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
- Thread::wait(10);
- }
- else if((flg_exp_status & 0x00F00000) == 0x00200000 ){
- sprintf( dbuffer, "RCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
- Thread::wait(10);
- }
- else if((flg_exp_status & 0x00F00000) == 0x00100000 ){
- sprintf( dbuffer, "RCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
- Thread::wait(10);
- }
- DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
- pc.printf("\t\t\t S2C: %s\r\n", dbuffer);
- Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
- }
- // -------------------------------------
- // Transform
- // -------------------------------------
- else if( flg_exp_status & 0x20000000 ){
- sprintf(dbuffer,"RF2I %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // RF Crawler Tfm I
- DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
- pc.printf("\t\t\t S2C: %s\r\n", dbuffer);
- Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
- }
- else if( flg_exp_status & 0x10000000 ){
- sprintf(dbuffer,"RF2K %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // RF Crawler Tfm K
- DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
- Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
- }
- else if( flg_exp_status & 0x80000000 ){
- sprintf(dbuffer,"LB2I %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // LB Crawler Tfm I
- DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
- Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
- }
- else if( flg_exp_status & 0x40000000 ){
- sprintf(dbuffer,"LB2K %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // LB Crawler Tfm K
- DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
- 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)
- sprintf(dbuffer,"WCDN %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
- DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
- 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)
- sprintf(dbuffer,"WCUP %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
- DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
- Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
- }
- else if( flg_exp_status & 0x0000000f ){
- if( cnt == 3 ){
- sprintf( dbuffer,"JSMD %03d %03d 0000 000\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid );
- cnt = 0;
- }
- else{
- sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
- }
- cnt++;
- DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
- Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
- }
- else{
- sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
- Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
- }
- /* Send data to client PC */
- udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
- Thread::wait(10);
-
- // ----------------------------------------------------------------------------
- // Read target(transform controller) status in each time.
- read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 4 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
- limitSw_Sts = I2C_res[3];
- // DEBUG_PRINT_L2( "Bd0> X: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- // DEBUG_PRINT_L2( "Bd0> X: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- // DEBUG_PRINT_L2( "Bd0> X: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
- // ----------------------------------------------------------------------------
-
- }
- }
- // 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;
-
- // Send status to Handy Ctrl controller, but currently this is only for Main Controller.
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = 0x00;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = 0x01;
- i2c.write(I2C_ADDRESS_HANDY, I2C_cmd, NumberOfI2CCommand);
- // DEBUG_PRINT_L2("Bd0> Send Handy Controller(%02x) >>>>>>>>>>\r\n",I2C_ADDRESS_HANDY );
-
- Thread::wait(500);
- }
- }
-}
-
-// ======================================================================
-// Read setting value from lpcal file system of mbed
-// ======================================================================
-
-bool read_LFS( setValue_t* setValue ){
- FILE *fp;
- char *fname = "/local/set.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,"%04d",&data);setValue->winchCtrl.sv_WDM_ith_F=data; pc.printf("%04d",setValue->winchCtrl.sv_WDM_ith_F); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WDM_ith_R=data; pc.printf("%04d",setValue->winchCtrl.sv_WDM_ith_R); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRM_ith_F=data; pc.printf("%04d",setValue->winchCtrl.sv_WRM_ith_F); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRM_ith_R=data; pc.printf("%04d",setValue->winchCtrl.sv_WRM_ith_R); fgets(s,100,fp); pc.printf("%s",s);
-
- fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_hsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_hsrto_F); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_hsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_hsrto_R); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_lsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_lsrto_F); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_lsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_lsrto_R); fgets(s,100,fp); pc.printf("%s",s);
-
- fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_hsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_hsrto_F); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_hsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_hsrto_R); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_lsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_lsrto_F); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_lsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_lsrto_R); fgets(s,100,fp); pc.printf("%s",s);
-
- fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRS_DramDmrx100=data; pc.printf("%04d",setValue->winchCtrl.sv_WRS_DramDmrx100); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRS_CCableDmrx100=data; pc.printf("%04d",setValue->winchCtrl.sv_WRS_CCableDmrx100); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRS_RResolution=data; pc.printf("%03d",setValue->winchCtrl.sv_WRS_RResolution); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->winchCtrl.reserved=data; pc.printf("%03d",setValue->winchCtrl.reserved); fgets(s,100,fp); pc.printf("%s",s);
-
- fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_RFTM_ith_F=data; pc.printf("%04d",setValue->tfmCtrl.sv_RFTM_ith_F); fgets(s, 100, fp ); pc.printf("%s",s);
- fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_RFTM_ith_R=data; pc.printf("%04d",setValue->tfmCtrl.sv_RFTM_ith_R); fgets(s, 100, fp ); pc.printf("%s",s);
- fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_LBTM_ith_F=data; pc.printf("%04d",setValue->tfmCtrl.sv_LBTM_ith_F); fgets(s, 100, fp ); pc.printf("%s",s);
- fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_LBTM_ith_R=data; pc.printf("%04d",setValue->tfmCtrl.sv_LBTM_ith_R); fgets(s, 100, fp ); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_RFTM_srto_F=data; pc.printf("%03d",setValue->tfmCtrl.sv_RFTM_srto_F); fgets(s, 100, fp ); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_RFTM_srto_R=data; pc.printf("%03d",setValue->tfmCtrl.sv_RFTM_srto_R); fgets(s, 100, fp ); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_LBTM_srto_F=data; pc.printf("%03d",setValue->tfmCtrl.sv_LBTM_srto_F); fgets(s, 100, fp ); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_LBTM_srto_R=data; pc.printf("%03d",setValue->tfmCtrl.sv_LBTM_srto_R); fgets(s, 100, fp ); pc.printf("%s",s);
-
- fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_RFCM_ith_F=data; pc.printf("%04d",setValue->crawlerCtrl.sv_RFCM_ith_F); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_RFCM_ith_R=data; pc.printf("%04d",setValue->crawlerCtrl.sv_RFCM_ith_R); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_LBCM_ith_F=data; pc.printf("%04d",setValue->crawlerCtrl.sv_LBCM_ith_F); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_LBCM_ith_R=data; pc.printf("%04d",setValue->crawlerCtrl.sv_LBCM_ith_R); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_srto_F=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_srto_F); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_srto_R=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_srto_R); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_srto_F=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_srto_F); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_srto_R=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_srto_R); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_dzu=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_dzu); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_dzc=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_dzc); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_dzl=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_dzl); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_dzu=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_dzu); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_dzc=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_dzc); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_dzl=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_dzl); fgets(s,100,fp); pc.printf("%s",s );
- fscanf(fp,"%03d",&data);setValue->crawlerCtrl.reserved1=data; pc.printf("%03d",setValue->crawlerCtrl.reserved1); fgets(s,100,fp); pc.printf("%s",s);
- fscanf(fp,"%03d",&data);setValue->crawlerCtrl.reserved2=data; pc.printf("%03d",setValue->crawlerCtrl.reserved2); 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;
-}
-
-bool write_LFS( setValue_t* setValue ){
- FILE *fp;
- char *fname = "/local/set.txt";
- bool rts = true;
-
- pc.printf("write setting data to setting file \r\n ");
-
- fp = fopen(fname, "w"); // Open "out.txt" on the local file system for writing
- if( fp != NULL ){
- fprintf(fp, "#### B2 DebrisServayor Setting ####\n");
- fclose(fp);
- }
- else{
- rts = false;
- pc.printf("File open error (0)\r\n");
- }
- Thread::wait(50);
- fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
- if( fp != NULL ){
- pc.printf("Writing ... %04d #WDM F-C th\n", setValue->winchCtrl.sv_WDM_ith_F);
- fprintf(fp, "%04d #WDM F-C th\n", setValue->winchCtrl.sv_WDM_ith_F);
- pc.printf("Writing ... %04d #WDM R-C th\n", setValue->winchCtrl.sv_WDM_ith_R);
- fprintf(fp, "%04d #WDM R-C th\n", setValue->winchCtrl.sv_WDM_ith_R);
- pc.printf("Writing ... %04d #WM2 F-C th\n", setValue->winchCtrl.sv_WRM_ith_F);
- fprintf(fp, "%04d #WM2 F-C th\n", setValue->winchCtrl.sv_WRM_ith_F);
- pc.printf("Writing ... %04d #WM2 R-C th\n", setValue->winchCtrl.sv_WRM_ith_R);
- fprintf(fp, "%04d #WM2 R-C th\n", setValue->winchCtrl.sv_WRM_ith_R);
- fclose(fp);
- }
- else{
- pc.printf("File open error (1)\r\n");
- }
- Thread::wait(50);
-
- fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
- if( fp != NULL ){
- pc.printf("Writing ... %03d #WDM F-HS\n", setValue->winchCtrl.sv_WDM_hsrto_F);
- fprintf(fp, "%03d #WDM F-HS\n", setValue->winchCtrl.sv_WDM_hsrto_F);
- pc.printf("Writing ... %03d #WDM R-HS\n", setValue->winchCtrl.sv_WDM_hsrto_R);
- fprintf(fp, "%03d #WDM R-HS\n", setValue->winchCtrl.sv_WDM_hsrto_R);
- pc.printf("Writing ... %03d #WDM F-LS\n", setValue->winchCtrl.sv_WDM_lsrto_F);
- fprintf(fp, "%03d #WDM F-LS\n", setValue->winchCtrl.sv_WDM_lsrto_F);
- pc.printf("Writing ... %03d #WDM R-LS\n", setValue->winchCtrl.sv_WDM_lsrto_R);
- fprintf(fp, "%03d #WDM R-LS\n", setValue->winchCtrl.sv_WDM_lsrto_R);
- fclose(fp);
- }
- else{
- rts = false;
- pc.printf("File open error (2)\r\n");
- }
- Thread::wait(50);
-
- fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
- if( fp != NULL ){
- pc.printf("Writing ... %03d #WM2 F-HS\n", setValue->winchCtrl.sv_WRM_hsrto_F);
- fprintf(fp, "%03d #WM2 F-HS\n", setValue->winchCtrl.sv_WRM_hsrto_F);
- pc.printf("Writing ... %03d #WM2 R-HS\n", setValue->winchCtrl.sv_WRM_hsrto_R);
- fprintf(fp, "%03d #WM2 R-HS\n", setValue->winchCtrl.sv_WRM_hsrto_R);
- pc.printf("Writing ... %03d #WM2 F-LS\n", setValue->winchCtrl.sv_WRM_lsrto_F);
- fprintf(fp, "%03d #WM2 F-LS\n", setValue->winchCtrl.sv_WRM_lsrto_F);
- pc.printf("Writing ... %03d #WM2 R-LS\n", setValue->winchCtrl.sv_WRM_lsrto_R);
- fprintf(fp, "%03d #WM2 R-LS\n", setValue->winchCtrl.sv_WRM_lsrto_R);
- fclose(fp);
- }
- else{
- rts = false;
- pc.printf("File open error (3)\r\n");
- }
- Thread::wait(50);
-
- fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
- if( fp != NULL ){
- pc.printf("Writing ... %04d #WD D-DMx100\n", setValue->winchCtrl.sv_WRS_DramDmrx100);
- fprintf(fp, "%04d #WD D-DMx100\n", setValue->winchCtrl.sv_WRS_DramDmrx100);
- pc.printf("Writing ... %04d #WCC Dx100\n", setValue->winchCtrl.sv_WRS_CCableDmrx100);
- fprintf(fp, "%04d #WCC Dx100\n", setValue->winchCtrl.sv_WRS_CCableDmrx100);
- pc.printf("Writing ... %03d #R-Res\n", setValue->winchCtrl.sv_WRS_RResolution);
- fprintf(fp, "%03d #R-Res\n", setValue->winchCtrl.sv_WRS_RResolution);
- pc.printf("Writing ... %03d #Res\n", setValue->winchCtrl.reserved);
- fprintf(fp, "%03d #res\n", setValue->winchCtrl.reserved);
- fclose(fp);
- }
- else{
- rts = false;
- pc.printf("File open error (4)\r\n");
- }
- Thread::wait(50);
-
- fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
- if( fp != NULL ){
- pc.printf("Writing ... %04d #R-TFM F-C\n", setValue->tfmCtrl.sv_RFTM_ith_F);
- fprintf(fp, "%04d #R-TFM F-C\n", setValue->tfmCtrl.sv_RFTM_ith_F);
- pc.printf("Writing ... %04d #R-TFM R-C\n", setValue->tfmCtrl.sv_RFTM_ith_R);
- fprintf(fp, "%04d #R-TFM R-C\n", setValue->tfmCtrl.sv_RFTM_ith_R);
- pc.printf("Writing ... %04d #L-TFM F-C\n", setValue->tfmCtrl.sv_LBTM_ith_F);;
- fprintf(fp, "%04d #L-TFM F-C\n", setValue->tfmCtrl.sv_LBTM_ith_F);;
- pc.printf("Writing ... %04d #L-TFM R-C\n", setValue->tfmCtrl.sv_LBTM_ith_R);
- fprintf(fp, "%04d #L-TFM R-C\n", setValue->tfmCtrl.sv_LBTM_ith_R);
- fclose(fp);
- }
- else{
- rts = false;
- pc.printf("File open error (5)\r\n");
- }
- Thread::wait(50);
-
- fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
- if( fp != NULL ){
- pc.printf("Writing ... %03d #R-TFM F-S\n", setValue->tfmCtrl.sv_RFTM_srto_F);
- fprintf(fp, "%03d #R-TFM F-S\n", setValue->tfmCtrl.sv_RFTM_srto_F);
- pc.printf("Writing ... %03d #R-TFM R-S\n", setValue->tfmCtrl.sv_RFTM_srto_R);
- fprintf(fp, "%03d #R-TFM R-S\n", setValue->tfmCtrl.sv_RFTM_srto_R);
- pc.printf("Writing ... %03d #L-TFM F-S\n", setValue->tfmCtrl.sv_LBTM_srto_F);
- fprintf(fp, "%03d #L-TFM F-S\n", setValue->tfmCtrl.sv_LBTM_srto_F);
- pc.printf("Writing ... %03d #L-TFM R-S\n", setValue->tfmCtrl.sv_LBTM_srto_R);
- fprintf(fp, "%03d #L-TFM R-S\n", setValue->tfmCtrl.sv_LBTM_srto_R);
- fclose(fp);
- }
- else{
- rts = false;
- pc.printf("File open error (6)\r\n");
- }
- Thread::wait(50);
-
- fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
- if( fp != NULL ){
- pc.printf("Writing ... %04d #RC F-C th\n", setValue->crawlerCtrl.sv_RFCM_ith_F);
- fprintf(fp, "%04d #RC F-C th\n", setValue->crawlerCtrl.sv_RFCM_ith_F);
- pc.printf("Writing ... %04d #RC R-C th\n", setValue->crawlerCtrl.sv_RFCM_ith_R);
- fprintf(fp, "%04d #RC R-C th\n", setValue->crawlerCtrl.sv_RFCM_ith_R);
- pc.printf("Writing ... %04d #LC F-C th\n", setValue->crawlerCtrl.sv_LBCM_ith_F);
- fprintf(fp, "%04d #LC F-C th\n", setValue->crawlerCtrl.sv_LBCM_ith_F);
- pc.printf("Writing ... %04d #LC R-C th\n", setValue->crawlerCtrl.sv_LBCM_ith_R);
- fprintf(fp, "%04d #LC R-C th\n", setValue->crawlerCtrl.sv_LBCM_ith_R);
- fclose(fp);
- }
- else{
- rts = false;
- pc.printf("File open error (7)\r\n");
- }
- Thread::wait(50);
-
- fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
- if( fp != NULL ){
- pc.printf("Writing ... %03d #RC F-S\n", setValue->crawlerCtrl.sv_RFCM_srto_F);
- fprintf(fp, "%03d #RC F-S\n", setValue->crawlerCtrl.sv_RFCM_srto_F);
- pc.printf("Writing ... %03d #RC R-S\n", setValue->crawlerCtrl.sv_RFCM_srto_R);
- fprintf(fp, "%03d #RC R-S\n", setValue->crawlerCtrl.sv_RFCM_srto_R);
- pc.printf("Writing ... %03d #LC F-S\n", setValue->crawlerCtrl.sv_LBCM_srto_F);
- fprintf(fp, "%03d #LC F-S\n", setValue->crawlerCtrl.sv_LBCM_srto_F);
- pc.printf("Writing ... %03d #LC R-S\n", setValue->crawlerCtrl.sv_LBCM_srto_R);
- fprintf(fp, "%03d #LC R-S\n", setValue->crawlerCtrl.sv_LBCM_srto_R);
- fclose(fp);
- }
- else{
- rts = false;
- pc.printf("File open error (8)\r\n");
- }
-
- fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
- if( fp != NULL ){
- pc.printf("Writing ... %03d #RJS U\n", setValue->crawlerCtrl.sv_RFCM_dzu);
- fprintf(fp, "%03d #RJS U\n", setValue->crawlerCtrl.sv_RFCM_dzu);
- pc.printf("Writing ... %03d #RJS C\n", setValue->crawlerCtrl.sv_RFCM_dzc);
- fprintf(fp, "%03d #RJS C\n", setValue->crawlerCtrl.sv_RFCM_dzc);
- pc.printf("Writing ... %03d #RJS L\n", setValue->crawlerCtrl.sv_RFCM_dzl);
- fprintf(fp, "%03d #RJS L\n", setValue->crawlerCtrl.sv_RFCM_dzl);
- pc.printf("Writing ... %03d #LJS U\n", setValue->crawlerCtrl.sv_LBCM_dzu);
- fprintf(fp, "%03d #LJS U\n", setValue->crawlerCtrl.sv_LBCM_dzu);
- fclose(fp);
- }
- else{
- rts = false;
- pc.printf("File open error (9)\r\n");
- }
-
- fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
- if( fp != NULL ){
- pc.printf("Writing ... %03d #LJS C\n", setValue->crawlerCtrl.sv_LBCM_dzc);
- fprintf(fp, "%03d #LJS C\n", setValue->crawlerCtrl.sv_LBCM_dzc);
- pc.printf("Writing ... %03d #LJS L\n", setValue->crawlerCtrl.sv_LBCM_dzl);
- fprintf(fp, "%03d #LJS L\n", setValue->crawlerCtrl.sv_LBCM_dzl);
- pc.printf("Writing ... %03d #res\n", setValue->crawlerCtrl.reserved1);
- fprintf(fp, "%03d #res\n", setValue->crawlerCtrl.reserved1);
- pc.printf("Writing ... %03d #res\n", setValue->crawlerCtrl.reserved2);
- fprintf(fp, "%03d #res\n", setValue->crawlerCtrl.reserved2);
- fclose(fp);
- }
- else{
- rts = false;
- pc.printf("File open error (10)\r\n");
- }
-
- pc.printf("settig file write completed \r\n ");
-
-
- return rts;
-}
-
-// ======================================================================
-// Read Network setting value from lpcal file system of mbed
-// ======================================================================
-int read_NetSetting_lfs( char* ip_address, char* subnet_mask, char* gateway )
-{
- FILE *rfp;
-
- DEBUG_PRINT_L3("Bd0> Read Network Setting data from local file system \r\n");
- rfp = fopen("/local/net.txt", "r"); // Open local file "set.txt" for writing
- if(!rfp){
- DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
- return _FAIL_;
- }
- else{
- Thread::wait(50);
- fscanf(rfp, "%s", ip_address);
- fscanf(rfp, "%s", subnet_mask);
- fscanf(rfp, "%s", gateway);
- fclose(rfp);
- 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;
-
- int cnt;
-
-
- bool flg_stop_operation = false;
-
- int16_t winchTempPosition;
-
- char I2C_read[NumberOfI2CCommand+1];
- char I2C_readcmd[NumberOfI2CCommand+1];
-
-// if (hwbtn_Opeflg == 1){
-// Thread::wait(1);
-// }
-// else{
-
-
- if( flg_ButtonOn == true ) {Thread::wait(2);}
-
- I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_R;
-
- if( mode == WINCH_POSITION_CLEAR ){
- led3 = ON;
- DEBUG_PRINT_L3("Bd0> === WINCH_POSITION_CLEAR ===\r\n");
- rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
- DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
- if( rcv_data_cnt < 0 ){
- DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
- // tcp_client.send( (char*)winchDataP, winchData_s);
- // break;
- }
- else{
- DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
- // if( !strcmp( dbuffer, "WinchStepDnOf" ) ){
- // break;
- // }
- }
-
- I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Zero clear
- i2c.write(I2C_ADDRESS_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 === \r\n");
- if ( mode == WINCH_MMODE_ABSOLUTE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_ABSOLUTE === \r\n");
-
- rcv_data_cnt = tcp_client.receive( dbuffer, dbuffer_s);
-
- *(dbufferP+rcv_data_cnt) = '\0';
- winchDataP->operation = '\r\n';
-
- DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
- // Copy received data to Winch data structure.
- memcpy( winchDataP, (winchData_t *)dbuffer, winchData_s);
- // winchDataP->dt_WinchDstPosition += winchDataP->dt_WinchCntPosition;
- DEBUG_PRINT_L3("Bd0> Winch Rtv Move [ From %04d >>> To %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
-
- swbtn_OpeMutex.lock();
- swbtn_Opeflg = 1;
- swbtn_OpeMutex.unlock();
-
- cnt = 0;
-
- while( true ){
- while( true ){
- led3 = ON;
- ////winchDataP->dt_WinchCntPosition = res_position; // Current position.
-
- DEBUG_PRINT_L3("Bd0> == Winch Position ==============\r\n");
- DEBUG_PRINT_L3("Bd0> CURRENT : %d\r\n", winchDataP->dt_WinchCntPosition );
- DEBUG_PRINT_L3("Bd0> DESTINATION: %d\r\n", winchDataP->dt_WinchDstPosition );
- DEBUG_PRINT_L3("Bd0> ================================\r\n");
-
- tcp_client.send( (char*)winchDataP, winchData_s);
- DEBUG_PRINT_L3("Bd0> Send Winch Rtv data [ %04d >>>> %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
-
- rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
- DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
- if( rcv_data_cnt < 0 ){
- DEBUG_PRINT_L3("##ERROR## Data receive\r\n" );
-
- // tcp_client.send( (char*)winchDataP, winchData_s);
- break;
- }
- else{
- DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
- if( !strcmp( dbuffer, "WinchRtvStop" ) ){
- flg_stop_operation = true;
- break;
- }
- }
- // Forward rotation : winch down
- if( winchDataP->dt_WinchCntPosition < winchDataP->dt_WinchDstPosition ){
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
- if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
- I2C_cmd[I2C_CP_M1_SPEED] = (setValue.winchCtrl.sv_WDM_lsrto_F >> 1); // very slow speed
- }
- else if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
- I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_lsrto_F; // slow speed
- }
- else{
- I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_F; // normal speed
- }
- }
- // Reverse rotation : winch up
- else if ( winchDataP->dt_WinchCntPosition > winchDataP->dt_WinchDstPosition ){
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
- if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
- I2C_cmd[I2C_CP_M1_SPEED] = (setValue.winchCtrl.sv_WDM_lsrto_R >> 1); // very slow speed
- }
- else if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
- I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_lsrto_R; // slow speed
- }
- else{
- I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_R; // normal speed
- }
- }
-
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- // Read winch current position from Resolver.
-
- winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
- if( winchTempPosition != -1 ){
- mtx_wcp.lock();
- winchCurrentPosition = winchTempPosition;
- mtx_wcp.unlock();
- }
- mtx_wcp.lock();
- winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
- mtx_wcp.unlock();
- winchDataP->operation = 0x00;
- //i2c.read(I2C_ADDRESS_RESOLVER, I2C_resdata, 2); // Read
- //res_position = (I2C_resdata[1] << 8) | I2C_resdata[0];
- // --------------------------------------
- // Read motor current
- // --------------------------------------
- read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
- 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> 15: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
- DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
- led3 = OFF;
- if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) == 0 ){
- // if( winchDataP->dt_WinchCntPosition == winchDataP->dt_WinchDstPosition ){
- DEBUG_PRINT_L3( "Bd0> Current:%d -> Destination:%d\r\n" , winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition);
- break;
- }
- Thread::wait(2); // Time interval for program debugging
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- }
-
-
-
- DEBUG_PRINT_L3( "Bd0> ! Winch Stop\r\n" );
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 STOP
- I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 STOP
- I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed
-
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-
- // Thread::wait(1000); // Time interval for program debugging
-
- winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
- if( winchTempPosition != -1 ){
- mtx_wcp.lock();
- winchCurrentPosition = winchTempPosition;
- mtx_wcp.unlock();
- }
-
-
- mtx_wcp.lock();
- winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
- mtx_wcp.unlock();
- winchDataP->operation = 0x00;
- DEBUG_PRINT_L3("Bd0> Check destination is same to setting point or not: %d\r\n", winchCurrentPosition);
- if( winchDataP->dt_WinchDstPosition == winchCurrentPosition ){
- cnt++;
- DEBUG_PRINT_L3("Bd0> Destination is same to setting point, then stop operation\r\n" );
- winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
- if( winchTempPosition != -1 ){
- mtx_wcp.lock();
- winchCurrentPosition = winchTempPosition;
- mtx_wcp.unlock();
- }
- if ( cnt >= 5 ){
- break; // When final destination == set point , then break. else adjust position again.
- }
- }
- // Force Stop by Stop button
- if( flg_stop_operation == true ){
- DEBUG_PRINT_L3("Bd0> Winch auto operation force stop\r\n" );
- flg_stop_operation = false;
- break;
- }
- }
-/*
- Thread::wait(300); // Time interval for program debugging
- winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
- if( winchTempPosition != -1 ){
- mtx_wcp.lock();
- winchCurrentPosition = winchTempPosition;
- mtx_wcp.unlock();
- }
- mtx_wcp.lock();
- winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
- mtx_wcp.unlock();
- winchDataP->operation = 0x00;
- DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
- tcp_client.send( (char*)winchDataP, winchData_s);
-
- Thread::wait(300); // Time interval for program debugging
- winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
- if( winchTempPosition != -1 ){
- mtx_wcp.lock();
- winchCurrentPosition = winchTempPosition;
- mtx_wcp.unlock();
- }
- mtx_wcp.lock();
- winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
- mtx_wcp.unlock();
- winchDataP->operation = 0x00;
- DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
- tcp_client.send( (char*)winchDataP, winchData_s);
-*/
- Thread::wait(300); // Time interval for program debugging
- winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
- if( winchTempPosition != -1 ){
- mtx_wcp.lock();
- winchCurrentPosition = winchTempPosition;
- mtx_wcp.unlock();
- }
- mtx_wcp.lock();
- winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
- mtx_wcp.unlock();
- winchDataP->operation = 0x77;
- DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
- 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 ===\r\n" );
- if ( mode == WINCH_U_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPDOWN_BTN_ON ===\r\n" );
-
- swbtn_OpeMutex.lock();
- swbtn_Opeflg = 1;
- swbtn_OpeMutex.unlock();
- while( 1 ){
- led3 = ON;
- DEBUG_PRINT_L3("Bd0> WINCH_STEPDOWN_BTN_ON\r\n");
- rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
- DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
- if( rcv_data_cnt < 0 ){
- DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
- // tcp_client.send( (char*)winchDataP, winchData_s);
- break;
- }
- else{
- DEBUG_PRINT_L3("Bd0> %s\r\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[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
- I2C_cmd[I2C_CP_M1_SPEED] = man_speed; // Speed
-
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-
- led3 = OFF;
-
- read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
- 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> 16: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
- if( winchDataP->operation == 0x88 ){
- winchDataP->dt_WinchMotor1Current = 0xFF;
- }
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-
- DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
-
- winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
- if( winchTempPosition != -1 ){
- mtx_wcp.lock();
- winchCurrentPosition = winchTempPosition;
- mtx_wcp.unlock();
- }
- mtx_wcp.lock();
- winchDataP->dt_WinchCntPosition = winchCurrentPosition;
- mtx_wcp.unlock();
- winchDataP->operation = 0x00;
- // ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x00 );
- tcp_client.send( (char*)winchDataP, winchData_s);
- DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
- // Thread::wait(2); // Time interval for program debugging
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- }
- DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 FWD
- I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 FWD
- I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed
-
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-
- DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
-
- winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
- if( winchTempPosition != -1 ){
- mtx_wcp.lock();
- winchCurrentPosition = winchTempPosition;
- mtx_wcp.unlock();
- }
- mtx_wcp.lock();
- winchDataP->dt_WinchCntPosition = winchCurrentPosition;
- mtx_wcp.unlock();
- winchDataP->operation = 0x77;
- //ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x77 );
- tcp_client.send( (char*)winchDataP, winchData_s);
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-
- 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 ===\r\n" );
- if ( mode == WINCH_U_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPUP_BTN_ON ===\r\n" );
-
- swbtn_OpeMutex.lock();
- swbtn_Opeflg = 1;
- swbtn_OpeMutex.unlock();
- while( 1 ){
- led3 = ON;
- DEBUG_PRINT_L3("Bd0> WINCH_STEPUP_BTN_ON\r\n");
- rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
- DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
- if( rcv_data_cnt < 0 ){
- DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
- // tcp_client.send( (char*)winchDataP, winchData_s);
- break;
- }
- else{
- DEBUG_PRINT_L3("Bd0> %s\r\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[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 FWD
- I2C_cmd[I2C_CP_M1_SPEED] = man_speed; // Speed
-
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-
- led3 = OFF;
-
- read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
- 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> 17: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
- if( winchDataP->operation == 0x88 ){
- winchDataP->dt_WinchMotor1Current = 0xFF;
- }
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-
- DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
-
- winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
- if( winchTempPosition != -1 ){
- mtx_wcp.lock();
- winchCurrentPosition = winchTempPosition;
- mtx_wcp.unlock();
- }
- mtx_wcp.lock();
- winchDataP->dt_WinchCntPosition = winchCurrentPosition;
- mtx_wcp.unlock();
- winchDataP->operation = 0x00;
- // ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x00 );
- tcp_client.send( (char*)winchDataP, winchData_s);
- DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
- // Thread::wait(2); // Time interval for program debugging
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- }
- DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
- I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 FWD
- I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed
- I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 FWD
- I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed
-
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-
- DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
- if( flg_ButtonOn == false ){
- winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
- if( winchTempPosition != -1 ){
- mtx_wcp.lock();
- winchCurrentPosition = winchTempPosition;
- mtx_wcp.unlock();
- }
- mtx_wcp.lock();
- winchDataP->dt_WinchCntPosition = winchCurrentPosition;
- mtx_wcp.unlock();
- winchDataP->operation = 0x77;
- // ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x77 );
- tcp_client.send( (char*)winchDataP, winchData_s);
- }
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-
- swbtn_OpeMutex.lock();
- swbtn_Opeflg = 0;
- swbtn_OpeMutex.unlock();
-
- led3 = OFF;
- }
-
- else {
- // DEBUG_PRINT_L3("STEPSTEPSTEPSTEPSTEP\r\n");
- }
-// }
-}
-
-// **********************************************************************
-//
-// Main Function of this program
-//
-// **********************************************************************
-int main()
-{
- Mutex file_access_mutex;
- setValue_t new_setValue; // Setting Data
- 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];
-
-// char* ip_address;
-// char* subnet_mask;
-// char* gateway;
-
- 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, 10, 15 );
-
- DEBUG_PRINT_L0("\r\n");
- DEBUG_PRINT_L0("Bd0> +--------------------------------------------------------------\r\n");
- DEBUG_PRINT_L0("Bd0> | Project: B2 Crawler Explorer for 1F-1 PCV internal inspection\r\n");
- DEBUG_PRINT_L0("Bd0> |---------\r\n");
- DEBUG_PRINT_L0("Bd0> | This is: Main Control Program of Main Controller\r\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> ---------------------------------------------------------------\r\n");
- sprintf( ttt, "%s", ProgramRevision );
-
- DEBUG_PRINT_L0("Bd0> Start ststem initializing ...\r\n");
- DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
- DEBUG_PRINT_L0("Bd0> 1. Initalizing Ethernet ...\r\n");
- DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
-
- read_NetSetting_lfs( ip_address, subnet_mask, gateway );
-
-// ip_address = "192.168.3.24";
-// subnet_mask = "255.255.255.0";
-// gateway = "192.168.3.1";
-
- DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
- DEBUG_PRINT_L0("Bd0> ip address : %s\r\n", ip_address);
- DEBUG_PRINT_L0("Bd0> subnet mask : %s\r\n", subnet_mask);
- DEBUG_PRINT_L0("Bd0> default gateway: %s\r\n", gateway);
- DEBUG_PRINT_L0("Bd0> --------------------------------------\r\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\r\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\r\n");
- DEBUG_PRINT_L0("Bd0> [ IP Address : %s ]\r\n", eth.getIPAddress());
- udp_server.bind(UDP_SERVER_PORT);
- tcp_server.bind(TCP_SERVER_PORT);
- tcp_server.listen();
- flg_ethernet = true;
- led4 = ON; // Ethernet OK
- }
- else{
- cf_led_error( &led1,&led2,&led3,&led4 );
- DEBUG_PRINT_L0("Bd0> ***ERROR*** Eternat connect Fali\r\n");
- }
- }
- else{
- DEBUG_PRINT_L0("Bd0> *** ERROR*** Eternat init Fail\r\n");
- }
-
- 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("Bd0> =============================================================\r\n");
- DEBUG_PRINT_L0("Bd0> 2. Read setting value from LFS\r\n");
- DEBUG_PRINT_L0("Bd0> =============================================================\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
- // --------------------------------------------------------------------
- try_cnt = LFS_READ_COUNT;
- while( 1 ){
- if( read_LFS(&setValue) == true ) break;
- else try_cnt -= 1;
- if( try_cnt == 0 ){
- DEBUG_PRINT_L0("Bd0> ***ERROR*** LFS read error\r\n");
- while(1){
- led1 = !led1;
- Thread::wait(30);
- }
- }
- }
-
- DEBUG_PRINT_L0("Bd0> LFS read OK\r\n");
- led3 = ON; // Setting Data Read OK
-
-#ifdef __TARGET_BOARD_CHECK__
- //---------------------------------------------------
- // 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("Bd0> =============================================================\r\n");
- DEBUG_PRINT_L0("Vd0> 3. Check the target controler\r\n");
- DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
-
- try_cnt = TARGET_CHECK_COUNT;
- while(1){
- // Check each target motor control 824 board here
- i2c.read(I2C_ADDRESS_WINCH, I2C_res, NumberOfI2CCommand);
- if( I2C_res[4] == 'S' ){
- DEBUG_PRINT_L0("Bd0> Try count : %02d/%02d\r\n", TARGET_CHECK_COUNT - try_cnt, TARGET_CHECK_COUNT );
- DEBUG_PRINT_L0("Bd0> Return from (0x%02x) : '%c'\r\n", I2C_ADDRESS_WINCH, I2C_res[4]);
- break;
- }
- else try_cnt -= 1;
- if( try_cnt == 0 ){
- DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_WINCH);
- led1 = OFF;
- while(1){
- led1 = !led1; // ON
- Thread::wait(80);
- }
- }
- }
-
- try_cnt = TARGET_CHECK_COUNT;
- while(1){
- // Check each target motor control 824 board here
- i2c.read(I2C_ADDRESS_TRANSFORM, I2C_res, NumberOfI2CCommand);
- if( I2C_res[4] == 'S' ){
- DEBUG_PRINT_L0("Bd0> Try count: %02d/%02d\r\n", TARGET_CHECK_COUNT - try_cnt, TARGET_CHECK_COUNT );
- DEBUG_PRINT_L0("Bd0> Return from (0x%02x): '%c'\r\n", I2C_ADDRESS_TRANSFORM, I2C_res[4]);
- break;
- }
- else try_cnt -= 1;
- if( try_cnt == 0 ){
- DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_TRANSFORM);
- led1 = OFF;
- while(1){
- led2 = !led2; // ON
- Thread::wait(80);
- }
- }
- }
- try_cnt = TARGET_CHECK_COUNT;
- while(1){
- // Check each target motor control 824 board here
- i2c.read(I2C_ADDRESS_CRAWLER, I2C_res, NumberOfI2CCommand);
- if( I2C_res[4] == 'S' ){
- DEBUG_PRINT_L0("Bd0> Try count : %02d/%02d\r\n", TARGET_CHECK_COUNT - try_cnt, TARGET_CHECK_COUNT );
- DEBUG_PRINT_L0("Bd0> Return from (0x%02x) : '%c'\r\n", I2C_ADDRESS_CRAWLER, I2C_res[4]);
- break;
- }
- else try_cnt -= 1;
- if( try_cnt == 0 ){
- DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_CRAWLER);
- led1 = OFF;
- while(1){
- led3 = !led3; // ON
- Thread::wait(80);
- }
- }
- }
- DEBUG_PRINT_L0("Bd0> -------------------\r\n");
- DEBUG_PRINT_L0("Bd0> Target system found\r\n");
- DEBUG_PRINT_L0("Bd0> -------------------\r\n");
-#endif // __TARGET_BOARD_CHECK__
-
-
- led2 = ON; // Check target OK
-
- /* Set basic function default setting */
- baseOperation.sv_JS_OpeMode = 0;
- baseOperation.sv_JS_OpeMode = 0;
- baseOperation.sv_WinchValid = 0;
-
-
- /*
- **************************************************
- Send Calculation Data to Resolver Controller
- **************************************************
- */
- DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
- DEBUG_PRINT_L0("Bd0> 4. Send the Calculation base data to Resolver Controller");
- DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
- I2C_cmd[I2C_CP_WDRAM_DIA_UPPER] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100>>8)&0xFF); // Dram diameter upper
- I2C_cmd[I2C_CP_WDRAM_DIA_LOWER] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100)&0xFF); // Dram diameter lower
- I2C_cmd[I2C_CP_CCABLE_DIA_UPPER] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100>>8)&0xFF); // Cable diameter upper
- I2C_cmd[I2C_CP_CCABLE_DIA_LOWER] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100)&0xFF); // Cable diameter lower
- I2C_cmd[I2C_CP_RESOLVER_RESO] = setValue.winchCtrl.sv_WRS_RResolution; // Resolver resolution
-
- for( int j = 0; j < NumberOfI2CCommand; j++)
- DEBUG_PRINT_L0("%02x ", I2C_cmd[j]);
- DEBUG_PRINT_L0( "\r\n" );
-
- i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- DEBUG_PRINT_L0(" ... done\r\n");
-
- // Thread (
- // void(*task)(void const *argument),
- // void *argument=NULL,
- // osPriority priority=osPriorityNormal,
- // uint32_t stack_size=DEFAULT_STACK_SIZE,
- // unsigned char *stack_pointer=NULL
- //)
-
- DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
- DEBUG_PRINT_L0("Bd0> 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("Bd0> Start host interface task ... ");
- // Thread thread_hif(clientPC_interface_task, NULL, osPriorityHigh, 128*4);
- Thread thread_hif(clientPC_interface_task, NULL, osPriorityNormal, 128*4);
- DEBUG_PRINT_L0("\r\nBd0> Start host gamepad task ... ");
- Thread thread_gpd(gamepad_task, NULL, osPriorityNormal, 128*4);
- DEBUG_PRINT_L0("\r\n");
- DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
-
- DEBUG_PRINT_L0( "Bd0> ----------------------------------\r\n");
- DEBUG_PRINT_L0( "Bd0> >>>> Initializing completed ! <<<<\r\n");
- DEBUG_PRINT_L0( "Bd0> ----------------------------------\r\n");
-
- led4 = OFF;
- led3 = OFF;
- led2 = OFF;
- led1 = ON; // Initializing is OK then Power Indicator LED ON
-
- I2C_cmd[4] = 0x00;
- I2C_cmd[5] = 0x01;
- i2c.write(I2C_ADDRESS_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> ----------------------------\r\n");
- DEBUG_PRINT_L3("Bd0> TCP Connection from: %s\r\n", tcp_client.get_address());
- DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
-
- while(true){
-
- // --------------------------------------------------------------
- // Following instructions are blocking when no ethernat access
- // --------------------------------------------------------------
- rcv_data_cnt = tcp_client.receive(dbuffer, sizeof(dbuffer));
- DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
- if( rcv_data_cnt < 0 ){
- // DEBUG_PRINT("## TCP Receive packet fail ##\r\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\r\n");
- Thread::wait(200);
- rcv_data_cnt = tcp_client.receive( (char*)&new_setValue, sizeof(new_setValue));
- Thread::wait(200);
- // DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
- // dspSetValue2Console( &pc, &new_setValue );
- // DEBUG_PRINT_L0("Bd0> write setting file to local file sysytem\r\n");
-
- thread_hif.terminate();
- thread_gpd.terminate();
-
- // file_access_mutex.lock();
- write_LFS(&new_setValue);
- // file_access_mutex.unlock();
-
- Thread::wait(500);
-
- DEBUG_PRINT_L0("Bd0> SetValue instruction is over\r\n");
- }
- else if(!strcmp( dbuffer, "GetValue" )){
- DEBUG_PRINT_L3("Bd0> GetValue request from TCP client\r\n");
-
- flg_mutex.lock(); // This is very important
- memcpy( &new_setValue, &setValue, sizeof( new_setValue ) );
- flg_mutex.unlock(); // This is very important
- dspSetValue2Console( &pc, &new_setValue );
- tcp_client.send_all( (char*)&new_setValue, sizeof(new_setValue) );
- DEBUG_PRINT_L2("(%d)\r\n", sizeof(new_setValue));
- }
- }
- if( rcv_data_cnt <= 0 ) break;
- }
- tcp_client.close();
- }
- }
-}
+
+/***************************************
+ * Project: B2
+ * Title: Bebris Prober Ctrl Main
+ * Target: LPC1768
+ * Author: sayzyas as ZNR
+ * ------------------------------------
+ *
+ *
+ *
+ * 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 "common.h"
+#include "stdio.h"
+#include "TextLCD.h"
+#include "com_func.h"
+
+// USBSerial serial setting
+Serial pc(USBTX, USBRX); // UART
+
+// Analog I/O setting : 2016.07.26 added
+AnalogIn winchDramMSpeed(p19); // Winch motor speed adjustment valiable volume for Motor 1 (Dram)
+AnalogIn winchCableMSpeed(p20); // Winch motor speed adjustment valiable volume for Motor 2 (Cable)
+
+// 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
+// I2C setting
+//I2C i2c_res(p28, p27); // I2C SDA, SCL is not good ???
+I2C i2c(p9, p10); // I2C SDA, SCL is good
+
+// 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 local("local"); // Create the local filesystem under the name "local"
+
+// Global
+uint32_t flg_gamePad_Connected = 0;
+char PC_cmd[11+1] = "&0100000000";
+basic_operation_t baseOperation;
+char dbuffer[128];
+uint8_t motor_speed = 0;
+
+// 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 flg_mutex;
+Mutex swbtn_OpeMutex;
+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';
+
+bool flg_winchButtonOn = false;
+
+
+/* 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 Winch Motor Speed from Analog volume
+// ============================================================
+void set_winchMotorSpeed( void ){
+ // if reserved is not 0 then read winch motor speed from analog volume.
+ if( setValue.winchCtrl.reserved != 0 ){
+ setValue.winchCtrl.sv_WDM_hsrto_F = (winchDramMSpeed.read_u16() & 0xFF00 ) / 655; // normal speed
+ setValue.winchCtrl.sv_WDM_hsrto_R = (winchDramMSpeed.read_u16() & 0xFF00 ) / 655; // normal speed
+ setValue.winchCtrl.sv_WRM_hsrto_F = (winchCableMSpeed.read_u16() & 0xFF00 ) / 655; // normal speed
+ setValue.winchCtrl.sv_WRM_hsrto_R = (winchCableMSpeed.read_u16() & 0xFF00 ) / 655; // normal speed
+ }
+// setValue.winchCtrl.sv_WDM_lsrto_F = setValue.winchCtrl.sv_WDM_hsrto_F >> 1; // slow speed
+// setValue.winchCtrl.sv_WDM_lsrto_R = setValue.winchCtrl.sv_WDM_hsrto_R >> 1; // slow speed
+// setValue.winchCtrl.sv_WRM_lsrto_F = setValue.winchCtrl.sv_WRM_hsrto_F >> 1; // slow speed
+// setValue.winchCtrl.sv_WRM_lsrto_R = setValue.winchCtrl.sv_WRM_hsrto_R >> 1; // slow speed
+
+
+ DEBUG_PRINT_L3( "Bd0> Dram motor F speed : %d\r\n", setValue.winchCtrl.sv_WDM_hsrto_F );
+ DEBUG_PRINT_L3( "Bd0> Dram motor R speed : %d\r\n", setValue.winchCtrl.sv_WDM_hsrto_R );
+ DEBUG_PRINT_L3( "Bd0> cable motor F speed: %d\r\n", setValue.winchCtrl.sv_WRM_hsrto_F );
+ DEBUG_PRINT_L3( "Bd0> Cable motor R speed: %d\r\n", setValue.winchCtrl.sv_WRM_hsrto_R );
+}
+
+// ============================================================
+// Read motor current
+// ============================================================
+int read_motorCurrent(
+ int i2c_addr,
+ char* I2C_data,
+ int NumberOfI2Cdata
+){
+ int rts;
+ int roop = 0;
+
+ while(1){
+ rts = i2c.read(i2c_addr, I2C_data, 6);
+ if( rts == 0 ){
+/* DEBUG_PRINT_L0(" ++++++++++++++++++++++++++++++\r\n" );
+ DEBUG_PRINT_L0(" Target [0x%02x]\r\n", i2c_addr );
+ DEBUG_PRINT_L0(" 00 Read Motor1 Current [%d]\r\n", I2C_data[0] );
+ DEBUG_PRINT_L0(" 01 Read Motor2 Current [%d]\r\n", I2C_data[1] );
+ DEBUG_PRINT_L0(" 02 Read [0x%02x]\r\n", I2C_data[2] );
+ DEBUG_PRINT_L0(" 03 Read [%d]\r\n", I2C_data[3] );
+ DEBUG_PRINT_L0(" 04 Read [%c]\r\n", I2C_data[4] );
+ DEBUG_PRINT_L0(" ++++++++++++++++++++++++++++++\r\n" );
+*/
+ break;
+ }
+ if( roop >= 3 ){
+ rts = -1;
+ break;
+ }
+ roop++;
+ }
+ return rts;
+}
+
+// ============================================================
+// 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;
+}
+
+
+int16_t debugWinchCurrentPosition = 0;
+// ============================================================
+// Read winch current position from Resolver.
+// ============================================================
+/*
+2016.11.09
+ Add dummy data to read winch position value.
+ byte[0]: Dummy data = 0x12 <--- New added !
+ byte[1]: Winch position upper byte
+ byte[2]: Winch position lower byte
+ byte[3]: Dummy data = 0x34 <--- New added !
+*/
+int16_t ReadWinchCurrentPosition( int32_t i2c_addr, int mode )
+{
+ char I2C_data[4];
+ int16_t res_position = 0;
+ int rts;
+
+ Thread::wait(5);
+ rts = i2c.read(i2c_addr, I2C_data, 4); // Read
+ if(( I2C_data[0] == 0x12 )&&( I2C_data[3] == 0x34 )&&(rts == 0))
+ {
+ res_position = (I2C_data[2] << 8) | I2C_data[1];
+ if( res_position == -1 ){
+ res_position = 9999;
+ }
+ else{
+ res_position += winchOffsetValue;
+ }
+ }
+ else{
+ res_position = 9999;
+ }
+ pc.printf("ReadWinchCurrentPosition [ %d ]mm\r\n", res_position );
+ return res_position;
+}
+
+uint8_t adj_crawlerSpeed( uint8_t in )
+{
+ uint8_t out;
+
+ if( ( in > 0 ) && ( in < 98 ))
+ {
+ out = (uint8_t)((float)in * 0.8);
+ }
+ else
+ {
+ out = 100;
+ }
+ return out;
+}
+
+bool flg_mc_tfmcrw = false;
+bool flg_mc_winch = false;
+char I2C_res[NumberOfI2CCommand+1] = "\0";
+// ============================================================
+// 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] = "#0100000000000";
+ char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
+
+ 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_RFLBI = 0; // RF-I and LB-I both button on same time
+ uint8_t btnID_RFLBK = 0; // RF-K and LB-K both button on same time
+ 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;
+
+ uint8_t cntcnt = 0;
+
+ int rrr;
+
+ 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;
+ // ---- This is temporary setting ------------------------------
+ btnID_RFLBI = 192; // RF-I and LB-I both button on same time
+ btnID_RFLBK = 48; // RF-K and LB-K both button on same time
+ // --------------------------------------------------------------
+ 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_WDN = btn04;
+ btnStatus_WUP = 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_CrossUp = btn06;
+ btnStatus_CrossDn = btn06;
+ btnStatus_CrossRt = btn06;
+ btnStatus_CrossLt = btn06;
+ btnStatus_Start = btn05; //
+ }
+ }
+ else if (gamePadVID == GAMEPAD_VID_ELECOM ){
+ DEBUG_PRINT_L4("Bd0> [ELECOM] ");
+ btnID_WDN = 4; // 0x04
+ btnID_WUP = 2; // 0x02
+ btnID_RFK = 32; // 0x20
+ btnID_RFI = 128; // 0x80
+ btnID_LBK = 16; // 0x10
+ btnID_LBI = 64; // 0x40
+ // ---------------------
+ btnID_RFLBI = 192; // 0xC0 RF-I and LB-I both button on same time
+ btnID_RFLBK = 48; // 0x30 RF-K and LB-K both button on same time
+ // ---------------------
+ 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_WDN = btn04;
+ btnStatus_WUP = 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_WDN = 40;
+ btnID_WUP = 136;
+ btnID_RFK = 2;
+ btnID_RFI = 8;
+ btnID_LBK = 1;
+ btnID_LBI = 4;
+ // ---------------------
+ btnID_RFLBI = 12; // RF-I and LB-I both button on same time
+ btnID_RFLBK = 3; // RF-K and LB-K both button on same time
+ // ---------------------
+ 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_WDN = btn05;
+ btnStatus_WUP = 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_WDN = btn04;
+ btnStatus_WUP = 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_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_WDN = 2;
+ btnID_WUP = 4;
+ btnID_RFK = 2;
+ btnID_RFI = 1;
+ btnID_LBK = 128;
+ btnID_LBI = 64;
+ // ---------------------
+ btnID_RFLBI = 80; // RF-I and LB-I both button on same time
+ btnID_RFLBK = 40; // RF-K and LB-K both button on same time
+ // ---------------------
+ btnID_CrossUp = 0;
+ btnID_CrossDn = 255;
+ btnID_CrossRt = 0;
+ btnID_CrossLt = 255;
+ if ( gamePadPID == GAMEPAD_PID_SANWA_JYP70US ){
+ btnStatus_WDN = btn05;
+ btnStatus_WUP = btn05;
+ btnStatus_RFK = btn06;
+ btnStatus_RFI = btn06;
+ btnStatus_LBK = btn05;
+ btnStatus_LBI = btn05;
+ 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_CrossUp = btn01;
+ btnStatus_CrossDn = btn01;
+ btnStatus_CrossRt = btn00;
+ btnStatus_CrossLt = btn00;
+ }
+ }
+
+#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_SW("Bd0> -- Button Status -------------------------------\r\n");
+ DEBUG_PRINT_SW("Bd0> 00(%02x) 01(%02x) 02(%02x) 03(%02x)\r\n", btn00,btn01,btn02,btn03);
+ DEBUG_PRINT_SW("Bd0> 04(%02x) 05(%02x) 06(%02x) 07(%02x) 08(%02x)\r\n", btn04,btn05,btn06,btn07,btn08);
+ DEBUG_PRINT_SW("Bd0> 09(%02x) 10(%02x) 11(%02x) 12(%02x) 13(%02x) 14(%02x) 15(%02x)\r\n", btn09,btn10,btn11,btn12,btn13,btn14,btn15);
+ DEBUG_PRINT_SW("Bd0> ------------------------------------------------\r\n");
+#endif
+
+ I2C_cmd[I2C_CP_M1_DIR] = '\0';
+ I2C_cmd[I2C_CP_M1_SPEED] = '\0';
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = '\0';
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = '\0';
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = '\0';
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = '\0';
+ I2C_cmd[I2C_CP_M2_DIR] = '\0';
+ I2C_cmd[I2C_CP_M2_SPEED] = '\0';
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = '\0';
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = '\0';
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = '\0';
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = '\0';
+
+ int tmpSpeed = 0;
+
+ if (swbtn_Opeflg == 1){
+ Thread::wait(1);
+ }
+ else{
+ if( flg_lsw_valid == true ){
+ I2C_cmd[1] = 'V';
+ }
+ else{
+ I2C_cmd[1] = '0';
+ }
+ if( gamePadVID == GAMEPAD_VID_RSTHANDY ){ // NRst own original controller
+ flg_exp_status &= 0xFFFFFFF0;
+ if(!( btnStatus_Start & 0x01 )){ // I-Shape
+ flg_mutex.lock();
+ baseOperation.sv_JS_ShapeMode = 0;
+ baseOperation.sv_WinchValid = 0;
+ flg_mutex.unlock();
+ flg_exp_status |= 0x00000001;
+ }
+ else{ // KO-Shape
+ flg_mutex.lock();
+ baseOperation.sv_JS_ShapeMode = 1;
+ flg_mutex.unlock();
+ flg_exp_status |= 0x00000002;
+ }
+
+ if(!(btnStatus_Start & 0x02 )){ // Tfm,crawler part valid
+ flg_mutex.lock();
+ baseOperation.sv_WinchValid = 0;
+ flg_mutex.unlock();
+ flg_exp_status |= 0x00000004;
+ if( flg_mc_tfmcrw == false ){
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP;
+ I2C_cmd[I2C_CP_M1_SPEED] = 0;
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP;
+ I2C_cmd[I2C_CP_M2_SPEED] = 0;
+ pc.printf("#### Winch Motor Stop ####\r\n");
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+
+ sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ flg_mc_tfmcrw = true;
+ flg_mc_winch = false;
+ }
+ else{ // Winch part valid
+ flg_mutex.lock();
+ baseOperation.sv_WinchValid = 1;
+ flg_mutex.unlock();
+ flg_exp_status |= 0x00000008;
+ if( flg_mc_winch == false ){
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP;
+ I2C_cmd[I2C_CP_M1_SPEED] = 0;
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP;
+ I2C_cmd[I2C_CP_M2_SPEED] = 0;
+ pc.printf("#### Transform and Crawler Motor Stop ####\r\n");
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+
+ sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ flg_mc_tfmcrw = false;
+ flg_mc_winch = true;
+ }
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ DEBUG_PRINT_L4( "%d : %d\r\n",btnStatus_Start, flg_exp_status );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ }
+ else{
+ /*
+ *
+ * GamePad software switch
+ * Cross button Up on : JS shape mode I
+ * Cross button Down on : JS shape mode KO
+ * Cross button Right on: Winch part valid
+ * Cross button Left on : Crawlerm, Transform part valid
+ * 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: Left part(Crawler, Tfm) part valid, 8: Winch part valid
+ * +-+-+-+-+-+-+-+-+
+ */
+ if ( btnStatus_CrossUp == btnID_CrossUp ){ // I Shape
+ flg_mutex.lock();
+ baseOperation.sv_JS_ShapeMode = 0;
+ flg_mutex.unlock();
+ DEBUG_PRINT_L4( "--------------------------------\r\n" );
+ DEBUG_PRINT_L4( " I\r\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
+ DEBUG_PRINT_L4( "--------------------------------\r\n" );
+ DEBUG_PRINT_L3( "Bd0> I Shape\r\n" );
+ flg_exp_status |= 0x00000001;
+ }
+ else if( btnStatus_CrossDn == btnID_CrossDn ){ // KO Shape
+ flg_mutex.lock();
+ baseOperation.sv_JS_ShapeMode = 1;
+ flg_mutex.unlock();
+ DEBUG_PRINT_L4( "-------------------------\r\n" );
+ DEBUG_PRINT_L4( " KO\r\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
+ DEBUG_PRINT_L4( "-------------------------\r\n" );
+ DEBUG_PRINT_L3( "Bd0> KO Shape\r\n" );
+ flg_exp_status |= 0x00000002;
+ }
+ else if( btnStatus_CrossRt == btnID_CrossLt ){ // Valid Part : Crawler (Left)
+ flg_mutex.lock();
+ baseOperation.sv_WinchValid = 0;
+ flg_mutex.unlock();
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ DEBUG_PRINT_L4( " Tfm, Crawler\r\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ DEBUG_PRINT_L3( "Bd0> Left Part\r\n" );
+ flg_exp_status |= 0x00000004;
+ if( flg_mc_tfmcrw == false ){
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP;
+ I2C_cmd[I2C_CP_M1_SPEED] = 0;
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP;
+ I2C_cmd[I2C_CP_M2_SPEED] = 0;
+ pc.printf("#### Winch Motor Stop ####\r\n");
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ flg_mc_tfmcrw = true;
+ flg_mc_winch = false;
+ }
+ else if( btnStatus_CrossLt == btnID_CrossRt ){ // Valid Part : Winch (Right)
+ flg_mutex.lock();
+ baseOperation.sv_WinchValid = 1;
+ flg_mutex.unlock();
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ DEBUG_PRINT_L4( " Winch\r\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ DEBUG_PRINT_L3( "Bd0> Right Part\r\n" );
+ flg_exp_status |= 0x00000008;
+ if( flg_mc_winch == false ){
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP;
+ I2C_cmd[I2C_CP_M1_SPEED] = 0;
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP;
+ I2C_cmd[I2C_CP_M2_SPEED] = 0;
+ pc.printf("#### Transform and Crawler Motor Stop ####\r\n");
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ flg_mc_tfmcrw = false;
+ flg_mc_winch = true;
+ }
+ else{
+ flg_exp_status &= 0xFFFFFFF0;
+ }
+ }
+ /*
+ // ====================================
+ // 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 ( baseOperation.sv_WinchValid == 0 ){ // TRANSFORM, CRAWLER PART Valid
+ if (btnStatus_RFK==btnID_RFLBK){ // Both sw on
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> BTN RF-K & LB-K\r\n" );
+ led3 = 1;
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_F; // Speed
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor1 FWD
+ I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_F; // Speed
+ flg_exp_status |= 0x30000000;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
+ // i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ #ifdef __READ_TFM_MOTOR_CURRENT__
+ rrr = read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_res, 5 );
+ if( rrr == 0 ) {
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ limitSw_Sts = I2C_res[3];
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
+ }
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 ERROR\r\n" );
+
+ #endif // __IIC_COMAMND_SEND__
+ flg_ButtonOn = false;
+ }
+ else if (btnStatus_RFI== btnID_RFLBI) {// Both sw on
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> BTN RF-I & LB-I\r\n" );
+ led3 = 1;
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 FWD
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_F; // Speed
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor1 FWD
+ I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_F; // Speed
+ flg_exp_status |= 0x10000000;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
+ // i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ #ifdef __READ_TFM_MOTOR_CURRENT__
+ rrr = read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_res, 5 );
+ if( rrr == 0 ){
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ limitSw_Sts = I2C_res[3];
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
+ }
+ #endif // __READ_TFM_MOTOR_CURRENT__
+ flg_ButtonOn = false;
+ }
+ else if (btnStatus_RFK==btnID_RFK){ // RF KO
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> BTN RF-K\r\n" );
+ led3 = 1;
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_F; // Speed
+ flg_exp_status |= 0x10000000;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
+ // i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ #ifdef __READ_TFM_MOTOR_CURRENT__
+ rrr = read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ limitSw_Sts = I2C_res[3];
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
+ #endif // __READ_TFM_MOTOR_CURRENT__
+ flg_ButtonOn = false;
+ }
+ else if (btnStatus_RFI==btnID_RFI){ // RF I
+ DEBUG_PRINT_L3( "Bd0> BTN RF-I\r\n" );
+ led3 = 1;
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_R; // Speed
+ flg_exp_status |= 0x20000000;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
+ // i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ #ifdef __READ_TFM_MOTOR_CURRENT__
+ rrr = read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ limitSw_Sts = I2C_res[3];
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
+ #endif // __READ_TFM_MOTOR_CURRENT__
+ flg_ButtonOn = false;
+ }
+ else if (btnStatus_LBK==btnID_LBK){ // LB KO
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> BTN LB-K\r\n" );
+ led3 = 1;
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 FWD
+ I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_F; // Speed
+ flg_exp_status |= 0x40000000;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
+ // i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ #ifdef __READ_TFM_MOTOR_CURRENT__
+ rrr = read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ limitSw_Sts = I2C_res[3];
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
+ #endif // __READ_TFM_MOTOR_CURRENT__
+ flg_ButtonOn = false;
+ }
+ else if (btnStatus_LBI==btnID_LBI) { // LB I
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> BTN LB-I\r\n" );
+ led3 = 1;
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor1 RVS
+ I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_R; // Speed
+ flg_exp_status |= 0x80000000;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
+ // i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ #ifdef __READ_TFM_MOTOR_CURRENT__
+ rrr = read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ limitSw_Sts = I2C_res[3];
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
+ #endif // __READ_TFM_MOTOR_CURRENT__
+ // 2016.09.05 comment out
+ flg_ButtonOn = false;
+ }
+ else { // Motor OFF
+ led3 = 0;
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP;
+ I2C_cmd[I2C_CP_M1_SPEED] = 0;
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP;
+ I2C_cmd[I2C_CP_M2_SPEED] = 0;
+ Thread::wait(5);
+ flg_exp_status &= 0x0FFFFFFF;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
+ // ------------------------------------------------------------------------------------------
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // <<<<=== is it necessary ?
+ Thread::wait(1); // <<<<=== is it necessary ?
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // <<<<=== is it necessary ?
+ Thread::wait(1);// <<<<=== is it necessary ?
+ // ----------------------------------------------------------------------------------------
+ #ifdef __READ_TFM_MOTOR_CURRENT__
+ rrr = read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ limitSw_Sts = I2C_res[3];
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
+ #endif // __READ_TFM_MOTOR_CURRENT__
+
+ }
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ /*
+ // ====================================
+ // Crawler Moving Control
+ // ====================================
+ // JoyStick mode 1: Independence mode ( 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( baseOperation.sv_JS_OpeMode == 1 ){
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_R & 0xFF );
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_R & 0xFF );
+ if ( btnStatus_LJSFwdRvs<setValue.crawlerCtrl.sv_LBCM_dzc-setValue.crawlerCtrl.sv_LBCM_dzl ){
+ flg_ButtonOn = true;
+ led3 = 1;
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 FWD
+ tmpSpeed = ( setValue.crawlerCtrl.sv_LBCM_dzc+1 - btnStatus_LJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc * setValue.crawlerCtrl.sv_LBCM_srto_F / 100; // Speed
+ I2C_cmd[I2C_CP_M2_SPEED] = (char)tmpSpeed;
+ // i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ DEBUG_PRINT_SW( "Bd0> Dual Mode: L-Fwd (Speed=%d)\r\n", tmpSpeed);
+ // Read motor current from target
+ rrr = read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+
+ flg_exp_status |= 0x00400000;
+ flg_ButtonOn = false;
+ }
+ else if (btnStatus_LJSFwdRvs>setValue.crawlerCtrl.sv_LBCM_dzc+setValue.crawlerCtrl.sv_LBCM_dzu){
+ flg_ButtonOn = true;
+ led3 = 1;
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 RVS
+ tmpSpeed = ( btnStatus_LJSFwdRvs - setValue.crawlerCtrl.sv_LBCM_dzc ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc+1 * setValue.crawlerCtrl.sv_LBCM_srto_F / 100; // Speed I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = (char)tmpSpeed;
+ I2C_cmd[I2C_CP_M2_SPEED] = (char)tmpSpeed;
+ // i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Rvs (Speed=%d)\r\n", tmpSpeed);
+ // Read motor current from target
+ rrr = read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+
+ flg_exp_status |= 0x00800000;
+ flg_ButtonOn = false;
+ }
+ else if (baseOperation.sv_WinchValid==0) {
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Stop
+ I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed=0
+ flg_exp_status &= 0xFF3FFFFF;
+ }
+
+ if (btnStatus_RJSFwdRvs<setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl){
+ flg_ButtonOn = true;
+ led3 = 1;
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
+ tmpSpeed = (( 128 - btnStatus_RJSFwdRvs ) * 100 / 127 ) * setValue.crawlerCtrl.sv_RFCM_srto_R / 100;
+ I2C_cmd[I2C_CP_M1_SPEED] = (char)tmpSpeed;
+ DEBUG_PRINT_SW( "Bd0> Dual Mode: R-Fwd (Speed=%d)\r\n", tmpSpeed);
+ // i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ // Read motor current from target
+ rrr = read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+
+ flg_exp_status |= 0x00100000;
+ flg_ButtonOn = false;
+ }
+ else if (btnStatus_RJSFwdRvs>setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu){
+ flg_ButtonOn = true;
+ led3 = 1;
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
+ tmpSpeed = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc+1 * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
+ I2C_cmd[I2C_CP_M1_SPEED] = (char)tmpSpeed;
+ DEBUG_PRINT_SW( "Bd0> Dual Mode: R-Rvs (Speed=%d)\r\n", tmpSpeed);
+ // i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ // Read motor current from target
+ rrr = read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+
+ flg_exp_status |= 0x00200000;
+ flg_ButtonOn = false;
+ }
+ else{
+ DEBUG_PRINT_L2("***** MOTOR STOP ****\r\n");
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Stop
+ I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed=0
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_exp_status &= 0xFFCFFFFF;
+ }
+ // ---------------------------------------------
+ // Send command to target here ! 2016.09.07
+ // ---------------------------------------------
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ led3 = 0;
+ }
+ /* JoyStick mode 0: Syncronous mode ( 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
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_R & 0xFF );
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_R & 0xFF );
+ if(
+ ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl ) &&
+ ( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu ) &&
+ ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl ) &&
+ ( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu )
+ ){
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Fwd
+ I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed=0
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 Rvs
+ I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed=0
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ flg_exp_status &= 0xFF0FFFFF; // These operation is absolutely necessary , if forgot then UDP connection will be fail !
+ }
+ else 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 + 50) &&
+ ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50 )
+ ){
+ flg_ButtonOn = true;
+ led3 = 1;
+ if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Reverse
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
+ }
+ else{
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Fwd
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
+ }
+ I2C_cmd[I2C_CP_M1_SPEED] = adj_crawlerSpeed(( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100 ); // Speed
+ I2C_cmd[I2C_CP_M2_SPEED] = adj_crawlerSpeed(( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100 ); // Speed
+
+ // I2C_cmd[I2C_CP_M1_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
+ // I2C_cmd[I2C_CP_M2_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
+ // i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ DEBUG_PRINT_SW( "Bd0> Single Mode: Dir1 M1[%d, %d] M2[%d, %d]\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M2_DIR], I2C_cmd[I2C_CP_M2_SPEED]);
+ pc.printf("Bd0> Single Mode: Dir1 M1[%d, %d] M2[%d, %d]\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M2_DIR], I2C_cmd[I2C_CP_M2_SPEED]);
+ flg_exp_status |= 0x00500000;
+ // Read motor current from target
+ rrr = read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ flg_ButtonOn = false;
+ Thread::wait(5);
+ }
+ 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 + 50 ) &&
+ ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50)
+ ){
+ flg_ButtonOn = true;
+ led3 = 1;
+ if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Reverse
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor1 Reverse
+ }
+ else{
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Rvs
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
+ }
+ I2C_cmd[I2C_CP_M1_SPEED] = adj_crawlerSpeed(( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100); // Speed
+ I2C_cmd[I2C_CP_M2_SPEED] = adj_crawlerSpeed(( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100); // Speed
+ // i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ DEBUG_PRINT_SW( "Bd0> Single Mode: Dir2 M1[%d, %d] M2[%d, %d]\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M2_DIR], I2C_cmd[I2C_CP_M2_SPEED]);
+ pc.printf("Bd0> Single Mode: Dir2 M1[%d, %d] M2[%d, %d]\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M2_DIR], I2C_cmd[I2C_CP_M2_SPEED]);
+ flg_exp_status |= 0x00600000;
+ // Read motor current from target
+ rrr = read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ flg_ButtonOn = false;
+ Thread::wait(5);
+ }
+ 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 + 50) &&
+ ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50)
+ ){
+ flg_ButtonOn = true;
+ led3 = 1;
+ if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Rvs
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
+ }
+ else{
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Rvs
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
+ }
+ I2C_cmd[I2C_CP_M1_SPEED] = adj_crawlerSpeed(( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100); // Speed
+ I2C_cmd[I2C_CP_M2_SPEED] = adj_crawlerSpeed(( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100); // Speed
+ // i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ DEBUG_PRINT_SW( "Bd0> Single Mode: Dir3 M1[%d, %d] M2[%d, %d]\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M2_DIR], I2C_cmd[I2C_CP_M2_SPEED]);
+ pc.printf("Bd0> Single Mode: Dir3 M1[%d, %d] M2[%d, %d]\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M2_DIR], I2C_cmd[I2C_CP_M2_SPEED]);
+ flg_exp_status |= 0x00A00000;
+ // Read motor current from target
+ rrr = read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ flg_ButtonOn = false;
+ Thread::wait(5);
+ }
+ 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 + 50) &&
+ ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50)
+ ){
+ flg_ButtonOn = true;
+ led3 = 1;
+ if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Reverse
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor1 Reverse
+ }
+ else{
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Fwd
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
+ }
+ I2C_cmd[I2C_CP_M1_SPEED] = adj_crawlerSpeed(( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100); // Speed
+ I2C_cmd[I2C_CP_M2_SPEED] = adj_crawlerSpeed(( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100); // Speed
+ // i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ DEBUG_PRINT_SW( "Bd0> Single Mode: Dir4 M1[%d, %d] M2[%d, %d]\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M2_DIR], I2C_cmd[I2C_CP_M2_SPEED]);
+ pc.printf("Bd0> Single Mode: Dir4 M1[%d, %d] M2[%d, %d]\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M2_DIR], I2C_cmd[I2C_CP_M2_SPEED]);
+ flg_exp_status |= 0x00900000;
+ // Read motor current from target
+ rrr = read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ flg_ButtonOn = false;
+ Thread::wait(5);
+ }
+ // ====================================
+ // ALL motor off commmand packet send
+ // ====================================
+ else{
+ led3 = 0;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Fwd
+ I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed=0
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 Rvs
+ I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed=0
+
+ // ------------------------------------------------------------------------------------------
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // <<<<=== is it necessary ?
+ Thread::wait(1); // <<<<=== is it necessary ?
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // <<<<=== is it necessary ?
+ Thread::wait(1);// <<<<=== is it necessary ?
+ // ------------------------------------------------------------------------------------------
+
+ #endif
+ flg_exp_status &= 0xFF0FFFFF; // These operation is absolutely necessary , if forgot then UDP connection will be fail !
+ }
+ // ---------------------------------------------
+ // Send command to target here ! 2016.09.07
+ // ---------------------------------------------
+ i2c.write(I2C_ADDRESS_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: -
+ * +-+-+-+-+-+-+-+-+
+ */
+ else if( baseOperation.sv_WinchValid == 1 ){
+ int16_t winchTempPosition;
+ if (btnStatus_WDN == btnID_WDN){ // Winch Down (FWD)
+ flg_ButtonOn = true;
+ flg_winchButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> BTN W-DN(Fwd)\r\n" );
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ set_winchMotorSpeed(); // 2016.08.05 for Fix falling winch motor speed adjustment
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_F; // Speed
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 FWD
+ I2C_cmd[I2C_CP_M2_SPEED] = setValue.winchCtrl.sv_WRM_hsrto_F; // Speed
+ #endif
+ flg_exp_status |= 0x01000000;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_R & 0xFF );
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_R & 0xFF );
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+/*
+ rrr = read_motorCurrent( I2C_ADDRESS_WINCH, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ pc.printf("**** MOTOR1 LOCK ****\r\n");
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ pc.printf("**** MOTOR2 LOCK ****\r\n");
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+*/
+ // ******** 2016.06.16 ***********************************************************
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER, 1);
+ if( winchTempPosition != 9999 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+
+ flg_ButtonOn = false;
+ Thread::wait(1);
+ }
+ else if ( btnStatus_WUP == btnID_WUP ) { // Winch Up (Rvs)
+ flg_ButtonOn = true;
+ flg_winchButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> BTN W-UP(Rvs)\r\n" );
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ set_winchMotorSpeed(); // 2016.08.05 for Fix falling winch motor speed adjustment
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_R; // Speed
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 RVS
+ I2C_cmd[I2C_CP_M2_SPEED] = setValue.winchCtrl.sv_WRM_hsrto_R; // Speed
+ #endif
+ flg_exp_status &= 0xF0FFFFFF;
+ flg_exp_status |= 0x02000000;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_R & 0xFF );
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_F >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_F & 0xFF );
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_R >> 8 ) & 0xFF;
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_R & 0xFF );
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+/*
+ rrr = read_motorCurrent( I2C_ADDRESS_WINCH, I2C_res, 5 );
+ motorLock_sts = I2C_res[4];
+ if( motorLock_sts == '1' ){
+ pc.printf("**** MOTOR1 LOCK ****\r\n");
+ motor1_current_pct = 999;
+ }
+ else{
+ motor1_current_pct = I2C_res[0];
+ }
+ if( motorLock_sts == '2' ){
+ pc.printf("**** MOTOR2 LOCK ****\r\n");
+ motor2_current_pct = 999;
+ }
+ else{
+ motor2_current_pct = I2C_res[1];
+ }
+*/
+ // ******** 2016.06.16 ***********************************************************
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER, 2);
+ if( winchTempPosition != 9999){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ flg_ButtonOn = false;
+ Thread::wait(1);
+ }
+ // ====================================
+ // ALL motor off commmand packet send
+ // ====================================
+ else {
+ led3 = 0;
+ #ifdef __IIC_COMAMND_SEND__
+ // pc.printf("MOTOR STOP STOP STOP STOP STOP STOP STOP STOP \r\n");
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP;
+ I2C_cmd[I2C_CP_M1_SPEED] = 0;
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP;
+ I2C_cmd[I2C_CP_M2_SPEED] = 0;
+ #endif
+ flg_exp_status &= 0xF0FFFFFF;
+ // pc.printf("WinchMotorSpeed: %d, %d\r\n", I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M2_SPEED]);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ Thread::wait(1);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ }
+ // ---------------------------------------------
+ // Send command to target here ! 2016.09.07
+ // ---------------------------------------------
+ // pc.printf("WinchMotorSpeed: %d, %d\r\n", I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M2_SPEED]);
+ // i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ }
+ }
+}
+
+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
+//
+// **************************************************************
+int first_counter = 0;
+void clientPC_interface_task(void const *)
+{
+ int rcv_data_cnt;
+ //winchData_t winchData;
+
+ char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
+
+// winchData_t winchData;
+// int16_t winchCurrentPosition;
+ int16_t winchTempPosition;
+
+ int cnt = 0;
+ int rrr;
+ int cnt2 = 0;
+
+ int16_t tempWinchPosition = 0;
+
+ while(1){
+
+ // DEBUG_PRINT("\r\nWaiting for UDP packet...\r\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 (%d) ??\r\n", rcv_data_cnt );
+ Thread::wait(100);
+ cnt2++;
+ if( cnt2 > 5 ){
+ DEBUG_PRINT_L0("Bd0> UDP Server restart\r\n", rcv_data_cnt );
+ udp_server.close();
+ udp_server.bind(UDP_SERVER_PORT);
+ }
+ }
+ else{
+ dbuffer[rcv_data_cnt] = '\0';
+ led4 = 1;
+
+ if(!strcmp( dbuffer, "Hello Z\r\n" )){
+ DEBUG_PRINT_L2("Bd0> Hello Z Packet received from client by UDP\r\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\r\n")){
+ DEBUG_PRINT_L2("Bd0> Hello Status req received from client by UDP\r\n");
+ strcpy(dbuffer,"XXXX\r\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, "set_jss")){ // Single JS mode
+ baseOperation.sv_JS_OpeMode = 0;
+ }
+ else if(!strcmp( dbuffer, "set_jsd")){ // Dual JS mode
+ baseOperation.sv_JS_OpeMode = 1;
+ }
+ else if(!strcmp( dbuffer, "lsw_valid")){ // Limit swich detection valid
+ flg_lsw_valid = true;
+ }
+ else if(!strcmp( dbuffer, "lsw_invalid")){ // Limit swich detection invalid
+ flg_lsw_valid = false;
+ }
+ else if(!strcmp( dbuffer, "Hello")){
+ DEBUG_PRINT_L0( "Bd0> Hello Packet received from [ 0x%08x ]\r\n", flg_exp_status );
+
+ /* ***************************************** */
+ /* Read Winch Current Position from Resolver */
+ /* ***************************************** */
+ if (baseOperation.sv_WinchValid == 1){ // read winch current position operation is valid in case of winch part is valid.
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER, 0);
+ if( winchTempPosition != 9999 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ }
+ Thread::wait(10);
+
+ first_counter++;
+ if( first_counter > 10 ) {
+ sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts); // Winch down
+ first_counter = 10;
+ }
+ else{
+ sprintf( dbuffer,"JSMD %03d %03d %04d %03d\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid, winchCurrentPosition, limitSw_Sts);
+ // sprintf( dbuffer,"JSMD %03d %03d 0000 000\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid );
+ }
+ // -------------------------------------
+ // 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 ){
+ // 01234 5678 9012 34569
+ sprintf( dbuffer, "BCFW %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
+ Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00a00000 ){
+ sprintf( dbuffer, "BCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
+ Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00600000 ){
+ sprintf( dbuffer, "BCRR %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
+ Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00900000 ){
+ sprintf( dbuffer, "BCLR %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
+ Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00800000 ){
+ sprintf( dbuffer, "LCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
+ Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00400000 ){
+ sprintf( dbuffer, "LCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
+ Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00200000 ){
+ sprintf( dbuffer, "RCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
+ Thread::wait(10);
+ }
+ else if((flg_exp_status & 0x00F00000) == 0x00100000 ){
+ sprintf( dbuffer, "RCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
+ Thread::wait(10);
+ }
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
+ // pc.printf("\t\t\t S2C: %s\r\n", dbuffer);
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ // -------------------------------------
+ // Transform
+ // -------------------------------------
+ else if( flg_exp_status & 0x20000000 ){
+ sprintf(dbuffer,"RF2I %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // RF Crawler Tfm I
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
+ // pc.printf("\t\t\t S2C: %s\r\n", dbuffer);
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ else if( flg_exp_status & 0x10000000 ){
+ sprintf(dbuffer,"RF2K %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // RF Crawler Tfm K
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ else if( flg_exp_status & 0x80000000 ){
+ sprintf(dbuffer,"LB2I %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // LB Crawler Tfm I
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ else if( flg_exp_status & 0x40000000 ){
+ sprintf(dbuffer,"LB2K %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // LB Crawler Tfm K
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
+ 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)
+ sprintf(dbuffer,"WCDN %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
+ DEBUG_PRINT_WINCH_DATA("Bd0> S2C: %s\r\n", dbuffer);
+ 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)
+ sprintf(dbuffer,"WCUP %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
+ DEBUG_PRINT_WINCH_DATA("Bd0> S2C: %s\r\n", dbuffer);
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ else if( flg_exp_status & 0x0000000f ){
+ if( cnt == 3 ){
+ sprintf( dbuffer,"JSMD %03d %03d %04d %03d\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid, winchCurrentPosition, limitSw_Sts);
+ cnt = 0;
+ }
+ else{
+ sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
+ }
+ cnt++;
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+
+ else{
+ sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
+ Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
+ }
+ // Send data to client PC
+ udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
+ Thread::wait(10);
+
+ // ----------------------------------------------------------------------------
+ // Read target(transform controller) status in each time.
+ rrr = read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_res, 4 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ limitSw_Sts = I2C_res[3];
+ // ----------------------------------------------------------------------------
+
+ }
+ }
+ led4 = 0;
+ }
+}
+
+// **************************************************************
+// 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;
+
+ // Send status to Handy Ctrl controller, but currently this is only for Main Controller.
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = 0x00;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = 0x01;
+ i2c.write(I2C_ADDRESS_HANDY, I2C_cmd, NumberOfI2CCommand);
+ // DEBUG_PRINT_L2("Bd0> Send Handy Controller(%02x) >>>>>>>>>>\r\n",I2C_ADDRESS_HANDY );
+
+ Thread::wait(500);
+ }
+ }
+}
+
+// ======================================================================
+// Read setting value from lpcal file system of mbed
+// ======================================================================
+
+bool read_LFS( setValue_t* setValue ){
+ FILE *fp;
+ char *fname = "/local/set.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,"%04d",&data);setValue->winchCtrl.sv_WDM_ith_F=data; pc.printf("%04d",setValue->winchCtrl.sv_WDM_ith_F); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WDM_ith_R=data; pc.printf("%04d",setValue->winchCtrl.sv_WDM_ith_R); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRM_ith_F=data; pc.printf("%04d",setValue->winchCtrl.sv_WRM_ith_F); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRM_ith_R=data; pc.printf("%04d",setValue->winchCtrl.sv_WRM_ith_R); fgets(s,100,fp); pc.printf("%s",s);
+
+ fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_hsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_hsrto_F); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_hsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_hsrto_R); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_lsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_lsrto_F); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_lsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_lsrto_R); fgets(s,100,fp); pc.printf("%s",s);
+
+ fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_hsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_hsrto_F); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_hsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_hsrto_R); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_lsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_lsrto_F); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_lsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_lsrto_R); fgets(s,100,fp); pc.printf("%s",s);
+
+ fscanf(fp,"%05d",&data);setValue->winchCtrl.sv_WRS_DramDmrx100=data; pc.printf("%04d",setValue->winchCtrl.sv_WRS_DramDmrx100); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%05d",&data);setValue->winchCtrl.sv_WRS_CCableDmrx100=data; pc.printf("%04d",setValue->winchCtrl.sv_WRS_CCableDmrx100); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRS_RResolution=data; pc.printf("%03d",setValue->winchCtrl.sv_WRS_RResolution); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->winchCtrl.reserved=data; pc.printf("%03d",setValue->winchCtrl.reserved); fgets(s,100,fp); pc.printf("%s",s);
+
+ fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_RFTM_ith_F=data; pc.printf("%04d",setValue->tfmCtrl.sv_RFTM_ith_F); fgets(s, 100, fp ); pc.printf("%s",s);
+ fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_RFTM_ith_R=data; pc.printf("%04d",setValue->tfmCtrl.sv_RFTM_ith_R); fgets(s, 100, fp ); pc.printf("%s",s);
+ fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_LBTM_ith_F=data; pc.printf("%04d",setValue->tfmCtrl.sv_LBTM_ith_F); fgets(s, 100, fp ); pc.printf("%s",s);
+ fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_LBTM_ith_R=data; pc.printf("%04d",setValue->tfmCtrl.sv_LBTM_ith_R); fgets(s, 100, fp ); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_RFTM_srto_F=data; pc.printf("%03d",setValue->tfmCtrl.sv_RFTM_srto_F); fgets(s, 100, fp ); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_RFTM_srto_R=data; pc.printf("%03d",setValue->tfmCtrl.sv_RFTM_srto_R); fgets(s, 100, fp ); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_LBTM_srto_F=data; pc.printf("%03d",setValue->tfmCtrl.sv_LBTM_srto_F); fgets(s, 100, fp ); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_LBTM_srto_R=data; pc.printf("%03d",setValue->tfmCtrl.sv_LBTM_srto_R); fgets(s, 100, fp ); pc.printf("%s",s);
+
+ fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_RFCM_ith_F=data; pc.printf("%04d",setValue->crawlerCtrl.sv_RFCM_ith_F); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_RFCM_ith_R=data; pc.printf("%04d",setValue->crawlerCtrl.sv_RFCM_ith_R); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_LBCM_ith_F=data; pc.printf("%04d",setValue->crawlerCtrl.sv_LBCM_ith_F); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_LBCM_ith_R=data; pc.printf("%04d",setValue->crawlerCtrl.sv_LBCM_ith_R); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_srto_F=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_srto_F); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_srto_R=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_srto_R); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_srto_F=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_srto_F); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_srto_R=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_srto_R); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_dzu=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_dzu); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_dzc=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_dzc); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_dzl=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_dzl); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_dzu=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_dzu); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_dzc=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_dzc); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_dzl=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_dzl); fgets(s,100,fp); pc.printf("%s",s );
+ fscanf(fp,"%03d",&data);setValue->crawlerCtrl.reserved1=data; pc.printf("%03d",setValue->crawlerCtrl.reserved1); fgets(s,100,fp); pc.printf("%s",s);
+ fscanf(fp,"%03d",&data);setValue->crawlerCtrl.reserved2=data; pc.printf("%03d",setValue->crawlerCtrl.reserved2); 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;
+}
+
+bool write_LFS_data( char *fname, char* data )
+{
+ FILE *fp;
+ bool rts;
+
+ fp = fopen( fname, "a" );
+ if( fp != NULL ){
+ pc.printf( "Writing ... " );
+ fprintf(fp, data );
+ fprintf(fp, "\r\n" );
+ pc.printf( data );
+ pc.printf( "\r\n" );
+ Thread::wait(30);
+ fclose(fp);
+ Thread::wait(30);
+ rts = true;
+ }
+ else{
+ rts = false;
+ pc.printf("Setting file write data File open error \r\n");
+ }
+ return rts;
+}
+
+bool write_LFS( setValue_t* setValue )
+{
+ FILE *fp;
+ char *fname = "/local/set.txt";
+ bool rts = true;
+ char data[128] = "\0";
+
+ pc.printf("Set setting data to SET.txt\r\n ");
+
+ fp = fopen(fname, "w"); // Open "SET.txt" on the local file system for writing
+ if( fp != NULL ){
+ fprintf(fp, "#### B2 DebrisProbe Setting ####\n");
+ fclose(fp);
+ }
+ else{
+ rts = false;
+ pc.printf("File open error (0)\r\n");
+ }
+
+ sprintf( data, "%04d # 01 Winch dram motor fwd c-th", setValue->winchCtrl.sv_WDM_ith_F); write_LFS_data( fname, data );
+ sprintf( data, "%04d # 02 Winch dram motor rvs c-th", setValue->winchCtrl.sv_WDM_ith_R); write_LFS_data( fname, data );
+ sprintf( data, "%04d # 03 Winch cable motor fwd c-th", setValue->winchCtrl.sv_WRM_ith_F); write_LFS_data( fname, data );
+ sprintf( data, "%04d # 04 Winch cable motor rvs c-th", setValue->winchCtrl.sv_WRM_ith_R); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 05 Winch dram motor fws speed high", setValue->winchCtrl.sv_WDM_hsrto_F); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 06 Winch dram motor rvs speed hign", setValue->winchCtrl.sv_WDM_hsrto_R); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 07 Winch dram motor fwd speed slow", setValue->winchCtrl.sv_WDM_lsrto_F); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 08 Winch dram motor rvs speed slow", setValue->winchCtrl.sv_WDM_lsrto_R); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 09 Winch cable motor fwd speed high", setValue->winchCtrl.sv_WRM_hsrto_F); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 10 Winch cable motor rvs speed high", setValue->winchCtrl.sv_WRM_hsrto_R); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 11 Winch cable motor fwd speed slow", setValue->winchCtrl.sv_WRM_lsrto_F); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 12 Winch cable motor rvs speed slow", setValue->winchCtrl.sv_WRM_lsrto_R); write_LFS_data( fname, data );
+ sprintf( data, "%05d # 13 Winch dram diameterx100", setValue->winchCtrl.sv_WRS_DramDmrx100); write_LFS_data( fname, data );
+ sprintf( data, "%05d # 14 Winch cable diameterx100", setValue->winchCtrl.sv_WRS_CCableDmrx100); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 15 Winch resolver resolution", setValue->winchCtrl.sv_WRS_RResolution); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 16 reserved", setValue->winchCtrl.reserved); write_LFS_data( fname, data );
+ sprintf( data, "%04d # 17 RF transform motor fwd c-th", setValue->tfmCtrl.sv_RFTM_ith_F); write_LFS_data( fname, data );
+ sprintf( data, "%04d # 18 RF transform motor rvs c-th", setValue->tfmCtrl.sv_RFTM_ith_R); write_LFS_data( fname, data );
+ sprintf( data, "%04d # 19 LB transform motor fwd F-C", setValue->tfmCtrl.sv_LBTM_ith_F); write_LFS_data( fname, data );
+ sprintf( data, "%04d # 20 LB transform motor rvs R-C", setValue->tfmCtrl.sv_LBTM_ith_R); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 21 RF transform motor fwd speed", setValue->tfmCtrl.sv_RFTM_srto_F); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 22 RF transform motor rvs speed", setValue->tfmCtrl.sv_RFTM_srto_R); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 23 LB transform motor fwd speed slow", setValue->tfmCtrl.sv_LBTM_srto_F); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 24 LB transform motor rvs speed slow", setValue->tfmCtrl.sv_LBTM_srto_R); write_LFS_data( fname, data );
+ sprintf( data, "%04d # 25 RF crawler fwd c-th", setValue->crawlerCtrl.sv_RFCM_ith_F); write_LFS_data( fname, data );
+ sprintf( data, "%04d # 26 RF crawler rvs c-th", setValue->crawlerCtrl.sv_RFCM_ith_R); write_LFS_data( fname, data );
+ sprintf( data, "%04d # 27 LB crawler fwd c-th", setValue->crawlerCtrl.sv_LBCM_ith_F); write_LFS_data( fname, data );
+ sprintf( data, "%04d # 28 LB crawler rvs c-th", setValue->crawlerCtrl.sv_LBCM_ith_R); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 29 RF crawler fwd speed", setValue->crawlerCtrl.sv_RFCM_srto_F); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 30 RF crawler rvs speed", setValue->crawlerCtrl.sv_RFCM_srto_R); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 31 LB crawler fwd speed", setValue->crawlerCtrl.sv_LBCM_srto_F); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 32 LB crawler rvs speed", setValue->crawlerCtrl.sv_LBCM_srto_R); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 33 R joystic upper band width", setValue->crawlerCtrl.sv_RFCM_dzu); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 34 R joystic center value", setValue->crawlerCtrl.sv_RFCM_dzc); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 35 R joystic lower band width", setValue->crawlerCtrl.sv_RFCM_dzl); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 36 L joystic upper band width", setValue->crawlerCtrl.sv_LBCM_dzu); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 37 L joystic center value", setValue->crawlerCtrl.sv_LBCM_dzc); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 38 L joystic lower band width", setValue->crawlerCtrl.sv_LBCM_dzl); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 39 reserved1", setValue->crawlerCtrl.reserved1); write_LFS_data( fname, data );
+ sprintf( data, "%03d # 40 reserved2", setValue->crawlerCtrl.reserved2); write_LFS_data( fname, data );
+
+ pc.printf("settig file write completed \r\n ");
+
+ return rts;
+}
+
+// ======================================================================
+// Read Network setting value from lpcal file system of mbed
+// ======================================================================
+int read_NetSetting_lfs( char* ip_address, char* subnet_mask, char* gateway )
+{
+ FILE *rfp;
+
+ DEBUG_PRINT_L3("Bd0> Read Network Setting data from local file system \r\n");
+ rfp = fopen("/local/net.txt", "r"); // Open local file "set.txt" for writing
+ if(!rfp){
+ DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
+ return _FAIL_;
+ }
+ else{
+ Thread::wait(50);
+ fscanf(rfp, "%s", ip_address);
+ fscanf(rfp, "%s", subnet_mask);
+ fscanf(rfp, "%s", gateway);
+ fclose(rfp);
+ 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_speed1;
+ int man_speed2;
+
+ int cnt;
+ int rrr;
+
+ int cnt2 = 0;
+
+
+ bool flg_stop_operation = false;
+ bool flg_network_error = false;
+
+ int16_t winchTempPosition;
+
+ char I2C_read[NumberOfI2CCommand+1];
+ char I2C_readcmd[NumberOfI2CCommand+1];
+
+ char winchPresetPosition[2];
+
+ int temp;
+
+// if (hwbtn_Opeflg == 1){
+// Thread::wait(1);
+// }
+// else{
+
+
+ if( flg_ButtonOn == true ) {Thread::wait(2);}
+
+ I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+
+ if( mode == WINCH_POSITION_CLEAR ){
+ led3 = ON;
+ DEBUG_PRINT_L3("Bd0> === WINCH_POSITION_CLEAR ===\r\n");
+ rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
+ if( rcv_data_cnt < 0 ){
+ DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
+ // tcp_client.send( (char*)winchDataP, winchData_s);
+ // break;
+ }
+ else{
+ DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
+ // if( !strcmp( dbuffer, "WinchStepDnOf" ) ){
+ // break;
+ // }
+ }
+
+ I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Zero clear
+ i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ led3 = OFF;
+ }
+ else if ( mode == WINCH_PRESET_BASEDATA )
+ {
+ led3 = ON;
+ DEBUG_PRINT_L3("Bd0> === WINCH_PRESET_BASE_DATA ===\r\n");
+ rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
+ if( rcv_data_cnt < 0 ){
+ DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
+ }
+ else{
+ DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
+ }
+ DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
+ DEBUG_PRINT_L0("Bd0> 4. Send the Calculation base data to Resolver Controller");
+ DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
+ I2C_cmd[I2C_CP_COMMAND_R] = 'R'; // 01: Zero clear
+ // I2C_cmd[I2C_CP_WDRAM_DIA_UPPER] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100>>8)&0xFF); // Dram diameter upper
+ // I2C_cmd[I2C_CP_WDRAM_DIA_LOWER] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100)&0xFF); // Dram diameter lower
+ I2C_cmd[I2C_CP_WDRAM_DIA_UPPER] = (uint8_t)((winchDramDiameter>>8)&0xFF); // Dram diameter upper
+ I2C_cmd[I2C_CP_WDRAM_DIA_LOWER] = (uint8_t)((winchDramDiameter)&0xFF); // Dram diameter lower
+ I2C_cmd[I2C_CP_CCABLE_DIA_UPPER] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100>>8)&0xFF); // Cable diameter upper
+ I2C_cmd[I2C_CP_CCABLE_DIA_LOWER] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100)&0xFF); // Cable diameter lower
+ I2C_cmd[I2C_CP_RESOLVER_RESO] = setValue.winchCtrl.sv_WRS_RResolution; // Resolver resolution
+ i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ led3 = OFF;
+ }
+ else if ( mode == WINCH_PRESET_POSITION )
+ {
+ led3 = ON;
+ winchOffsetValue = 0;
+ DEBUG_PRINT_L3("Bd0> === WINCH_PRESET_CURRENT_POSITION ===\r\n");
+ rcv_data_cnt = tcp_client.receive( winchPresetPosition, sizeof(winchPresetPosition));
+
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
+ if( rcv_data_cnt < 0 ){
+ DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
+ }
+ else{
+ DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
+ DEBUG_PRINT_L0("Bd0> =================================================================\r\n");
+ I2C_cmd[I2C_CP_COMMAND_R] = 'S';
+ I2C_cmd[I2C_CP_PRESET_CPOS_UPPER] = (uint8_t)(winchPresetPosition[1]); // <<<<!!!!!!
+ I2C_cmd[I2C_CP_PRESET_CPOS_LOWER] = (uint8_t)(winchPresetPosition[0]); // <<<<!!!!!!
+ DEBUG_PRINT_L0("Bd0> Preset winch position: %02x %02x (%d) \r\n", winchPresetPosition[1], winchPresetPosition[0], winchOffsetValue);
+ i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ DEBUG_PRINT_L0("Bd0> =================================================================\r\n");
+ }
+ led3 = OFF;
+ }
+ else if (( mode == WINCH_MMODE_RELATIVE ) || ( mode == WINCH_MMODE_ABSOLUTE )){
+ if ( mode == WINCH_MMODE_RELATIVE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_RELATIVE === \r\n");
+ if ( mode == WINCH_MMODE_ABSOLUTE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_ABSOLUTE === \r\n");
+
+ rcv_data_cnt = tcp_client.receive( dbuffer, dbuffer_s);
+
+ *(dbufferP+rcv_data_cnt) = '\0';
+ winchDataP->operation = '\r\n';
+
+ DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
+ // Copy received data to Winch data structure.
+ memcpy( winchDataP, (winchData_t *)dbuffer, winchData_s);
+ // winchDataP->dt_WinchDstPosition += winchDataP->dt_WinchCntPosition;
+ DEBUG_PRINT_L3("Bd0> Winch Rtv Move [ From %04d >>> To %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
+
+ swbtn_OpeMutex.lock();
+ swbtn_Opeflg = 1;
+ swbtn_OpeMutex.unlock();
+
+ cnt = 0;
+ cnt2 = 0;
+
+ while( true ){
+ while( true ){
+ led3 = ON;
+ DEBUG_PRINT_L3("Bd0> == Winch Position ==============\r\n");
+ DEBUG_PRINT_L3("Bd0> CURRENT : %d\r\n", winchDataP->dt_WinchCntPosition );
+ DEBUG_PRINT_L3("Bd0> DESTINATION: %d\r\n", winchDataP->dt_WinchDstPosition );
+ DEBUG_PRINT_L3("Bd0> ================================\r\n");
+
+ tcp_client.set_blocking(false, 500); // Timeout after (3500) msec
+
+ tcp_client.send( (char*)winchDataP, winchData_s);
+ DEBUG_PRINT_L3("Bd0> Send Winch Rtv data [ %04d >>>> %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
+
+ rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
+ if( rcv_data_cnt < 0 ){
+ DEBUG_PRINT_L3("##ERROR## Data receive\r\n" );
+ pc.printf("##ERROR## Data receive Stop Auto Moving mode\r\n" );
+ cnt++;
+ if( cnt > 5 ) flg_stop_operation = true;
+ flg_network_error = true;
+ break;
+ }
+ else{
+ DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
+ if( !strcmp( dbuffer, "WinchRtvStop" ) ){
+ flg_stop_operation = true;
+ break;
+ }
+ }
+ // Forward rotation : winch down
+ if( winchDataP->dt_WinchCntPosition < winchDataP->dt_WinchDstPosition ){
+ pc.printf("WINCH DN\r\n");
+ set_winchMotorSpeed(); // 2016.08.05 for Fix falling winch motor speed adjustment
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor1 FWD
+ if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
+ I2C_cmd[I2C_CP_M1_SPEED] = (setValue.winchCtrl.sv_WDM_lsrto_F >> 1); // very slow speed
+ I2C_cmd[I2C_CP_M2_SPEED] = (setValue.winchCtrl.sv_WRM_lsrto_F >> 1); // very slow speed
+ }
+ else if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_lsrto_F; // slow speed
+ I2C_cmd[I2C_CP_M2_SPEED] = setValue.winchCtrl.sv_WRM_lsrto_F; // slow speed
+ }
+ else{
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_F; // normal speed
+ I2C_cmd[I2C_CP_M2_SPEED] = setValue.winchCtrl.sv_WRM_hsrto_F; // normal speed
+ }
+ temp = 1;
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ pc.printf("SPEED [%d, %d]\r\n", I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M2_SPEED]);
+ }
+ // Reverse rotation : winch up
+ else if ( winchDataP->dt_WinchCntPosition > winchDataP->dt_WinchDstPosition ){
+ pc.printf("WINCH UP\r\n");
+ set_winchMotorSpeed(); // 2016.08.05 for Fix falling winch motor speed adjustment
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor1 RVS
+ if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
+ I2C_cmd[I2C_CP_M1_SPEED] = (setValue.winchCtrl.sv_WDM_lsrto_R >> 1); // very slow speed
+ I2C_cmd[I2C_CP_M2_SPEED] = (setValue.winchCtrl.sv_WRM_lsrto_R >> 1); // very slow speed
+ }
+ else if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_lsrto_R; // slow speed
+ I2C_cmd[I2C_CP_M2_SPEED] = setValue.winchCtrl.sv_WRM_lsrto_R; // slow speed
+ }
+ else{
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_R; // normal speed
+ I2C_cmd[I2C_CP_M2_SPEED] = setValue.winchCtrl.sv_WRM_hsrto_R; // normal speed
+ }
+ temp = 2;
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ pc.printf("SPEED [%d, %d]\r\n", I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M2_SPEED]);
+ }
+
+ // Read winch current position from Resolver.
+
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER, temp);
+ if( winchTempPosition != 9999 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+
+ if ( winchCurrentPosition == winchDataP->dt_WinchDstPosition ){
+ pc.printf("!!! WINCH STOP !!!\r\n");
+ break;
+ }
+
+ mtx_wcp.lock();
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ mtx_wcp.unlock();
+ winchDataP->operation = 0x00;
+ //i2c.read(I2C_ADDRESS_RESOLVER, I2C_resdata, 2); // Read
+ //res_position = (I2C_resdata[1] << 8) | I2C_resdata[0];
+ // --------------------------------------------------
+ // Read motor current => commented out : 2016.11.08
+ // --------------------------------------------------
+/*
+ rrr = read_motorCurrent( I2C_ADDRESS_WINCH, I2C_read, 3 );
+ 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> 15: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
+ DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
+ DEBUG_PRINT_WINCH_DATA("Bd0> MC1(%d) MC2(%d) Ope(%d)\r\n", winchDataP->dt_WinchMotor1Current, winchDataP->dt_WinchMotor2Current, winchDataP->operation);
+*/
+ led3 = OFF;
+ if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) == 0 ){
+ // if( winchDataP->dt_WinchCntPosition == winchDataP->dt_WinchDstPosition ){
+ DEBUG_PRINT_L3( "Bd0> Current:%d -> Destination:%d\r\n" , winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition);
+ break;
+ }
+ Thread::wait(1); // Time interval for program debugging
+ // 2016.08.31: Following will be commented out. Must not send IIC commnad after read motor current !!
+ // i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ }
+
+ DEBUG_PRINT_L3( "Bd0> ! Winch Stop\r\n" );
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 STOP
+ I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 STOP
+ I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ if( flg_network_error == true )
+ {
+ // Network Error then abort !
+ break;
+ }
+
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER, 0);
+ if( winchTempPosition != 9999 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+
+ mtx_wcp.lock();
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ mtx_wcp.unlock();
+ winchDataP->operation = 0x00;
+ DEBUG_PRINT_L3("Bd0> Check destination is same to setting point or not: %d\r\n", winchCurrentPosition);
+ if( winchDataP->dt_WinchDstPosition == winchCurrentPosition ){
+ cnt++;
+ DEBUG_PRINT_L3("Bd0> Destination is same to setting point, then stop operation\r\n" );
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER, 0);
+ if( winchTempPosition != 9999 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ if ( cnt >= 5 ){
+ break; // When final destination == set point , then break. else adjust position again.
+ }
+ }
+ // Force Stop by Stop button
+ if( flg_stop_operation == true ){
+ DEBUG_PRINT_L3("Bd0> Winch auto operation force stop\r\n" );
+ flg_stop_operation = false;
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ break;
+ }
+ }
+/*
+ Thread::wait(300); // Time interval for program debugging
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ mtx_wcp.unlock();
+ winchDataP->operation = 0x00;
+ DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
+ tcp_client.send( (char*)winchDataP, winchData_s);
+
+ Thread::wait(300); // Time interval for program debugging
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ mtx_wcp.unlock();
+ winchDataP->operation = 0x00;
+ DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
+ tcp_client.send( (char*)winchDataP, winchData_s);
+*/
+ Thread::wait(300); // Time interval for program debugging
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER, 0);
+ if( winchTempPosition != 9999 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ mtx_wcp.unlock();
+ winchDataP->operation = 0x77;
+ DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
+ pc.printf("SEND 0x99 0x99 0x99 [ %x]\r\n", winchDataP->operation);
+ tcp_client.send( (char*)winchDataP, winchData_s);
+
+ swbtn_OpeMutex.lock();
+ swbtn_Opeflg = 0;
+ swbtn_OpeMutex.unlock();
+
+ led3 = OFF;
+ tcp_client.set_blocking(false, 3500); // Timeout after (3500) msec
+ }
+
+ // ----------------------------------------------------------
+ // 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 ===\r\n" );
+ if ( mode == WINCH_U_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPDOWN_BTN_ON ===\r\n" );
+
+ swbtn_OpeMutex.lock();
+ swbtn_Opeflg = 1;
+ swbtn_OpeMutex.unlock();
+ while( 1 ){
+ tcp_client.set_blocking(false, 500); // Timeout after (3500) msec
+ led3 = ON;
+ DEBUG_PRINT_L3("Bd0> WINCH_STEPDOWN_BTN_ON\r\n");
+ rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
+ if( rcv_data_cnt < 0 ){
+ DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
+ // tcp_client.send( (char*)winchDataP, winchData_s);
+ break;
+ }
+ else{
+ DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
+ if(( !strcmp( dbuffer, "WinchStepDnOf" ))||( !strcmp( dbuffer, "WinchuStepDnOf" )) ){
+ break;
+ }
+ }
+
+ set_winchMotorSpeed(); // 2016.08.05 for Fix falling winch motor speed adjustment
+ // if ( mode == WINCH_U_STEPDOWN_BTN_ON ) man_speed = 50;
+ // else man_speed = 100;
+ if ( mode == WINCH_U_STEPDOWN_BTN_ON ){
+ man_speed1 = (setValue.winchCtrl.sv_WDM_hsrto_F >> 1);
+ man_speed2 = (setValue.winchCtrl.sv_WRM_hsrto_F >> 1);
+ }
+ else{
+ man_speed1 = setValue.winchCtrl.sv_WDM_hsrto_F;
+ man_speed2 = setValue.winchCtrl.sv_WRM_hsrto_F;
+ }
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
+ I2C_cmd[I2C_CP_M1_SPEED] = man_speed1; // Speed
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor1 FWD
+ I2C_cmd[I2C_CP_M2_SPEED] = man_speed2; // Speed
+ DEBUG_PRINT_L3("Bd0> MFSPEED: %02x %02x \r\n", I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_SPEED] );
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ led3 = OFF;
+
+/*
+ rrr = read_motorCurrent( I2C_ADDRESS_WINCH, I2C_read, 3 );
+ 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> 16: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
+ if( winchDataP->operation == 0x88 ){
+ winchDataP->dt_WinchMotor1Current = 0xFF;
+ }
+*/
+ // 2016.08.31: Following will be commented out. Must not send IIC commnad after read motor current !!
+ // i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
+
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER, 1);
+ if( winchTempPosition != 9999 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition;
+ mtx_wcp.unlock();
+ winchDataP->operation = 0x00;
+ // ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x00 );
+ tcp_client.send( (char*)winchDataP, winchData_s);
+ DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
+ // Thread::wait(2); // Time interval for program debugging
+ // 2016.08.31: Following will be commented out. Must not send IIC commnad after read motor current !!
+ // i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ }
+ DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 FWD
+ I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 FWD
+ I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
+
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER, 0);
+ if( winchTempPosition != 9999 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition;
+ mtx_wcp.unlock();
+ winchDataP->operation = 0x77;
+ //ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x77 );
+ tcp_client.send( (char*)winchDataP, winchData_s);
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ swbtn_OpeMutex.lock();
+ swbtn_Opeflg = 0;
+ swbtn_OpeMutex.unlock();
+ led3 = OFF;
+ tcp_client.set_blocking(false, 3500); // Timeout after (3500) msec
+ }
+ // ----------------------------------------------------------
+ // 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 ===\r\n" );
+ if ( mode == WINCH_U_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPUP_BTN_ON ===\r\n" );
+
+ swbtn_OpeMutex.lock();
+ swbtn_Opeflg = 1;
+ swbtn_OpeMutex.unlock();
+ while( 1 ){
+ tcp_client.set_blocking(false, 500); // Timeout after (3500) msec
+ led3 = ON;
+ DEBUG_PRINT_L3("Bd0> WINCH_STEPUP_BTN_ON\r\n");
+ rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
+ if( rcv_data_cnt < 0 ){
+ DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
+ // tcp_client.send( (char*)winchDataP, winchData_s);
+ break;
+ }
+ else{
+ DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
+ if(
+ ( !strcmp( dbuffer, "WinchStepUpOf" ))||(!strcmp( dbuffer, "WinchuStepUpOf" )) ){
+ break;
+ }
+ }
+
+ set_winchMotorSpeed(); // 2016.08.05 for Fix falling winch motor speed adjustment
+ // if ( mode == WINCH_U_STEPDOWN_BTN_ON ) man_speed = 50;
+ // else man_speed = 100;
+ if ( mode == WINCH_U_STEPUP_BTN_ON ){
+ man_speed1 = (setValue.winchCtrl.sv_WDM_hsrto_R >> 1);
+ man_speed2 = (setValue.winchCtrl.sv_WRM_hsrto_R >> 1);
+ }
+ else{
+ man_speed1 = setValue.winchCtrl.sv_WDM_hsrto_R;
+ man_speed2 = setValue.winchCtrl.sv_WRM_hsrto_R;
+ }
+
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 FWD
+ I2C_cmd[I2C_CP_M1_SPEED] = man_speed1; // Speed
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor1 FWD
+ I2C_cmd[I2C_CP_M2_SPEED] = man_speed2; // Speed
+ DEBUG_PRINT_L3("Bd0> MRSPEED: %02x %02x \r\n", I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_SPEED] );
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ led3 = OFF;
+ // read winch motor current value is commented out : 2016.11.08
+ /*
+ rrr = read_motorCurrent( I2C_ADDRESS_WINCH, I2C_read, 3 );
+ 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> 17: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
+ if( winchDataP->operation == 0x88 ){
+ winchDataP->dt_WinchMotor1Current = 0xFF;
+ }
+*/
+ // 2016.08.31: Following will be commented out. Must not send IIC commnad after read motor current !!
+ // i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
+
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER, 2);
+ if( winchTempPosition != 9999 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition;
+ mtx_wcp.unlock();
+ winchDataP->operation = 0x00;
+ // ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x00 );
+ tcp_client.send( (char*)winchDataP, winchData_s);
+ DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
+ // Thread::wait(2); // Time interval for program debugging
+ // 2016.08.31: Following will be commented out. Must not send IIC commnad after read motor current !!
+ // i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ }
+ DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 FWD
+ I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 FWD
+ I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
+ if( flg_ButtonOn == false ){
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER, 0);
+ if( winchTempPosition != 9999 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition;
+ mtx_wcp.unlock();
+ winchDataP->operation = 0x77;
+ // ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x77 );
+ tcp_client.send( (char*)winchDataP, winchData_s);
+ }
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+
+ swbtn_OpeMutex.lock();
+ swbtn_Opeflg = 0;
+ swbtn_OpeMutex.unlock();
+
+ led3 = OFF;
+ tcp_client.set_blocking(false, 3500); // Timeout after (3500) msec
+ }
+
+ else {
+ // DEBUG_PRINT_L3("STEPSTEPSTEPSTEPSTEP\r\n");
+ }
+// }
+}
+
+extern "C" {
+#include "rt_TypeDef.h"
+#include "rt_System.h"
+}
+// **********************************************************************
+//
+// Main Function of this program
+//
+// **********************************************************************
+int main()
+{
+ Mutex file_access_mutex;
+ setValue_t new_setValue; // Setting Data
+ winchData_t winchData;
+
+ char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
+// char I2C_res[NumberOfI2CCommand+1] = "\0";
+
+// char* ip_address;
+// char* subnet_mask;
+// char* gateway;
+
+ int ret;
+ int try_cnt;
+ int rcv_data_cnt;
+
+ bool flg_ethernet = false;
+
+ char ttt[20];
+ char winchOffset[4];
+
+ // Set UART(USB) baudrate
+ pc.baud(115200);
+
+ cf_led_demo( &led1, &led2, &led3, &led4, 10, 15 );
+
+ DEBUG_PRINT_L0("\r\n");
+ DEBUG_PRINT_L0("Bd0> +--------------------------------------------------------------\r\n");
+ DEBUG_PRINT_L0("Bd0> | Project: B2 Crawler Explorer for 1F-1 PCV internal inspection\r\n");
+ DEBUG_PRINT_L0("Bd0> |---------\r\n");
+ DEBUG_PRINT_L0("Bd0> | This is: Main Control Program of Main Controller\r\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> ---------------------------------------------------------------\r\n");
+ sprintf( ttt, "%s", ProgramRevision );
+
+ DEBUG_PRINT_L0("Bd0> Start ststem initializing ...\r\n");
+ DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
+ DEBUG_PRINT_L0("Bd0> 1. Initalizing Ethernet ...\r\n");
+ DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
+
+#ifdef __NET_SETTING_FROM_LFS__
+ char ip_address[20];
+ char subnet_mask[20];
+ char gateway[20];
+ read_NetSetting_lfs( ip_address, subnet_mask, gateway );
+#else
+ const char* ip_address;
+ const char* subnet_mask;
+ const char* gateway;
+ ip_address = "192.168.0.24";
+ subnet_mask = "255.255.255.0";
+ gateway = "192.168.0.0";
+#endif
+ DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
+ DEBUG_PRINT_L0("Bd0> ip address : %s\r\n", ip_address);
+ DEBUG_PRINT_L0("Bd0> subnet mask : %s\r\n", subnet_mask);
+ DEBUG_PRINT_L0("Bd0> default gateway: %s\r\n", gateway);
+ DEBUG_PRINT_L0("Bd0> --------------------------------------\r\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\r\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\r\n");
+ DEBUG_PRINT_L0("Bd0> [ IP Address : %s ]\r\n", eth.getIPAddress());
+ udp_server.bind(UDP_SERVER_PORT);
+ tcp_server.bind(TCP_SERVER_PORT);
+ tcp_server.listen();
+ flg_ethernet = true;
+ led4 = ON; // Ethernet OK
+ }
+ else{
+ cf_led_error( &led1,&led2,&led3,&led4 );
+ DEBUG_PRINT_L0("Bd0> ***ERROR*** Eternat connect Fali\r\n");
+ DEBUG_PRINT_L0("Bd0> This programis booting in Stand alone mode.\r\n");
+ }
+ }
+ else{
+ DEBUG_PRINT_L0("Bd0> *** ERROR*** Eternat init Fail\r\n");
+ DEBUG_PRINT_L0("Bd0> This programis booting in Stand alone mode.\r\n");
+ }
+
+ 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("Bd0> =============================================================\r\n");
+ DEBUG_PRINT_L0("Bd0> 2. Read setting value from LFS\r\n");
+ DEBUG_PRINT_L0("Bd0> =============================================================\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
+ // --------------------------------------------------------------------
+ try_cnt = LFS_READ_COUNT;
+ while( 1 ){
+ if( read_LFS(&setValue) == true ) break;
+ else try_cnt -= 1;
+ if( try_cnt == 0 ){
+ DEBUG_PRINT_L0("Bd0> ***ERROR*** LFS read error\r\n");
+ while(1){
+ led1 = !led1;
+ Thread::wait(30);
+ }
+ }
+ }
+
+ DEBUG_PRINT_L0("Bd0> LFS read OK\r\n");
+ led3 = ON; // Setting Data Read OK
+
+#ifdef __TARGET_BOARD_CHECK__
+ //---------------------------------------------------
+ // 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("Bd0> =============================================================\r\n");
+ DEBUG_PRINT_L0("Vd0> 3. Check the target controler\r\n");
+ DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
+
+ try_cnt = TARGET_CHECK_COUNT;
+ while(1){
+ // Check each target motor control 824 board here
+ i2c.read(I2C_ADDRESS_WINCH, I2C_res, NumberOfI2CCommand);
+ if( I2C_res[4] == 'S' ){
+ DEBUG_PRINT_L0("Bd0> Try count : %02d/%02d\r\n", TARGET_CHECK_COUNT - try_cnt, TARGET_CHECK_COUNT );
+ DEBUG_PRINT_L0("Bd0> Return from (0x%02x) : '%c'\r\n", I2C_ADDRESS_WINCH, I2C_res[4]);
+ break;
+ }
+ else try_cnt -= 1;
+ if( try_cnt == 0 ){
+ DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_WINCH);
+ led1 = OFF;
+ while(1){
+ led1 = !led1; // ON
+ Thread::wait(80);
+ }
+ }
+ }
+
+ try_cnt = TARGET_CHECK_COUNT;
+ while(1){
+ // Check each target motor control 824 board here
+ i2c.read(I2C_ADDRESS_TRANSFORM, I2C_res, NumberOfI2CCommand);
+ if( I2C_res[4] == 'S' ){
+ DEBUG_PRINT_L0("Bd0> Try count: %02d/%02d\r\n", TARGET_CHECK_COUNT - try_cnt, TARGET_CHECK_COUNT );
+ DEBUG_PRINT_L0("Bd0> Return from (0x%02x): '%c'\r\n", I2C_ADDRESS_TRANSFORM, I2C_res[4]);
+ break;
+ }
+ else try_cnt -= 1;
+ if( try_cnt == 0 ){
+ DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_TRANSFORM);
+ led1 = OFF;
+ while(1){
+ led2 = !led2; // ON
+ Thread::wait(80);
+ }
+ }
+ }
+ try_cnt = TARGET_CHECK_COUNT;
+ while(1){
+ // Check each target motor control 824 board here
+ i2c.read(I2C_ADDRESS_CRAWLER, I2C_res, NumberOfI2CCommand);
+ if( I2C_res[4] == 'S' ){
+ DEBUG_PRINT_L0("Bd0> Try count : %02d/%02d\r\n", TARGET_CHECK_COUNT - try_cnt, TARGET_CHECK_COUNT );
+ DEBUG_PRINT_L0("Bd0> Return from (0x%02x) : '%c'\r\n", I2C_ADDRESS_CRAWLER, I2C_res[4]);
+ break;
+ }
+ else try_cnt -= 1;
+ if( try_cnt == 0 ){
+ DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_CRAWLER);
+ led1 = OFF;
+ while(1){
+ led3 = !led3; // ON
+ Thread::wait(80);
+ }
+ }
+ }
+ DEBUG_PRINT_L0("Bd0> -------------------\r\n");
+ DEBUG_PRINT_L0("Bd0> Target system found\r\n");
+ DEBUG_PRINT_L0("Bd0> -------------------\r\n");
+#endif // __TARGET_BOARD_CHECK__
+
+ led2 = ON; // Check target OK
+
+ /* Set basic function default setting */
+ baseOperation.sv_JS_OpeMode = 0;
+ baseOperation.sv_JS_OpeMode = 0;
+ baseOperation.sv_WinchValid = 0;
+
+
+ /*
+ **************************************************
+ Send Calculation Data to Resolver Controller
+ **************************************************
+ */
+ DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
+ DEBUG_PRINT_L0("Bd0> 4. Send the Calculation base data to Resolver Controller");
+ DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
+ I2C_cmd[I2C_CP_COMMAND_R] = 'R'; // 01: Preset resolver base data
+ // I2C_cmd[I2C_CP_WDRAM_DIA_UPPER] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100>>8)&0xFF); // Dram diameter upper
+ // I2C_cmd[I2C_CP_WDRAM_DIA_LOWER] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100)&0xFF); // Dram diameter lower
+ I2C_cmd[I2C_CP_WDRAM_DIA_UPPER] = (uint8_t)((winchDramDiameter>>8)&0xFF); // Dram diameter upper
+ I2C_cmd[I2C_CP_WDRAM_DIA_LOWER] = (uint8_t)((winchDramDiameter)&0xFF); // Dram diameter lower
+ I2C_cmd[I2C_CP_CCABLE_DIA_UPPER] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100>>8)&0xFF); // Cable diameter upper
+ I2C_cmd[I2C_CP_CCABLE_DIA_LOWER] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100)&0xFF); // Cable diameter lower
+ I2C_cmd[I2C_CP_RESOLVER_RESO] = setValue.winchCtrl.sv_WRS_RResolution; // Resolver resolution
+
+ for( int j = 0; j < NumberOfI2CCommand; j++)
+ DEBUG_PRINT_L0("%02x ", I2C_cmd[j]);
+ DEBUG_PRINT_L0( "\r\n" );
+
+ i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ DEBUG_PRINT_L0(" ... done\r\n");
+
+ // Thread (
+ // void(*task)(void const *argument),
+ // void *argument=NULL,
+ // osPriority priority=osPriorityNormal,
+ // uint32_t stack_size=DEFAULT_STACK_SIZE,
+ // unsigned char *stack_pointer=NULL
+ //)
+
+ DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
+ DEBUG_PRINT_L0("Bd0> 5. Start the task\r\n");
+ /* Max thread count is (may be ..) 2, How can I increase this , I don't know ?? */
+// Thread thread_hif(clientPC_interface_task, NULL, osPriorityNormal, DEFAULT_STACK_SIZE*2.25);
+// if( flg_ethernet == true )
+// {
+ DEBUG_PRINT_L0("Bd0> Start host interface task ... ");
+ Thread thread_hif(clientPC_interface_task, NULL, osPriorityNormal, 128*4);
+ DEBUG_PRINT_L0("\r\nBd0> Start host gamepad task ... ");
+ Thread thread_gpd(gamepad_task, NULL, osPriorityNormal, 128*4);
+// }
+// else{
+// DEBUG_PRINT_L0("\r\nBd0> Start host gamepad task ... ");
+// Thread thread_gpd(gamepad_task, NULL, osPriorityNormal, 256*4);
+// }
+ DEBUG_PRINT_L0("\r\n");
+ DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
+
+ DEBUG_PRINT_L0( "Bd0> ----------------------------------\r\n");
+ DEBUG_PRINT_L0( "Bd0> >>>> Initializing completed ! <<<<\r\n");
+ DEBUG_PRINT_L0( "Bd0> ----------------------------------\r\n");
+
+ led4 = OFF;
+ led3 = OFF;
+ led2 = OFF;
+ led1 = ON; // Initializing is OK then Power Indicator LED ON
+
+ I2C_cmd[4] = 0x00;
+ I2C_cmd[5] = 0x01;
+ i2c.write(I2C_ADDRESS_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 (3500) msec
+ DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
+ DEBUG_PRINT_L3("Bd0> TCP Connection from: %s\r\n", tcp_client.get_address());
+ DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
+
+ while(true){
+
+ // --------------------------------------------------------------
+ // Following instructions are blocking when no ethernet access
+ // --------------------------------------------------------------
+ rcv_data_cnt = tcp_client.receive(dbuffer, sizeof(dbuffer));
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
+ if( rcv_data_cnt < 0 ){
+ // DEBUG_PRINT("## TCP Receive packet fail ##\r\n");
+ break;
+ }
+ else{
+ if( !strcmp( dbuffer, "WinchPositionClear" ) ){
+ winchMovingControl(
+ WINCH_POSITION_CLEAR,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ winchOffsetValue = 0;
+ DEBUG_PRINT_L0( "Bd0> WinchPositionClear\r\n");
+ }
+ else if( !strcmp( dbuffer, "WinchPresetBaseData" ) ){
+ winchMovingControl(
+ WINCH_PRESET_BASEDATA,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ DEBUG_PRINT_L0( "Bd0> WinchPresetBaseData\r\n");
+ }
+ else if( !strcmp( dbuffer, "WinchPresetPosition" ) ){
+ winchMovingControl(
+ WINCH_PRESET_POSITION,
+ dbuffer,
+ sizeof(dbuffer),
+ &winchData,
+ sizeof( winchData ),
+ I2C_cmd
+ );
+ winchOffsetValue = 0;
+ DEBUG_PRINT_L0( "Bd0> WinchPresetPosition\r\n");
+ }
+ 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, "SetOffset" ) ){
+ // Get winch offset value and winch bash parameter ( dram diameter ) from host PC.
+ // 2017.01.06 added
+ strcpy( dbuffer, "\0" ); // Prevent for this instruction is done twice.
+ DEBUG_PRINT_L3("Bd0> SetOffset value Request from client\r\n");
+ rcv_data_cnt = tcp_client.receive( winchOffset, sizeof(winchOffset));
+ winchOffsetValue = (winchOffset[1]<<8 | winchOffset[0]);
+ winchDramDiameter = (winchOffset[3]<<8 | winchOffset[2]);
+ DEBUG_PRINT_L0("//////////////////////////////////////////////////////////////////////////\r\n");
+ DEBUG_PRINT_L0("Bd0> Set offset instruction received [ cnt=%d, offset=%d, parameter=%d\r\n", rcv_data_cnt, winchOffsetValue, winchDramDiameter );
+ DEBUG_PRINT_L0("//////////////////////////////////////////////////////////////////////////\r\n");
+ Thread::wait(1500);
+ }
+ else if( !strcmp( dbuffer, "SetValue" ) ){
+ strcpy( dbuffer, "\0" ); // Prevent for this instruction is done twice.
+ DEBUG_PRINT_L3("Bd0> SetValue Request from client\r\n");
+ rcv_data_cnt = tcp_client.receive( (char*)&new_setValue, sizeof(new_setValue));
+ Thread::wait(1500);
+ DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
+ dspSetValue2Console( &pc, &new_setValue );
+ // DEBUG_PRINT_L0("Bd0> write setting file to local file sysytem\r\n");
+ // thread_hif.terminate();
+ // thread_gpd.terminate();
+ // file_access_mutex.lock();
+ write_LFS(&new_setValue);
+ // file_access_mutex.unlock();
+ Thread::wait(1000);
+ DEBUG_PRINT_L0("Bd0> SetValue instruction is over\r\n");
+ }
+ else if(!strcmp( dbuffer, "GetValue" )){
+ DEBUG_PRINT_L3("Bd0> GetValue request from TCP client\r\n");
+ flg_mutex.lock(); // This is very important
+ memcpy( &new_setValue, &setValue, sizeof( new_setValue ) );
+ flg_mutex.unlock(); // This is very important
+ dspSetValue2Console( &pc, &new_setValue );
+ tcp_client.send_all( (char*)&new_setValue, sizeof(new_setValue) );
+ DEBUG_PRINT_L2("(%d)\r\n", sizeof(new_setValue));
+ }
+ else if(!strcmp( dbuffer, "SetJSSingle" )){
+ baseOperation.sv_JS_OpeMode = 0;
+ }
+ else if(!strcmp( dbuffer, "SetJSDual" )){
+ baseOperation.sv_JS_OpeMode = 1;
+ }
+ }
+ if( rcv_data_cnt <= 0 ) break;
+ }
+
+ tcp_client.close();
+ }
+ }
+}
\ No newline at end of file
--- a/EthernetInterface.lib Thu Apr 14 10:25:08 2016 +0000 +++ b/EthernetInterface.lib Thu Jul 26 00:20:39 2018 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/EthernetInterface/#2fc406e2553f +http://mbed.org/users/mbed_official/code/EthernetInterface/#183490eb1b4a
--- a/USBDevice.lib Thu Apr 14 10:25:08 2016 +0000 +++ b/USBDevice.lib Thu Jul 26 00:20:39 2018 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/mbed_official/code/USBDevice/#2af474687369 +http://developer.mbed.org/users/mbed_official/code/USBDevice/#01321bd6ff89
--- a/USBHost.lib Thu Apr 14 10:25:08 2016 +0000 +++ b/USBHost.lib Thu Jul 26 00:20:39 2018 +0000 @@ -1,1 +1,1 @@ -https://mbed.org/users/mbed_official/code/USBHost/#8fbe33e98b2b +https://mbed.org/users/mbed_official/code/USBHost/#1675750cca08
--- a/common.h Thu Apr 14 10:25:08 2016 +0000
+++ b/common.h Thu Jul 26 00:20:39 2018 +0000
@@ -4,19 +4,29 @@
http://tricky-code.net/nicecode/code10.php
*/
/* Information */
-#define LatestUpDate "2016.04.07"
-#define ProgramRevision "Rev 0.941"
-#define Author "ZNSR"
+#define LatestUpDate "2016.11.04"
+#define ProgramRevision "Rev 2.40"
+#define Author "ZisNotRevast"
#define Company "Revast Co.,Ltd."
//#define __CREATE_SETTING_FILE__
//#define __TARGET_BOARD_CHECK__
+
+//#define __WInchDebug__ // For debugging
+
+
+// Comment out if net setting is FIX.
+// #define __NET_SETTING_FROM_LFS__
+
+
// Read motor current
#define __READ_TFM_MOTOR_CURRENT__
#define __IIC_COMAMND_SEND__
+#define __SET_WINCH_MOTOR_SPEED_BY_VOLUME__
+
// ======================================================================
// For Debugging
// ======================================================================
@@ -24,10 +34,19 @@
#define __DEBUG_L0__
//#define __DEBUG_L1__
//#define __DEBUG_L2__
-#define __DEBUG_L3__
+//#define __DEBUG_L3__
//#define __DEBUG_L4__
//#define __DEBUG_L5__
+#define __DEBUG_WINCH_DATA__
+
+
+#ifdef __DEBUG_WINCH_DATA__
+ #define DEBUG_PRINT_WINCH_DATA(...) pc.printf(__VA_ARGS__)
+#else
+ #define DEBUG_PRINT_WINCH_DATA(...)
+#endif
+
#ifdef __DEBUG_PRINT_SW__
#define DEBUG_PRINT_SW(...) pc.printf(__VA_ARGS__)
#else
@@ -118,15 +137,15 @@
};
/* For resolver reader controller */
enum{
- I2C_CP_PREAMBLE_R, // Preamble of command packet
+ I2C_CP_PREAMBLE_R, // Sub command
I2C_CP_COMMAND_R, // instruction command
I2C_CP_WDRAM_DIA_UPPER, // motor1 rotation direction
I2C_CP_WDRAM_DIA_LOWER, // motor1 rotation speed
I2C_CP_CCABLE_DIA_UPPER, // motor1 current limit detection threshold upper byte
I2C_CP_CCABLE_DIA_LOWER, // motor1 current limit detection threshold lower byte
I2C_CP_RESOLVER_RESO, // motor1 current limit detection threshold upper byte
- I2C_CP_RES1, // reserved
- I2C_CP_RES2, // reserved
+ I2C_CP_PRESET_CPOS_UPPER, // reserved
+ I2C_CP_PRESET_CPOS_LOWER, // reserved
I2C_CP_RES3, // reserved
I2C_CP_RES4, // reserved
I2C_CP_RES5, // reserved
@@ -139,6 +158,7 @@
/* Winch Operating mode */
enum{
WINCH_POSITION_CLEAR,
+ WINCH_PRESET_BASEDATA,
WINCH_MMODE_RELATIVE,
WINCH_MMODE_ABSOLUTE,
WINCH_STEPDOWN_BTN_ON,
@@ -149,6 +169,7 @@
WINCH_U_STEPDOWN_BTN_OFF,
WINCH_U_STEPUP_BTN_ON,
WINCH_U_STEPUP_BTN_OFF,
+ WINCH_PRESET_POSITION,
};
/* Game Pad defnition */
@@ -211,8 +232,8 @@
uint16_t sv_LBTM_ith_R; // 2 LB transform motor Rvs roration current threshold
uint8_t sv_RFTM_srto_F; // 1 RF transform motor Fwd rotation speed ratio
uint8_t sv_RFTM_srto_R; // 1 RF transform motor Rvs rotation speed ratio
- int8_t sv_LBTM_srto_F; // 1 LB transform motor Fwd rotation speed ratio
- int8_t sv_LBTM_srto_R; // 1 LB transform motor Rvs rotation speed ratio
+ uint8_t sv_LBTM_srto_F; // 1 LB transform motor Fwd rotation speed ratio
+ uint8_t sv_LBTM_srto_R; // 1 LB transform motor Rvs rotation speed ratio
} tfm_SetValue_t; // 12 byte
typedef struct {
@@ -251,7 +272,7 @@
-#define SLOWDOWN_DISTANCE 10
+#define SLOWDOWN_DISTANCE 23 // <--10 2016.06.01
#define SLOWDOWN_NEAR_DISTANCE 3
/*
typedef struct {
--- a/mbed.bld Thu Apr 14 10:25:08 2016 +0000 +++ b/mbed.bld Thu Jul 26 00:20:39 2018 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/252557024ec3 \ No newline at end of file +http://mbed.org/users/mbed_official/code/mbed/builds/6c34061e7c34 \ No newline at end of file
--- a/memo.txt Thu Apr 14 10:25:08 2016 +0000 +++ b/memo.txt Thu Jul 26 00:20:39 2018 +0000 @@ -1,3 +1,33 @@ + // ------------------------------------------------------------------------------------------ +1239 i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // <<<<=== is it necessary ? +1240 Thread::wait(1); // <<<<=== is it necessary ? + // ------------------------------------------------------------------------------------------ +クローラが止まりにくい現象はこれが影響していないか？？ + +また、Validエリア判定条件を修正：まともに動くか？？ + + +2016.10.07(Fri) +バグ修正 +レゾルバ位置情報取得のボード４へのI2Cアクセスがエラーになる場合があった。 +エラーの場合関数自体の戻り値で位置情報を返していたため、エラーの場合には +−１、それい以外の場合には位置データが帰ってくきていいた。 +統合プロプログラムにも変える場合があったと思われる。 +（露見しなかったのが不思議レベル） + +ReadWinchCurrentPosition関数を修正し、 +エラーデータが戻らないようにした。 +またエラーの場合、最大３回リトライをするように修正 + +int16_t ReadWinchCurrentPosition( int32_t i2c_addr, int16_t *winchPositionP, int mode ) + + i2c_addr 送信先I2Cアドレス（レゾルバコントローラ） + winchPositionP位置データ + mode 動作モード（未使用） + + 戻り値 0:正常 0以外:エラー + + 2016.03.17(Wed) --------------- 形状変形操作変更