z ysaito
2018.07.26
Dependencies: EthernetInterface TextLCD USBDevice USBHost3 mbed
Fork of
USBHostHub_HelloWorld
by 
Samuel Mokrani
Diff: 0_main.cpp
- Revision:
- 14:3a5ae61ab1f4
- Parent:
- 13:2c70c772fe24
- Child:
- 15:01680ed6b799
diff -r 2c70c772fe24 -r 3a5ae61ab1f4 0_main.cpp
--- a/0_main.cpp Mon Mar 28 00:07:19 2016 +0000
+++ b/0_main.cpp Thu Apr 14 10:25:08 2016 +0000
@@ -62,15 +62,18 @@
// LCD
TextLCD lcd(p11, p12, p24, p23, p22, p21); // rs, e, d4-d7
// Local File System
-LocalFileSystem SettingFile("local"); // Create the local filesystem under the name "local"
+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;
-setValue_t setValue; // Setting Data
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
@@ -88,15 +91,14 @@
Mutex mtx_wcp;
bool flg_ButtonOn = false;
-
-// Mutex hwbtn_OpeMutex;
-// int hwbtn_Opeflg = 0;
+bool flg_lsw_valid = false;
int flg_JS_shape_mode = 0;
int flg_JS_ope_mode = 0;
-uint8_t motor1_current_pct;
-uint8_t motor2_current_pct;
+int motor1_current_pct;
+int motor2_current_pct;
uint8_t limitSw_Sts = 0;
+char motorLock_sts = '\0';
/* Prototype */
@@ -116,7 +118,7 @@
char* I2C_res,
int NumberOfI2Cdata
){
- i2c.read(i2c_addr, I2C_res, 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] );
@@ -127,17 +129,6 @@
}
// ============================================================
-// Send stop command to motor specific controller
-// ============================================================
-void stop_motor( int32_t i2c_addr, char* i2c_cmd, int32_t flg ){
- if (flg == 1 ){
- i2c_cmd[3] = MOTOR_STP;
- i2c_cmd[4] = MOTOR_STP;
- }
- i2c.write(i2c_addr, i2c_cmd, 6 ); // Send command to motor control board.
-}
-
-// ============================================================
// Send Status to PC
// ============================================================
void sendStatus2PC( char *cmd, int32_t numberOfCmd ){
@@ -205,7 +196,7 @@
[8] : N/F
[9] : N/F
*/
- char I2C_cmd[NumberOfI2CCommand+1] = "#010000";
+ char I2C_cmd[NumberOfI2CCommand+1] = "#0100000000000";
char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
uint8_t btnStatus_RFK = 0;
@@ -263,8 +254,8 @@
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_WDN = btn04;
btnStatus_RFK = btn04;
btnStatus_RFI = btn04;
btnStatus_LBK = btn04;
@@ -273,17 +264,17 @@
btnStatus_RJSLftRgt = btn02; // Assign analog js to this sw input by LPC1768mbed
btnStatus_LJSFwdRvs = btn01; // Assign analog js to this sw input by LPC1768mbed
btnStatus_LJSLftRgt = btn00; // Assign analog js to this sw input by LPC1768mbed
- btnStatus_Start = btn05; //
btnStatus_CrossUp = btn06;
btnStatus_CrossDn = btn06;
btnStatus_CrossRt = btn06;
btnStatus_CrossLt = btn06;
+ btnStatus_Start = btn05; //
}
}
else if (gamePadVID == GAMEPAD_VID_ELECOM ){
DEBUG_PRINT_L4("Bd0> [ELECOM] ");
+ btnID_WDN = 4;
btnID_WUP = 2;
- btnID_WDN = 4;
btnID_RFK = 32;
btnID_RFI = 128;
btnID_LBK = 16;
@@ -298,8 +289,8 @@
btnID_CrossRt = 2;
btnID_CrossLt = 6;
if ( gamePadPID == GAMEPAD_PID_ELECOM_JCU3613M ){
+ btnStatus_WDN = btn04;
btnStatus_WUP = btn04;
- btnStatus_WDN = btn04;
btnStatus_RFK = btn04;
btnStatus_RFI = btn04;
btnStatus_LBK = btn04;
@@ -316,8 +307,8 @@
}
}
else if( gamePadVID == GAMEPAD_VID_LOGICOOL ){
+ btnID_WDN = 40;
btnID_WUP = 136;
- btnID_WDN = 40;
btnID_RFK = 2;
btnID_RFI = 8;
btnID_LBK = 1;
@@ -329,8 +320,8 @@
btnID_CrossLt = 6;
if ( gamePadPID == GAMEPAD_PID_LOGICOOL_F710 ){
DEBUG_PRINT_L4("Bd0> [LOGI F710] ");
+ btnStatus_WDN = btn05;
btnStatus_WUP = btn05;
- btnStatus_WDN = btn05;
btnStatus_RFK = btn06;
btnStatus_RFI = btn06;
btnStatus_LBK = btn06;
@@ -338,8 +329,8 @@
}
else if ( gamePadPID == GAMEPAD_PID_LOGICOOL_F310 ){
DEBUG_PRINT_L4("Bd0> [LOGI F310] ");
+ btnStatus_WDN = btn04;
btnStatus_WUP = btn04;
- btnStatus_WDN = btn04;
btnStatus_RFK = btn05;
btnStatus_RFI = btn05;
btnStatus_LBK = btn05;
@@ -348,10 +339,6 @@
btnStatus_RJSLftRgt = btn02;
btnStatus_LJSFwdRvs = btn01;
btnStatus_LJSLftRgt = btn00;
- btnStatus_RJSFwdRvs = btn03;
- btnStatus_RJSLftRgt = btn02;
- btnStatus_LJSFwdRvs = btn01;
- btnStatus_LJSLftRgt = btn00;
btnStatus_Start = btn05; // Guide button status for ELECOM GamePad
btnStatus_CrossUp = btn04;
btnStatus_CrossDn = btn04;
@@ -361,19 +348,35 @@
}
else if ( gamePadVID == GAMEPAD_VID_SANWA){
DEBUG_PRINT_L4("Bd0> [SANWA] ");
+ btnID_WDN = 2;
btnID_WUP = 4;
- btnID_WDN = 2;
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_WDN = 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;
}
}
@@ -381,18 +384,25 @@
// DEBUG_PRINT_BTN(" Btn 00:%d, 01:%d, 02:%d, 03:%d, 04:%d, 05:%d, 06:%d, 07:%d, 08:%d | VID=0x%04x, PID=0x%04x\r\n",
// btn00,btn01,btn02,btn03,btn04,btn05,btn06,btn07,btn08,
// gamePadVID, gamePadPID);
- DEBUG_PRINT_L4("Bd0> -- Button Status -------------------------------\r\n");
- DEBUG_PRINT_L4("Bd0> 00(%02x) 01(%02x) 02(%02x) 03(%02x)\r\n", btn00,btn01,btn02,btn03);
- DEBUG_PRINT_L4("Bd0> 04(%02x) 05(%02x) 06(%02x) 07(%02x) 08(%02x)\r\n", btn04,btn05,btn06,btn07,btn08);
- DEBUG_PRINT_L4("Bd0> ------------------------------------------------\r\n");
+ 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[4] = '\0';
- I2C_cmd[5] = '\0';
- I2C_cmd[6] = '\0';
- I2C_cmd[7] = '\0';
- I2C_cmd[8] = '\0';
- I2C_cmd[9] = '\0';
+ 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;
@@ -400,6 +410,12 @@
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
@@ -438,11 +454,13 @@
/*
*
* GamePad software switch
- * [Guide] + [B] : JS shape mode toggle: I <--> KO
- * [Guide] + [X} : JS Operation mode toggle: Dual <--> Single
+ * 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: Single JSmode, 8: Dual JSmode
+ * |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
@@ -508,25 +526,41 @@
DEBUG_PRINT_L3( "Bd0> BTN RF-K & LB-K\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
- I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
- I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_F; // Speed
- I2C_cmd[6] = MOTOR_FWD; // Motor1 FWD
- I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_F; // Speed
+ 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[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
- I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
- I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
- I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ I2C_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, 4 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ 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;
@@ -537,25 +571,41 @@
DEBUG_PRINT_L3( "Bd0> BTN RF-I & LB-I\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
- I2C_cmd[2] = MOTOR_RVS; // Motor1 FWD
- I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_F; // Speed
- I2C_cmd[6] = MOTOR_RVS; // Motor1 FWD
- I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_F; // Speed
+ 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[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
- I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
- I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
- I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ I2C_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, 4 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ 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;
@@ -566,23 +616,39 @@
DEBUG_PRINT_L3( "Bd0> BTN RF-K\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
- I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
- I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_F; // Speed
+ 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[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
- I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
- I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
- I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ I2C_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, 4 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ 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;
@@ -591,23 +657,39 @@
DEBUG_PRINT_L3( "Bd0> BTN RF-I\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
- I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
- I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_R; // Speed
+ 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[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
- I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
- I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
- I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ I2C_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, 4 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ 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;
@@ -618,24 +700,39 @@
DEBUG_PRINT_L3( "Bd0> BTN LB-K\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
- I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
- I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_F; // Speed
+ 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[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
- I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
- I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
- I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ I2C_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, 4 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ 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;
@@ -645,24 +742,39 @@
DEBUG_PRINT_L3( "Bd0> BTN LB-I\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
- I2C_cmd[6] = MOTOR_RVS; // Motor1 RVS
- I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_R; // Speed
+ 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[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
- I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
- I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
- I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ I2C_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, 4 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ 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;
@@ -674,28 +786,44 @@
else if (baseOperation.sv_WinchValid==0){
led3 = 0;
#ifdef __IIC_COMAMND_SEND__
- I2C_cmd[2] = MOTOR_STP;
- I2C_cmd[3] = 0;
- I2C_cmd[6] = MOTOR_STP;
- I2C_cmd[7] = 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);
#endif
flg_exp_status &= 0x0FFFFFFF;
- I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
- I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
- I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
- I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ I2C_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, 4 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ 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__
}
@@ -703,7 +831,7 @@
/*/ ====================================
// Crawler Moving Control
// ====================================
- // JoyStick mode 1: Dual JoyStick mode
+ // JoyStick mode 1: Independence mode ( Dual JoyStick mode )
//
// *** ***
// * L * * R *
@@ -722,23 +850,31 @@
* +-+-+-+-+-+-+-+-+
*/
if( baseOperation.sv_JS_OpeMode == 1 ){
- I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ I2C_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[6] = MOTOR_FWD; // Motor2 FWD
- tmpSpeed = ( setValue.crawlerCtrl.sv_LBCM_dzc+1 - btnStatus_LJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc * setValue.crawlerCtrl.sv_LBCM_sli_F / 100; // Speed
- I2C_cmd[7] = (char)tmpSpeed;
+ 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, 3 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
- DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ 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;
@@ -748,17 +884,21 @@
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[6] = MOTOR_RVS; // Motor2 RVS
- tmpSpeed = ( btnStatus_LJSFwdRvs - setValue.crawlerCtrl.sv_LBCM_dzc ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc+1 * setValue.crawlerCtrl.sv_LBCM_sli_F / 100; // Speed I2C_cmd[5] = (char)tmpSpeed;
- I2C_cmd[7] = (char)tmpSpeed;
+ 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, 3 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
- DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ 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;
@@ -766,9 +906,9 @@
flg_ButtonOn = false;
}
else if (baseOperation.sv_WinchValid==0) {
- I2C_cmd[6] = MOTOR_STP; // Stop
- I2C_cmd[7] = 0; // Speed=0
- flg_exp_status &= 0xFF3F000F;
+ 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.
}
@@ -776,18 +916,22 @@
if ((btnStatus_RJSFwdRvs<setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)&&(baseOperation.sv_WinchValid==0)){
flg_ButtonOn = true;
led3 = 1;
- I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
- tmpSpeed = (( 128 - btnStatus_RJSFwdRvs ) * 100 / 127 ) * setValue.crawlerCtrl.sv_RFCM_sli_R / 100;
- I2C_cmd[3] = (char)tmpSpeed;
+ 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, 3 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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 );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
flg_exp_status |= 0x00100000;
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
@@ -796,34 +940,38 @@
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[2] = MOTOR_RVS; // Motor1 RVS
- tmpSpeed = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc+1 * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
- I2C_cmd[3] = (char)tmpSpeed;
+ 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, 3 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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 );
- DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_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) {
- pc.printf("***** MOTOR STOP ****\r\n");
- I2C_cmd[2] = MOTOR_STP; // Stop
- I2C_cmd[3] = 0; // Speed=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: Single JoyStick mode
+ /* JoyStick mode 0: Syncronous mode ( Single JoyStick mode )
//
// *** ****
// * X * * LR *
@@ -842,6 +990,14 @@
* +-+-+-+-+-+-+-+-+
*/
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) &&
@@ -851,24 +1007,35 @@
flg_ButtonOn = true;
led3 = 1;
if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
- I2C_cmd[2] = MOTOR_RVS; // Motor1 Reverse
- I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Reverse
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
}
else{
- I2C_cmd[2] = MOTOR_FWD; // Motor1 Fwd
- I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Fwd
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
}
- I2C_cmd[3] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
- I2C_cmd[7] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_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_L3( "Bd0> Single Mode: Dir1 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[2], I2C_cmd[3], I2C_cmd[5], I2C_cmd[7]);
+ 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, 3 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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 );
@@ -884,24 +1051,35 @@
flg_ButtonOn = true;
led3 = 1;
if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
- I2C_cmd[2] = MOTOR_STP; // Motor1 Reverse
- I2C_cmd[6] = MOTOR_STP; // Motor1 Reverse
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Reverse
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor1 Reverse
}
else{
- I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
- I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Rvs
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
}
- I2C_cmd[3] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
- I2C_cmd[7] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
+ 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[2], I2C_cmd[3], I2C_cmd[5], I2C_cmd[7]);
+ 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, 3 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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 );
@@ -917,24 +1095,35 @@
flg_ButtonOn = true;
led3 = 1;
if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
- I2C_cmd[2] = MOTOR_FWD; // Motor1 Rvs
- I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Rvs
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
}
else{
- I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
- I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Rvs
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
}
- I2C_cmd[3] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
- I2C_cmd[7] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
+ 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[2], I2C_cmd[3], I2C_cmd[5], I2C_cmd[7]);
+ 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, 3 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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 );
@@ -950,24 +1139,35 @@
flg_ButtonOn = true;
led3 = 1;
if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
- I2C_cmd[2] = MOTOR_STP; // Motor1 Reverse
- I2C_cmd[6] = MOTOR_STP; // Motor1 Reverse
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Reverse
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor1 Reverse
}
else{
- I2C_cmd[2] = MOTOR_FWD; // Motor1 Fwd
- I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Fwd
+ I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
}
- I2C_cmd[3] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
- I2C_cmd[7] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
+ 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[2], I2C_cmd[3], I2C_cmd[5], I2C_cmd[7]);
+ 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, 3 );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
+ 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 );
@@ -980,20 +1180,19 @@
else if (baseOperation.sv_WinchValid==0) {
led3 = 0;
#ifdef __IIC_COMAMND_SEND__
- pc.printf("***** MOTOR STOP ****\r\n");
- I2C_cmd[2] = MOTOR_STP; // Motor1 Fwd
- I2C_cmd[3] = 0; // Speed=0
- I2C_cmd[6] = MOTOR_STP; // Motor2 Rvs
- I2C_cmd[7] = 0; // Speed=0
+ 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[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- // I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- // I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- // I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ // I2C_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;
}
}
@@ -1011,16 +1210,21 @@
DEBUG_PRINT_L3( "Bd0> BTN W-DN(Fwd)\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
- I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
- I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_F; // Speed
- // I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
- // I2C_cmd[7] = setValue.winchCtrl.sv_WRM_sli_F; // Speed
+ 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[4] = setValue.winchCtrl.sv_WDM_ith_F;
- I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
- I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
- I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
+
+ I2C_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;
}
@@ -1029,16 +1233,21 @@
DEBUG_PRINT_L3( "Bd0> BTN W-UP(Rvs)\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
- I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
- I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_R; // Speed
- // I2C_cmd[6] = MOTOR_RVS; // Motor2 RVS
- // I2C_cmd[7] = setValue.winchCtrl.sv_WRM_sli_R; // Speed
+ 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[4] = setValue.winchCtrl.sv_WDM_ith_F;
- I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
- I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
- I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
+
+ I2C_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;
}
@@ -1048,17 +1257,22 @@
else if (baseOperation.sv_WinchValid == 1){
led3 = 0;
#ifdef __IIC_COMAMND_SEND__
- I2C_cmd[2] = MOTOR_STP;
- I2C_cmd[3] = 0;
- I2C_cmd[6] = MOTOR_STP;
- I2C_cmd[7] = 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);
#endif
flg_exp_status &= 0xF0FFFFFF;
- I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
- I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
- I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
- I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
+
+ I2C_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.
}
@@ -1083,6 +1297,7 @@
// TASK: Hoat Interface Task
//
// **************************************************************
+int first_counter = 0;
void clientPC_interface_task(void const *)
{
int rcv_data_cnt;
@@ -1120,6 +1335,18 @@
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 );
@@ -1136,7 +1363,14 @@
}
Thread::wait(10);
- sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts); // Winch down
+ 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
// -------------------------------------
@@ -1148,38 +1382,39 @@
if((flg_exp_status & 0x00F00000) == 0x00500000 ){
// 01234 5678 9012 34569
- sprintf( dbuffer, "WCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
- // Thread::wait(10);
+ 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, "WCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
- // Thread::wait(10);
+ 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, "WCRR %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
- // Thread::wait(10);
+ 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, "WCLR %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
- // Thread::wait(10);
+ 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);
+ 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);
+ 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);
+ 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);
+ 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
}
// -------------------------------------
@@ -1188,6 +1423,7 @@
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 ){
@@ -1220,7 +1456,7 @@
}
else if( flg_exp_status & 0x0000000f ){
if( cnt == 3 ){
- sprintf( dbuffer,"JSMD %03d %03d\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid );
+ sprintf( dbuffer,"JSMD %03d %03d 0000 000\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid );
cnt = 0;
}
else{
@@ -1251,9 +1487,9 @@
}
}
- Thread::wait(100); // When this period seto to short time, gamepad command can not get. 50msec OK
+ // 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
+ // Thread::wait(100); // When this period seto to short time, gamepad command can not get. 50msec OK
}
}
@@ -1293,8 +1529,8 @@
// led1 = 1;
// Send status to Handy Ctrl controller, but currently this is only for Main Controller.
- I2C_cmd[4] = 0x00;
- I2C_cmd[5] = 0x01;
+ 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 );
@@ -1304,100 +1540,282 @@
}
// ======================================================================
-// Write setting value from lpcal file system of mbed
+// Read setting value from lpcal file system of mbed
// ======================================================================
-void write_Setting_lfs( void )
-{
- FILE *wfp;
+
+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();
- DEBUG_PRINT_L3("\r\nBd0> Local file system write ... ");
- wfp = fopen("/local/set.dat", "wb"); // Open local file "set.txt" for writing
- if(!wfp){
- DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
+ 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{
- Thread::wait(200);
- fwrite( &setValue, sizeof( int8_t ),sizeof(setValue), wfp );
- Thread::wait(200);
- fclose(wfp);
-
- DEBUG_PRINT_L3("file writen\r\n");
- }
+ pc.printf( "#### ERROR local file open error ####\r\n");
+ rts = false;
+ }
flg_mutex.unlock();
+ return rts;
}
-// ======================================================================
-// Read setting value from lpcal file system of mbed
-// ======================================================================
-int read_Setting_lfs( void )
-{
- FILE *rfp;
- int rts;
-
- //setValue_t lsetValue; // Setting Data
-
- DEBUG_PRINT_L3("\r\nBd0> Read Setting data from local file system \r\n");
- DEBUG_PRINT_L3( "Bd0> ---------------------------------------- \r\n");
- rfp = fopen("/local/set.dat", "rb"); // Open local file "set.txt" for writing
- if(!rfp){
- DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
- rts = _FAIL_;
+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{
- Thread::wait(100);
- DEBUG_PRINT_L3("fread\r\n");
- flg_mutex.lock();
- fread( (int8_t*)&setValue, sizeof(int8_t), sizeof(setValue), rfp );
- DEBUG_PRINT_L3("fread done\r\n");
- dspSetValue2Console(&pc, &setValue);
- flg_mutex.unlock();
-
- // Thread::wait(500);
- DEBUG_PRINT_L3("fclose\r\n");
- fclose(rfp);
- DEBUG_PRINT_L3("\r\n");
+ 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);
- rts = _OK_;
- }
- flg_mutex.unlock();
- return rts;
-}
-
-// ======================================================================
-// Read setting value from lpcal file system of mbed
-// ======================================================================
-int read_Setting2_lfs( void )
-{
- FILE *rfp;
- int rts;
-
-// setValue_t lsetValue; // Setting Data
-
- DEBUG_PRINT_L3("\r\nBd0> Read Setting data from local file system \r\n");
- DEBUG_PRINT_L3( "Bd0> ---------------------------------------- \r\n");
- rfp = fopen("/local/set.dat", "rb"); // Open local file "set.txt" for writing
- if(!rfp){
- DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
- rts = _FAIL_;
+ 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{
- Thread::wait(100);
- DEBUG_PRINT_L3("fread\r\n");
- flg_mutex.lock();
- fread( (int8_t*)&setValue, sizeof(int8_t), sizeof(setValue), rfp );
- DEBUG_PRINT_L3("fread done\r\n");
- dspSetValue2Console(&pc, &setValue);
- flg_mutex.unlock();
-
- // Thread::wait(500);
- DEBUG_PRINT_L3("fclose\r\n");
- fclose(rfp);
- DEBUG_PRINT_L3("\r\n");
+ 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);
- rts = _OK_;
- }
- flg_mutex.unlock();
+ 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;
}
@@ -1407,33 +1825,19 @@
int read_NetSetting_lfs( char* ip_address, char* subnet_mask, char* gateway )
{
FILE *rfp;
-// char ip_address[20];
-// char subnet_mask[20];
-// char gateway[20];
-
- DEBUG_PRINT_L3("\r\nBd0> Read Network Setting data from local file system \r\n");
- rfp = fopen("/local/net.dat", "r"); // Open local file "set.txt" for writing
+
+ 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(500);
-
+ Thread::wait(50);
fscanf(rfp, "%s", ip_address);
fscanf(rfp, "%s", subnet_mask);
fscanf(rfp, "%s", gateway);
-
- DEBUG_PRINT_L3("Bd0> ---------------------------------------\r\n");
- DEBUG_PRINT_L3("Bd0> ip address : %s\r\n", ip_address);
- DEBUG_PRINT_L3("Bd0> subnet mask : %s\r\n", subnet_mask);
- DEBUG_PRINT_L3("Bd0> default gateway: %s\r\n", gateway);
- DEBUG_PRINT_L3("Bd0> ---------------------------------------\r\n");
-
- Thread::wait(500);
fclose(rfp);
- DEBUG_PRINT_L3("\r\n");
-
return _OK_;
}
}
@@ -1470,10 +1874,10 @@
if( flg_ButtonOn == true ) {Thread::wait(2);}
- I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
+ 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;
@@ -1492,7 +1896,7 @@
// }
}
- I2C_cmd[1] = 'Z'; // Zero clear
+ I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Zero clear
i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
led3 = OFF;
@@ -1548,28 +1952,28 @@
}
// Forward rotation : winch down
if( winchDataP->dt_WinchCntPosition < winchDataP->dt_WinchDstPosition ){
- I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
- I2C_cmd[3] = (setValue.winchCtrl.sv_WDM_mst >> 1); // very slow speed
+ 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[3] = setValue.winchCtrl.sv_WDM_mst; // slow speed
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_lsrto_F; // slow speed
}
else{
- I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_F; // normal speed
+ 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[2] = MOTOR_RVS; // Motor1 RVS
+ I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
- I2C_cmd[3] = (setValue.winchCtrl.sv_WDM_mrt >> 1); // very slow speed
+ 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[3] = setValue.winchCtrl.sv_WDM_mrt; // slow speed
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_lsrto_R; // slow speed
}
else{
- I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_R; // normal speed
+ I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_R; // normal speed
}
}
@@ -1595,7 +1999,7 @@
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> 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 ){
@@ -1610,10 +2014,10 @@
DEBUG_PRINT_L3( "Bd0> ! Winch Stop\r\n" );
- I2C_cmd[2] = MOTOR_STP; // Motor1 STOP
- I2C_cmd[3] = 0; // Speed
- I2C_cmd[6] = MOTOR_STP; // Motor2 STOP
- I2C_cmd[7] = 0; // Speed
+ I2C_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
@@ -1735,8 +2139,8 @@
if ( mode == WINCH_U_STEPDOWN_BTN_ON ) man_speed = 50;
else man_speed = 100;
- I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
- I2C_cmd[3] = man_speed; // Speed
+ I2C_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
@@ -1771,10 +2175,10 @@
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[2] = MOTOR_STP; // Motor1 FWD
- I2C_cmd[3] = 0; // Speed
- I2C_cmd[6] = MOTOR_STP; // Motor2 FWD
- I2C_cmd[7] = 0; // Speed
+ 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
@@ -1832,8 +2236,8 @@
if ( mode == WINCH_U_STEPUP_BTN_ON ) man_speed = 50;
else man_speed = 100;
- I2C_cmd[2] = MOTOR_RVS; // Motor1 FWD
- I2C_cmd[3] = man_speed; // Speed
+ I2C_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
@@ -1868,10 +2272,10 @@
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[2] = MOTOR_STP; // Motor1 FWD
- I2C_cmd[3] = 0; // Speed
- I2C_cmd[6] = MOTOR_STP; // Motor2 FWD
- I2C_cmd[7] = 0; // Speed
+ 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
@@ -1912,18 +2316,20 @@
// **********************************************************************
int main()
{
- winchData_t winchData;
+ 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[20];
+ char subnet_mask[20];
+ char gateway[20];
- char* ip_address;
- char* subnet_mask;
- char* gateway;
+// char* ip_address;
+// char* subnet_mask;
+// char* gateway;
int ret;
int try_cnt;
@@ -1936,7 +2342,7 @@
// Set UART(USB) baudrate
pc.baud(115200);
- cf_led_demo( &led1, &led2, &led3, &led4, 15, 35 );
+ cf_led_demo( &led1, &led2, &led3, &led4, 10, 15 );
DEBUG_PRINT_L0("\r\n");
DEBUG_PRINT_L0("Bd0> +--------------------------------------------------------------\r\n");
@@ -1950,17 +2356,17 @@
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> Initalizing Ethernet ...\r\n");
- DEBUG_PRINT_L0("Bd0> ------------------------\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 );
+ read_NetSetting_lfs( ip_address, subnet_mask, gateway );
- ip_address = "192.168.11.24";
- subnet_mask = "255.255.255.0";
- gateway = "192.168.11.1";
+// 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);
@@ -2006,12 +2412,20 @@
// setting file "SET.DAT".
// When error occured, LED1 will be blinking shortly.
//---------------------------------------------------
- DEBUG_PRINT_L0("Bd0> ---------------------------\r\n");
- DEBUG_PRINT_L0("Bd0> Read setting value from LFS\r\n");
- DEBUG_PRINT_L0("Bd0> ---------------------------\r\n");
+ 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_Setting_lfs() == _OK_ ) break;
+ if( read_LFS(&setValue) == true ) break;
else try_cnt -= 1;
if( try_cnt == 0 ){
DEBUG_PRINT_L0("Bd0> ***ERROR*** LFS read error\r\n");
@@ -2021,64 +2435,85 @@
}
}
}
+
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("--------------------------\r\n");
- DEBUG_PRINT_L0("Check the target controler\r\n");
- DEBUG_PRINT_L0("--------------------------\r\n");
+ 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
- // currently only one target checked for debugging ...
- // **********************************************************************
- // **********************************************************************
- I2C_cmd[1] = MOTOR_FWD;
- i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- Thread::wait(100);
i2c.read(I2C_ADDRESS_WINCH, I2C_res, NumberOfI2CCommand);
- DEBUG_PRINT_L0("Bd0> Try count down: %d/%d\r\n", try_cnt, TARGET_CHECK_COUNT );
- DEBUG_PRINT_L0("Bd0> Return from [ i2c address = 0x%2x : %s ]\r\n", I2C_ADDRESS_WINCH, I2C_res);
- DEBUG_PRINT_L0("Bd0> MCTR1[Winch motor controller] found\r\n");
- if( I2C_res[0] == 'S' ){
+ 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##\r\n");
+ DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_WINCH);
led1 = OFF;
while(1){
led1 = !led1; // ON
Thread::wait(80);
- led1 = !led1; // OFF
- Thread::wait(80);
- led1 = !led1; // ON
- Thread::wait(80);
- led1 = !led1; // OFF
- Thread::wait(80);
- led1 = !led1; // ON
- Thread::wait(80);
- led1 = !led1; // OFF
- Thread::wait(80);
- led1 = !led1; // ON
- Thread::wait(80);
- led1 = !led1; // OFF
- Thread::wait(500);
}
}
}
+
+ 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 */
@@ -2086,31 +2521,50 @@
baseOperation.sv_JS_OpeMode = 0;
baseOperation.sv_WinchValid = 0;
+
/*
**************************************************
Send Calculation Data to Resolver Controller
**************************************************
*/
- DEBUG_PRINT_L0("Bd0> Send Calculation Data to Resolver Controller");
- I2C_cmd[2] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100>>8)&0xFF); // Dram diameter upper
- I2C_cmd[3] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100)&0xFF); // Dram diameter lower
- I2C_cmd[4] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100>>8)&0xFF); // Cable diameter upper
- I2C_cmd[5] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100)&0xFF); // Cable diameter lower
- I2C_cmd[6] = setValue.winchCtrl.sv_WRS_RResolution; // Resolver resolution
+ 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");
- DEBUG_PRINT_L0("Bd0> ----------------------------------------\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, 256 * 2);
+ // 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, osPriorityHigh, 256 * 2);
- DEBUG_PRINT_L0("\r\nBd0> ----------------------------------------\r\n");
+ 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");
+ DEBUG_PRINT_L0( "Bd0> ----------------------------------\r\n");
+ DEBUG_PRINT_L0( "Bd0> >>>> Initializing completed ! <<<<\r\n");
+ DEBUG_PRINT_L0( "Bd0> ----------------------------------\r\n");
led4 = OFF;
led3 = OFF;
@@ -2120,7 +2574,7 @@
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);
// -----------------------------------------------------------------
@@ -2262,27 +2716,33 @@
else if( !strcmp( dbuffer, "SetValue" ) ){
DEBUG_PRINT_L3("Bd0> SetValue Request from client\r\n");
- rcv_data_cnt = tcp_client.receive( dbuffer, sizeof(dbuffer));
- DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
- memcpy( &setValue, (setValue_t *)dbuffer, sizeof( setValue)) ;
- // Display setting value list to consol
- dspSetValue2Console( &pc, &setValue );
- // Write setting data to local file system
- // Thread::wait(100);
- write_Setting_lfs();
- // Thread::wait(500);
- // Read setting data from local file system
- if ( read_Setting2_lfs() == _FAIL_ ){
- DEBUG_PRINT_L0("Bd0> ### ERROR### Can't read out setting data from lfs\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 <-- send");
- read_Setting2_lfs();
- // Display setting value list to consol
- dspSetValue2Console( &pc, &setValue );
- tcp_client.send_all( (char*)&setValue, sizeof(setValue));
- DEBUG_PRINT_L2("(%d)\r\n", sizeof(setValue));
+ 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;