z ysaito
2018.07.26
Dependencies: EthernetInterface TextLCD USBDevice USBHost3 mbed
Fork of
USBHostHub_HelloWorld
by 
Samuel Mokrani
Diff: 0_main.cpp
- Revision:
- 13:2c70c772fe24
- Parent:
- 12:3e6b6fcf540b
- Child:
- 14:3a5ae61ab1f4
--- a/0_main.cpp Fri Feb 19 07:02:35 2016 +0000
+++ b/0_main.cpp Mon Mar 28 00:07:19 2016 +0000
@@ -45,19 +45,14 @@
// USBSerial serial setting
Serial pc(USBTX, USBRX); // UART
// Digital I/O setting
-DigitalOut led1(LED1); // 1: on, 0: off
-DigitalOut led2(LED2); // 1: on, 0: off
-DigitalOut led3(LED3); // 1: on, 0: off
-DigitalOut led4(LED4); // 1: on, 0: off
+DigitalOut led1(LED1); // 1:on,0:off System is OK then ON.
+DigitalOut led2(LED2); // 1:on,0:off GamePad is connected.
+DigitalOut led3(LED3); // 1:on,0:off When got the GamePas switch input then ON
+DigitalOut led4(LED4); // 1:on,0:off Access indicator with PC
// I2C setting
-//i2c(p28, p27); // I2C SDA, SCL is not good ???
+I2C i2c_res(p28, p27); // I2C SDA, SCL is not good ???
I2C i2c(p9, p10); // I2C SDA, SCL is good
-// I2C address
-const int32_t i2c_addr_handy = I2C_ADDRESS_HANDY << 1; // Ctrl Board0 : Handy Controller
-const int32_t i2c_addr_winch = I2C_ADDRESS_WINCH << 1; // Ctrl Board1 : Winch
-const int32_t i2c_addr_transform = I2C_ADDRESS_TRANSFORM << 1; // Ctrl Board2 : Transform
-const int32_t i2c_addr_crawler = I2C_ADDRESS_CRAWLER << 1; // Ctrl Board3 : Crawler
-const int32_t i2c_addr_resolver = I2C_ADDRESS_RESOLVER << 1; // Ctrl Board4 : Resolver
+
// Ethernet
EthernetInterface eth;
TCPSocketServer tcp_server; // TCP Server
@@ -70,11 +65,12 @@
LocalFileSystem SettingFile("local"); // Create the local filesystem under the name "local"
// Global
-uint32_t flg_gamePad_Connected = 0;
-char PC_cmd[11+1] = "&0100000000";
-setValue_t setValue; // Setting Data
-char dbuffer[128];
-uint8_t motor_speed = 0;
+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;
/* Status flag */
/*
0000 0000 : button LI LK RI RK PCW PCCW TU TD
@@ -87,6 +83,10 @@
Mutex swbtn_OpeMutex;
int swbtn_Opeflg = 0;
+int16_t winchCurrentPosition;
+
+Mutex mtx_wcp;
+
bool flg_ButtonOn = false;
// Mutex hwbtn_OpeMutex;
@@ -96,6 +96,7 @@
int flg_JS_ope_mode = 0;
uint8_t motor1_current_pct;
uint8_t motor2_current_pct;
+uint8_t limitSw_Sts = 0;
/* Prototype */
@@ -158,15 +159,15 @@
int rts;
rts = i2c.read(i2c_addr, I2C_data, 2); // Read
- // flg_mutex.lock();
+ mtx_wcp.lock();
if( rts == 0 ){
res_position = (I2C_data[1] << 8) | I2C_data[0];
}
else{
- res_position = 0;
+ res_position = -1;
}
// winchCurrentPosition = res_position;
- // flg_mutex.unlock();
+ mtx_wcp.unlock();
return res_position;
}
@@ -229,6 +230,8 @@
uint8_t btnID_RFI = 0;
uint8_t btnID_LBK = 0;
uint8_t btnID_LBI = 0;
+ uint8_t btnID_RFLBI = 0; // RF-I and LB-I both button on same time
+ uint8_t btnID_RFLBK = 0; // RF-K and LB-K both button on same time
uint8_t btnID_WUP = 0;
uint8_t btnID_WDN = 0;
@@ -253,6 +256,10 @@
btnID_CrossDn = 4;
btnID_CrossRt = 2;
btnID_CrossLt = 6;
+ // ---- This is temporary setting ------------------------------
+ btnID_RFLBI = 192; // RF-I and LB-I both button on same time
+ btnID_RFLBK = 48; // RF-K and LB-K both button on same time
+ // --------------------------------------------------------------
btnID_JS_SD = 1; // JS mode Single or Dual
btnID_JD_IK = 2; // JS mode I-Shape KO-Shape
if ( gamePadPID == GAMEPAD_PID_RSTHANDY ){
@@ -281,6 +288,10 @@
btnID_RFI = 128;
btnID_LBK = 16;
btnID_LBI = 64;
+ // ---------------------
+ btnID_RFLBI = 192; // RF-I and LB-I both button on same time
+ btnID_RFLBK = 48; // RF-K and LB-K both button on same time
+ // ---------------------
btnID_Start = 8; // Guide button ID for ELECOM GamePad
btnID_CrossUp = 0;
btnID_CrossDn = 4;
@@ -370,8 +381,10 @@
// DEBUG_PRINT_BTN(" Btn 00:%d, 01:%d, 02:%d, 03:%d, 04:%d, 05:%d, 06:%d, 07:%d, 08:%d | VID=0x%04x, PID=0x%04x\r\n",
// btn00,btn01,btn02,btn03,btn04,btn05,btn06,btn07,btn08,
// gamePadVID, gamePadPID);
- DEBUG_PRINT_L4(" Btn 00:%d, 01:%d, 02:%d, 03:%d, 04:%d, 05:%d, 06:%d, 07:%d, 08:%d\r\n",
- btn00,btn01,btn02,btn03,btn04,btn05,btn06,btn07,btn08);
+ 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");
#endif
I2C_cmd[4] = '\0';
@@ -391,34 +404,34 @@
flg_exp_status &= 0xFFFFFFF0;
if(!( btnStatus_Start & 0x01 )){ // I-Shape
flg_mutex.lock();
- setValue.operation.sv_JS_ShapeMode = 0;
- setValue.operation.sv_JS_OpeMode = 0;
+ baseOperation.sv_JS_ShapeMode = 0;
+ baseOperation.sv_WinchValid = 0;
flg_mutex.unlock();
flg_exp_status |= 0x00000001;
}
else{ // KO-Shape
flg_mutex.lock();
- setValue.operation.sv_JS_ShapeMode = 1;
+ baseOperation.sv_JS_ShapeMode = 1;
flg_mutex.unlock();
flg_exp_status |= 0x00000002;
}
- if(!(btnStatus_Start & 0x02 )&&( btnStatus_Start & 0x01 )){ // Dual
+ if(!(btnStatus_Start & 0x02 )){ // Tfm,crawler part valid
flg_mutex.lock();
- setValue.operation.sv_JS_OpeMode = 1;
+ baseOperation.sv_WinchValid = 0;
flg_mutex.unlock();
flg_exp_status |= 0x00000004;
}
- else{ // Single
+ else{ // Winch part valid
flg_mutex.lock();
- setValue.operation.sv_JS_OpeMode = 0;
+ baseOperation.sv_WinchValid = 1;
flg_mutex.unlock();
flg_exp_status |= 0x00000008;
}
DEBUG_PRINT_L4( "-----------------------------\r\n" );
DEBUG_PRINT_L4( "%d : %d\r\n",btnStatus_Start, flg_exp_status );
- DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", setValue.operation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " JS operation mode change: %d\r\n", setValue.operation.sv_JS_OpeMode );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
DEBUG_PRINT_L4( "-----------------------------\r\n" );
}
else{
@@ -432,59 +445,261 @@
* |x|x|x|x|x|x|x|o| 1: I-Shape JSmode, 2: K-Shape JSmode, 4: Single JSmode, 8: Dual JSmode
* +-+-+-+-+-+-+-+-+
*/
- if( btnStatus_Start == btnID_Start ){
- if ( btnStatus_CrossUp == btnID_CrossUp ){ // I Shape
- flg_mutex.lock();
- setValue.operation.sv_JS_ShapeMode = 0;
- setValue.operation.sv_JS_OpeMode = 0;
- flg_mutex.unlock();
- DEBUG_PRINT_L4( "--------------------------------\r\n" );
- DEBUG_PRINT_L4( " I\r\n" );
- DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", setValue.operation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " JS operation mode change: %d\r\n", setValue.operation.sv_JS_OpeMode );
- DEBUG_PRINT_L4( "--------------------------------\r\n" );
- flg_exp_status |= 0x00000001;
- }
- else if( btnStatus_CrossDn == btnID_CrossDn ){ // KO Shape
- flg_mutex.lock();
- setValue.operation.sv_JS_ShapeMode = 1;
- flg_mutex.unlock();
- DEBUG_PRINT_L4( "-------------------------\r\n" );
- DEBUG_PRINT_L4( " KO\r\n" );
- DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", setValue.operation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " JS operation mode change: %d\r\n", setValue.operation.sv_JS_OpeMode );
- DEBUG_PRINT_L4( "-------------------------\r\n" );
- flg_exp_status |= 0x00000002;
- }
- else if( btnStatus_CrossRt == btnID_CrossRt ){ // Single JS
- flg_mutex.lock();
- setValue.operation.sv_JS_OpeMode = 0;
- flg_mutex.unlock();
- DEBUG_PRINT_L4( "-----------------------------\r\n" );
- DEBUG_PRINT_L4( " Single\r\n" );
- DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", setValue.operation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " JS operation mode change: %d\r\n", setValue.operation.sv_JS_OpeMode );
- DEBUG_PRINT_L4( "-----------------------------\r\n" );
- flg_exp_status |= 0x00000004;
- }
- else if( btnStatus_CrossLt == btnID_CrossLt ){ // Dual JS
- if( setValue.operation.sv_JS_ShapeMode == 1 ){ // KO Shape
- flg_mutex.lock();
- setValue.operation.sv_JS_OpeMode = 1;
- flg_mutex.unlock();
- DEBUG_PRINT_L4( "-----------------------------\r\n" );
- DEBUG_PRINT_L4( " Dual\r\n" );
- DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", setValue.operation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " JS operation mode change: %d\r\n", setValue.operation.sv_JS_OpeMode );
- DEBUG_PRINT_L4( "-----------------------------\r\n" );
- flg_exp_status |= 0x00000008;
- }
- }
+ if ( btnStatus_CrossUp == btnID_CrossUp ){ // I Shape
+ flg_mutex.lock();
+ baseOperation.sv_JS_ShapeMode = 0;
+ flg_mutex.unlock();
+ DEBUG_PRINT_L4( "--------------------------------\r\n" );
+ DEBUG_PRINT_L4( " I\r\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
+ DEBUG_PRINT_L4( "--------------------------------\r\n" );
+ flg_exp_status |= 0x00000001;
}
+ else if( btnStatus_CrossDn == btnID_CrossDn ){ // KO Shape
+ flg_mutex.lock();
+ baseOperation.sv_JS_ShapeMode = 1;
+ flg_mutex.unlock();
+ DEBUG_PRINT_L4( "-------------------------\r\n" );
+ DEBUG_PRINT_L4( " KO\r\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
+ DEBUG_PRINT_L4( "-------------------------\r\n" );
+ flg_exp_status |= 0x00000002;
+ }
+ else if( btnStatus_CrossRt == btnID_CrossLt ){ // Valid Part : Crawler (Left)
+ flg_mutex.lock();
+ baseOperation.sv_WinchValid = 0;
+ flg_mutex.unlock();
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ DEBUG_PRINT_L4( " Tfm, Crawler\r\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ flg_exp_status |= 0x00000004;
+ }
+ else if( btnStatus_CrossLt == btnID_CrossRt ){ // Valid Part : Winch (Right)
+ flg_mutex.lock();
+ baseOperation.sv_WinchValid = 1;
+ flg_mutex.unlock();
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ DEBUG_PRINT_L4( " Winch\r\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ flg_exp_status |= 0x00000008;
+ }
else{
flg_exp_status &= 0xFFFFFFF0;
- }
+ }
+ }
+ /*
+ // ====================================
+ // TRANSFORM Motor Control
+ // ====================================
+ * 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+
+ * |o|x|x|x|x|x|x|x| 1: RF-I, 2: RF-K, 4: LB-I, 8: LB-K
+ * +-+-+-+-+-+-+-+-+
+ */
+
+ if ((btnStatus_RFK==btnID_RFLBK)&&(baseOperation.sv_WinchValid==0)){ // Both sw on
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> BTN RF-K & LB-K\r\n" );
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[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
+ #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.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];
+ 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 );
+ #endif // __IIC_COMAMND_SEND__
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+
+ else if ((btnStatus_RFI== btnID_RFLBI)&&(baseOperation.sv_WinchValid==0)){ // Both sw on
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> BTN RF-I & LB-I\r\n" );
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[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
+ #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.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];
+ 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 );
+ #endif // __IIC_COMAMND_SEND__
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
}
+
+ else if ((btnStatus_RFK==btnID_RFK)&&(baseOperation.sv_WinchValid==0)){ // RF KO
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> BTN RF-K\r\n" );
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
+ I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_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.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];
+ 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 );
+ #endif // __IIC_COMAMND_SEND__
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+ else if ((btnStatus_RFI==btnID_RFI)&&(baseOperation.sv_WinchValid==0)){ // RF I
+ DEBUG_PRINT_L3( "Bd0> BTN RF-I\r\n" );
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
+ I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_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.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];
+ 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 );
+ #endif // __READ_TFM_MOTOR_CURRENT__
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+
+ else if ((btnStatus_LBK==btnID_LBK)&&(baseOperation.sv_WinchValid==0)){ // LB KO
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> BTN LB-K\r\n" );
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
+ I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_F; // Speed
+ #endif
+ flg_exp_status |= 0x40000000;
+
+ I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
+ I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
+ I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
+ I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ i2c.write(I2C_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];
+ 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 );
+ #endif // __READ_TFM_MOTOR_CURRENT__
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+ else if ((btnStatus_LBI==btnID_LBI)&&(baseOperation.sv_WinchValid==0)) { // LB I
+ flg_ButtonOn = true;
+ DEBUG_PRINT_L3( "Bd0> BTN LB-I\r\n" );
+ led3 = 1;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[6] = MOTOR_RVS; // Motor1 RVS
+ I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_R; // Speed
+ #endif
+ flg_exp_status |= 0x80000000;
+
+ I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
+ I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
+ I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
+ I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ i2c.write(I2C_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];
+ 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 );
+ #endif // __READ_TFM_MOTOR_CURRENT__
+ i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
+ }
+ // ====================================
+ // ALL motor off commmand packet send
+ // -- This part isn't operated in case of general game controller, because no event. --
+ // ====================================
+ else if (baseOperation.sv_WinchValid==0){
+ led3 = 0;
+ #ifdef __IIC_COMAMND_SEND__
+ I2C_cmd[2] = MOTOR_STP;
+ I2C_cmd[3] = 0;
+ I2C_cmd[6] = MOTOR_STP;
+ I2C_cmd[7] = 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.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];
+ 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 );
+ #endif // __READ_TFM_MOTOR_CURRENT__
+
+ }
+
/*/ ====================================
// Crawler Moving Control
// ====================================
@@ -506,95 +721,105 @@
* |x|x|o|x|x|x|x|x| 1: R Fwd, 2: R Rvs, 4: L Fwd, 8: L Rvs
* +-+-+-+-+-+-+-+-+
*/
- if( setValue.operation.sv_JS_OpeMode == 1 ){
+ if( 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;
- if( btnStatus_LJSFwdRvs < setValue.crawlerCtrl.sv_LBCM_dzc - setValue.crawlerCtrl.sv_LBCM_dzl ){
+ 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.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);
- i2c.write(0x40<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
-
- // read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // motor2_current_pct = I2C_res[1];
- // DEBUG_PRINT_L2( "Bd0> 1: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- // DEBUG_PRINT_L2( "Bd0> 1: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
-
+ // 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 );
+ DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+
flg_exp_status |= 0x00400000;
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
- else if( btnStatus_LJSFwdRvs > setValue.crawlerCtrl.sv_LBCM_dzc + setValue.crawlerCtrl.sv_LBCM_dzu ){
+ 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.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);
- i2c.write(0x40<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // read_motorCurrent( i2c_addr_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 );
- // DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ // 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 );
+ DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
flg_exp_status |= 0x00800000;
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
- else{
+ else if (baseOperation.sv_WinchValid==0) {
I2C_cmd[6] = MOTOR_STP; // Stop
I2C_cmd[7] = 0; // Speed=0
flg_exp_status &= 0xFF3F000F;
- i2c.write(0x40<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
}
- if( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl ){
+ 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;
DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Fwd (Speed=%d)\r\n", tmpSpeed);
- i2c.write(0x40<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // 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 );
+ // 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> 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.
flg_ButtonOn = false;
}
- else if( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu ){
+ 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;
DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Rvs (Speed=%d)\r\n", tmpSpeed);
- i2c.write(0x40<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // 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 );
+ // 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> 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{
+ else if (baseOperation.sv_WinchValid==0) {
+ pc.printf("***** MOTOR STOP ****\r\n");
I2C_cmd[2] = MOTOR_STP; // Stop
I2C_cmd[3] = 0; // Speed=0
flg_exp_status &= 0xFFCFFFFF;
- i2c.write(0x40<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ 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;
}
@@ -617,37 +842,48 @@
* +-+-+-+-+-+-+-+-+
*/
else{ // Single JoyStick mode
- if(( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl ) && ( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu) && ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)){
+ if(
+ ( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl ) &&
+ ( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50) &&
+ ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50 ) &&
+ ( baseOperation.sv_WinchValid == 0)
+ ){
flg_ButtonOn = true;
led3 = 1;
- if( setValue.operation.sv_JS_ShapeMode == 0 ){ // I-Shape
+ if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
I2C_cmd[2] = MOTOR_RVS; // Motor1 Reverse
+ I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
}
else{
I2C_cmd[2] = MOTOR_FWD; // Motor1 Fwd
+ I2C_cmd[6] = 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[6] = MOTOR_FWD; // Motor2 Fwd
I2C_cmd[7] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
- DEBUG_PRINT_L3( "Bd0> Single Mode: Dir1 (Speed: %d, %d)\r\n", I2C_cmd[5], I2C_cmd[7]);
- flg_exp_status |= 0x00100000;
- flg_exp_status |= 0x00400000; // 0x00500000
- I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- i2c.write(0x40<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // motor2_current_pct = I2C_res[1];
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ 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]);
+ 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];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
- else if(( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu ) && ( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu) && ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)){
+ else if(
+ ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu ) &&
+ ( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50 ) &&
+ ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
+ ( baseOperation.sv_WinchValid == 0)
+ ){
flg_ButtonOn = true;
led3 = 1;
- if( setValue.operation.sv_JS_ShapeMode == 0 ){ // I-Shape
+ if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
I2C_cmd[2] = MOTOR_STP; // Motor1 Reverse
I2C_cmd[6] = MOTOR_STP; // Motor1 Reverse
}
@@ -657,52 +893,63 @@
}
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
- DEBUG_PRINT_L3( "Bd0> Single Mode: Dir2 (Speed: %d, %d)\r\n", I2C_cmd[5], I2C_cmd[7]);
- flg_exp_status |= 0x00200000;
- flg_exp_status |= 0x00400000; // 0x00600000
- I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- i2c.write(0x40<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // motor2_current_pct = I2C_res[1];
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ 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]);
+ 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];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
- else if(( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu ) && ( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu) && ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)){
+ else if(
+ ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu ) &&
+ ( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50) &&
+ ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
+ ( baseOperation.sv_WinchValid == 0)
+ ){
flg_ButtonOn = true;
led3 = 1;
- if( setValue.operation.sv_JS_ShapeMode == 0 ){ // I-Shape
+ if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
I2C_cmd[2] = MOTOR_FWD; // Motor1 Rvs
+ I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
}
else{
I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
+ I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
}
- I2C_cmd[6] = 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
- DEBUG_PRINT_L3( "Bd0> Single Mode: Dir3 (Speed: %d, %d)\r\n", I2C_cmd[5], I2C_cmd[7]);
- flg_exp_status |= 0x00200000;
- flg_exp_status |= 0x00800000; // 0x00A00000
- I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- i2c.write(0x40<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // motor2_current_pct = I2C_res[1];
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ 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]);
+ 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];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
- else if(( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzu ) && ( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu) && ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)){
+ else if(
+ ( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzu ) &&
+ ( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50) &&
+ ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
+ ( baseOperation.sv_WinchValid == 0)
+ ){
flg_ButtonOn = true;
led3 = 1;
- if( setValue.operation.sv_JS_ShapeMode == 0 ){ // I-Shape
+ if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
I2C_cmd[2] = MOTOR_STP; // Motor1 Reverse
I2C_cmd[6] = MOTOR_STP; // Motor1 Reverse
}
@@ -712,42 +959,44 @@
}
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
- DEBUG_PRINT_L3( "Bd0> Single Mode: Dir4 (Speed: %d, %d)\r\n", I2C_cmd[5], I2C_cmd[7]);
- flg_exp_status |= 0x00100000;
- flg_exp_status |= 0x00800000; // 0x00900000
- I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- i2c.write(0x40<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // motor2_current_pct = I2C_res[1];
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ 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]);
+ 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];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+
+ i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
// ====================================
// ALL motor off commmand packet send
// ====================================
- else {
+ 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
- Thread::wait(5);
+ 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.write(0x40<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
-
+ // I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ // I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ // I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ // I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
}
}
+
/*
* ====================================
* Winch Motor Control
@@ -757,7 +1006,7 @@
* |x|o|x|x|x|x|x|x| 1: W Down, 2: W Up, 4: -, 8: -
* +-+-+-+-+-+-+-+-+
*/
- if ( btnStatus_WDN == btnID_WDN ) { // Winch Down (FWD)
+ if ((btnStatus_WDN == btnID_WDN) && (baseOperation.sv_WinchValid == 1)){ // Winch Down (FWD)
flg_ButtonOn = true;
DEBUG_PRINT_L3( "Bd0> BTN W-DN(Fwd)\r\n" );
led3 = 1;
@@ -767,28 +1016,15 @@
// I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
// I2C_cmd[7] = setValue.winchCtrl.sv_WRM_sli_F; // Speed
#endif
- //// Are these part necessary ?? ////
- //// = ReadWinchCurrentPosition(i2c_addr_resolver);
- // ReadCurrentPositon(i2c_addr_resolver, &winchData, sizeof(winchData), 0x00 );
- // winchCurrentPosition = winchData.dt_WinchCntPosition;
- ////DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
-
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.write(0x70<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
-
- // read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // motor2_current_pct = I2C_res[1];
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
-
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
- else if ( btnStatus_WUP == btnID_WUP ) { // Winch Up (Rvs)
+ else if (( btnStatus_WUP == btnID_WUP ) && (baseOperation.sv_WinchValid == 1)) { // Winch Up (Rvs)
flg_ButtonOn = true;
DEBUG_PRINT_L3( "Bd0> BTN W-UP(Rvs)\r\n" );
led3 = 1;
@@ -798,32 +1034,18 @@
// I2C_cmd[6] = MOTOR_RVS; // Motor2 RVS
// I2C_cmd[7] = setValue.winchCtrl.sv_WRM_sli_R; // Speed
#endif
- //// Are these part necessary ?? ////
- ////winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
- // ReadCurrentPositon(i2c_addr_resolver, &winchData, sizeof(winchData), 0x00 );
- // winchCurrentPosition = winchData.dt_WinchCntPosition;
- ////DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
-
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.write(0x70<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
-
- // read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // motor2_current_pct = I2C_res[1];
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- i2c.write(0x70<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
-
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
// ====================================
// ALL motor off commmand packet send
// ====================================
- else {
+ else if (baseOperation.sv_WinchValid == 1){
led3 = 0;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[2] = MOTOR_STP;
@@ -837,134 +1059,8 @@
I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
- i2c.write(0x70<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(0x70<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- }
- //// Are these part necessary ?? ////
- ////winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
- //ReadCurrentPositon(i2c_addr_resolver, &winchData, sizeof(winchData), 0x00 );
- //winchCurrentPosition = winchData.dt_WinchCntPosition;
- ////DEBUG_PRINT_L3("Bd0> Winch Current Position (button) : %04d\r\n", winchCurrentPosition );
-
- /*
- // ====================================
- // TRANSFORM Motor Control
- // ====================================
- * 7 6 5 4 3 2 1 0
- * +-+-+-+-+-+-+-+-+
- * |o|x|x|x|x|x|x|x| 1: RF-I, 2: RF-K, 4: LB-I, 8: LB-K
- * +-+-+-+-+-+-+-+-+
- */
-
- if ( btnStatus_RFK == btnID_RFK ) { // RF KO
- flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> 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
- #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.write(0x30<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // read_motorCurrent( i2c_addr_transform, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // motor2_current_pct = I2C_res[1];
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- i2c.write(0x30<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
- else if ( btnStatus_RFI == btnID_RFI ) { // RF I
- 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
- #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.write(0x30<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // read_motorCurrent( i2c_addr_transform, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // motor2_current_pct = I2C_res[1];
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- i2c.write(0x30<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
- else if ( btnStatus_LBK == btnID_LBK ) { // LB KO
- flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> 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
- #endif
- flg_exp_status |= 0x40000000;
-
- I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
- I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
- I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
- I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
- i2c.write(0x30<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // read_motorCurrent( i2c_addr_transform, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // motor2_current_pct = I2C_res[1];
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- i2c.write(0x30<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
- else if ( btnStatus_LBI == btnID_LBI ) { // LB I
- flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> BTN LB-I\r\n" );
- led3 = 1;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[6] = MOTOR_RVS; // Motor1 RVS
- I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_R; // Speed
- #endif
- flg_exp_status |= 0x80000000;
-
- I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
- I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
- I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
- I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
- i2c.write(0x30<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // read_motorCurrent( i2c_addr_transform, I2C_readcmd, I2C_res, 3 );
- // motor1_current_pct = I2C_res[0];
- // motor2_current_pct = I2C_res[1];
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
- // DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
- i2c.write(0x30<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
- }
- // ====================================
- // ALL motor off commmand packet send
- // ====================================
- else {
- led3 = 0;
- #ifdef __IIC_COMAMND_SEND__
- I2C_cmd[2] = MOTOR_STP;
- I2C_cmd[3] = 0;
- I2C_cmd[6] = MOTOR_STP;
- I2C_cmd[7] = 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.write(0x30<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(0x30<<1, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
-
+ 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.
}
}
}
@@ -992,10 +1088,11 @@
int rcv_data_cnt;
//winchData_t winchData;
- char I2C_cmd[NumberOfI2CCommand+1] = "#010000";
+ char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
// winchData_t winchData;
- int16_t winchCurrentPosition;
+// int16_t winchCurrentPosition;
+ int16_t winchTempPosition;
int cnt = 0;
@@ -1029,10 +1126,17 @@
/* ***************************************** */
/* Read Winch Current Position from Resolver */
/* ***************************************** */
- winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
+ if (baseOperation.sv_WinchValid == 1){ // read winch current position operation is valid in case of winch part is valid.
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ }
Thread::wait(10);
- sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
+ sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts); // Winch down
// -------------------------------------
// Crawler Moving
// -------------------------------------
@@ -1044,61 +1148,60 @@
if((flg_exp_status & 0x00F00000) == 0x00500000 ){
// 01234 5678 9012 34569
- sprintf( dbuffer, "WCFW %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
+ sprintf( dbuffer, "WCFW %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\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
+ sprintf( dbuffer, "WCRV %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\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
+ sprintf( dbuffer, "WCRR %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\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
+ sprintf( dbuffer, "WCLR %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\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
+ sprintf( dbuffer, "LCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
// Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00400000 ){
- sprintf( dbuffer, "LCFW %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
+ sprintf( dbuffer, "LCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
// Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00200000 ){
- sprintf( dbuffer, "RCRV %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
+ sprintf( dbuffer, "RCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
// Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00100000 ){
- sprintf( dbuffer, "RCFW %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
+ sprintf( dbuffer, "RCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
// Thread::wait(10);
}
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
// -------------------------------------
-
// Transform
// -------------------------------------
else if( flg_exp_status & 0x20000000 ){
- sprintf(dbuffer,"RF2I %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // RF Crawler Tfm I
+ 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);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x10000000 ){
- sprintf(dbuffer,"RF2K %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // RF Crawler Tfm K
+ sprintf(dbuffer,"RF2K %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // RF Crawler Tfm K
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x80000000 ){
- sprintf(dbuffer,"LB2I %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // LB Crawler Tfm I
+ sprintf(dbuffer,"LB2I %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // LB Crawler Tfm I
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x40000000 ){
- sprintf(dbuffer,"LB2K %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // LB Crawler Tfm K
+ sprintf(dbuffer,"LB2K %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // LB Crawler Tfm K
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
@@ -1106,22 +1209,22 @@
// Winch Moving
// -------------------------------------
else if( flg_exp_status & 0x01000000 ){ // Wincd down (FWD)
- sprintf(dbuffer,"WCDN %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
+ sprintf(dbuffer,"WCDN %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x02000000 ){ // Winch up (RVS)
- sprintf(dbuffer,"WCUP %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
+ sprintf(dbuffer,"WCUP %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x0000000f ){
- if( cnt == 5 ){
- sprintf( dbuffer,"JSMD %03d %03d %04d\0", setValue.operation.sv_JS_ShapeMode, setValue.operation.sv_JS_OpeMode );
+ if( cnt == 3 ){
+ sprintf( dbuffer,"JSMD %03d %03d\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid );
cnt = 0;
}
else{
- sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
+ sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
}
cnt++;
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
@@ -1134,6 +1237,18 @@
/* Send data to client PC */
udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
Thread::wait(10);
+
+ // ----------------------------------------------------------------------------
+ // Read target(transform controller) status in each time.
+ read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 4 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ limitSw_Sts = I2C_res[3];
+ // DEBUG_PRINT_L2( "Bd0> X: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ // DEBUG_PRINT_L2( "Bd0> X: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ // DEBUG_PRINT_L2( "Bd0> X: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
+ // ----------------------------------------------------------------------------
+
}
}
Thread::wait(100); // When this period seto to short time, gamepad command can not get. 50msec OK
@@ -1180,8 +1295,8 @@
// Send status to Handy Ctrl controller, but currently this is only for Main Controller.
I2C_cmd[4] = 0x00;
I2C_cmd[5] = 0x01;
- i2c.write(i2c_addr_handy, I2C_cmd, NumberOfI2CCommand);
- // DEBUG_PRINT_L2("Bd0> Send Handy Controller(%02x) >>>>>>>>>>\r\n",i2c_addr_handy );
+ i2c.write(I2C_ADDRESS_HANDY, I2C_cmd, NumberOfI2CCommand);
+ // DEBUG_PRINT_L2("Bd0> Send Handy Controller(%02x) >>>>>>>>>>\r\n",I2C_ADDRESS_HANDY );
Thread::wait(500);
}
@@ -1336,10 +1451,13 @@
int rcv_data_cnt;
// int moving_data;
int man_speed;
-
+
+ int cnt;
+
+
bool flg_stop_operation = false;
- uint16_t winchCurrentPosition;
+ int16_t winchTempPosition;
char I2C_read[NumberOfI2CCommand+1];
char I2C_readcmd[NumberOfI2CCommand+1];
@@ -1375,7 +1493,7 @@
}
I2C_cmd[1] = 'Z'; // Zero clear
- i2c.write(i2c_addr_resolver, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
led3 = OFF;
}
@@ -1398,6 +1516,8 @@
swbtn_Opeflg = 1;
swbtn_OpeMutex.unlock();
+ cnt = 0;
+
while( true ){
while( true ){
led3 = ON;
@@ -1426,81 +1546,154 @@
break;
}
}
+ // Forward rotation : winch down
if( winchDataP->dt_WinchCntPosition < winchDataP->dt_WinchDstPosition ){
I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
- if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
- I2C_cmd[3] = setValue.winchCtrl.sv_WDM_mst; // Speed
+ if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
+ I2C_cmd[3] = (setValue.winchCtrl.sv_WDM_mst >> 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
}
else{
- I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_F; // Speed
+ I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_F; // normal speed
}
}
- else{
+ // Reverse rotation : winch up
+ else if ( winchDataP->dt_WinchCntPosition > winchDataP->dt_WinchDstPosition ){
I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
- if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
- I2C_cmd[3] = setValue.winchCtrl.sv_WDM_mrt;; // Speed
+ if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
+ I2C_cmd[3] = (setValue.winchCtrl.sv_WDM_mrt >> 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
}
else{
- I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_R; // Speed
+ I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_R; // normal speed
}
}
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
// Read winch current position from Resolver.
- winchCurrentPosition = ReadWinchCurrentPosition( i2c_addr_resolver );
+
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ mtx_wcp.unlock();
winchDataP->operation = 0x00;
- //i2c.read(i2c_addr_resolver, I2C_resdata, 2); // Read
+ //i2c.read(I2C_ADDRESS_RESOLVER, I2C_resdata, 2); // Read
//res_position = (I2C_resdata[1] << 8) | I2C_resdata[0];
// --------------------------------------
// Read motor current
// --------------------------------------
- read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_read, 3 );
+ read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
winchDataP->dt_WinchMotor1Current = I2C_read[0]; // Motor current set
winchDataP->dt_WinchMotor2Current = I2C_read[1]; // Motor current set
winchDataP->operation = I2C_read[2];
DEBUG_PRINT_L3( "Bd0> 15: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
- DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", i2c_addr_winch, I2C_read[0]);
+ DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
led3 = OFF;
- if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) <= 0 ){
+ if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) == 0 ){
+ // if( winchDataP->dt_WinchCntPosition == winchDataP->dt_WinchDstPosition ){
+ DEBUG_PRINT_L3( "Bd0> Current:%d -> Destination:%d\r\n" , winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition);
break;
}
Thread::wait(2); // Time interval for program debugging
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
}
+
+
+
+ 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.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- Thread::wait(500); // Time interval for program debugging
+ // Thread::wait(1000); // Time interval for program debugging
- winchCurrentPosition = ReadWinchCurrentPosition( i2c_addr_resolver );
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+
+
+ mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ mtx_wcp.unlock();
winchDataP->operation = 0x00;
- //winchCurrentPosition = res_position; // Set curretn winch position that send for Host PC here when auto move mode.
DEBUG_PRINT_L3("Bd0> Check destination is same to setting point or not: %d\r\n", winchCurrentPosition);
- if( winchDataP->dt_WinchDstPosition == winchDataP->dt_WinchCntPosition ){
+ if( winchDataP->dt_WinchDstPosition == winchCurrentPosition ){
+ cnt++;
DEBUG_PRINT_L3("Bd0> Destination is same to setting point, then stop operation\r\n" );
- break; // When final destination == set point , then break. else adjust position again.
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ if ( cnt >= 5 ){
+ break; // When final destination == set point , then break. else adjust position again.
+ }
}
+ // Force Stop by Stop button
if( flg_stop_operation == true ){
DEBUG_PRINT_L3("Bd0> Winch auto operation force stop\r\n" );
flg_stop_operation = false;
break;
}
}
-
- // Thread::wait(500); // Time interval for program debugging
-
- winchCurrentPosition = ReadWinchCurrentPosition( i2c_addr_resolver );
+/*
+ Thread::wait(300); // Time interval for program debugging
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ mtx_wcp.unlock();
+ winchDataP->operation = 0x00;
+ DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
+ tcp_client.send( (char*)winchDataP, winchData_s);
+
+ Thread::wait(300); // Time interval for program debugging
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ mtx_wcp.unlock();
+ winchDataP->operation = 0x00;
+ DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
+ tcp_client.send( (char*)winchDataP, winchData_s);
+*/
+ Thread::wait(300); // Time interval for program debugging
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
+ mtx_wcp.unlock();
winchDataP->operation = 0x77;
- DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchCurrentPosition);
-
+ DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
tcp_client.send( (char*)winchDataP, winchData_s);
swbtn_OpeMutex.lock();
@@ -1545,11 +1738,11 @@
I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
I2C_cmd[3] = man_speed; // Speed
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
led3 = OFF;
- read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_read, 3 );
+ read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
winchDataP->dt_WinchMotor1Current = I2C_read[0];
winchDataP->dt_WinchMotor2Current = I2C_read[1];
winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
@@ -1557,17 +1750,25 @@
if( winchDataP->operation == 0x88 ){
winchDataP->dt_WinchMotor1Current = 0xFF;
}
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", i2c_addr_winch, I2C_read[0]);
- winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
+ DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
+
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition;
+ mtx_wcp.unlock();
winchDataP->operation = 0x00;
- // ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x00 );
+ // ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x00 );
tcp_client.send( (char*)winchDataP, winchData_s);
DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
// Thread::wait(2); // Time interval for program debugging
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ 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
@@ -1575,15 +1776,23 @@
I2C_cmd[6] = MOTOR_STP; // Motor2 FWD
I2C_cmd[7] = 0; // Speed
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
- winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
+
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition;
+ mtx_wcp.unlock();
winchDataP->operation = 0x77;
- //ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x77 );
+ //ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x77 );
tcp_client.send( (char*)winchDataP, winchData_s);
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
swbtn_OpeMutex.lock();
swbtn_Opeflg = 0;
@@ -1626,11 +1835,11 @@
I2C_cmd[2] = MOTOR_RVS; // Motor1 FWD
I2C_cmd[3] = man_speed; // Speed
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
led3 = OFF;
- read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_read, 3 );
+ read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
winchDataP->dt_WinchMotor1Current = I2C_read[0];
winchDataP->dt_WinchMotor2Current = I2C_read[1];
winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
@@ -1638,17 +1847,25 @@
if( winchDataP->operation == 0x88 ){
winchDataP->dt_WinchMotor1Current = 0xFF;
}
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", i2c_addr_winch, I2C_read[0]);
- winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
+ DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
+
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition;
+ mtx_wcp.unlock();
winchDataP->operation = 0x00;
- // ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x00 );
+ // ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x00 );
tcp_client.send( (char*)winchDataP, winchData_s);
DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
// Thread::wait(2); // Time interval for program debugging
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ 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
@@ -1656,15 +1873,24 @@
I2C_cmd[6] = MOTOR_STP; // Motor2 FWD
I2C_cmd[7] = 0; // Speed
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
- winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
- winchDataP->dt_WinchCntPosition = winchCurrentPosition;
- winchDataP->operation = 0x77;
- // ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x77 );
- tcp_client.send( (char*)winchDataP, winchData_s);
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ if( flg_ButtonOn == false ){
+ winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
+ if( winchTempPosition != -1 ){
+ mtx_wcp.lock();
+ winchCurrentPosition = winchTempPosition;
+ mtx_wcp.unlock();
+ }
+ mtx_wcp.lock();
+ winchDataP->dt_WinchCntPosition = winchCurrentPosition;
+ mtx_wcp.unlock();
+ winchDataP->operation = 0x77;
+ // ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x77 );
+ tcp_client.send( (char*)winchDataP, winchData_s);
+ }
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
swbtn_OpeMutex.lock();
swbtn_Opeflg = 0;
@@ -1689,11 +1915,15 @@
winchData_t winchData;
char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
- char I2C_res[NumberOfI2CCommand+1] = "\0";
+// 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;
int ret;
int try_cnt;
@@ -1702,7 +1932,7 @@
bool flg_ethernet = false;
char ttt[20];
-
+
// Set UART(USB) baudrate
pc.baud(115200);
@@ -1726,7 +1956,11 @@
DEBUG_PRINT_L0("Bd0> 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";
DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
DEBUG_PRINT_L0("Bd0> ip address : %s\r\n", ip_address);
@@ -1754,6 +1988,7 @@
tcp_server.bind(TCP_SERVER_PORT);
tcp_server.listen();
flg_ethernet = true;
+ led4 = ON; // Ethernet OK
}
else{
cf_led_error( &led1,&led2,&led3,&led4 );
@@ -1764,7 +1999,7 @@
DEBUG_PRINT_L0("Bd0> *** ERROR*** Eternat init Fail\r\n");
}
- Thread::wait(500);
+ Thread::wait(50);
//---------------------------------------------------
// Read CrExp setting value from Local File System
@@ -1787,6 +2022,7 @@
}
}
DEBUG_PRINT_L0("Bd0> LFS read OK\r\n");
+ led3 = ON; // Setting Data Read OK
//---------------------------------------------------
// Checking Targer LCXpresso824-MAX board here.
@@ -1808,11 +2044,11 @@
// **********************************************************************
// **********************************************************************
I2C_cmd[1] = MOTOR_FWD;
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
Thread::wait(100);
- i2c.read(i2c_addr_winch, I2C_res, NumberOfI2CCommand);
+ 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_addr_winch>>1, I2C_res);
+ 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' ){
break;
@@ -1843,6 +2079,12 @@
}
DEBUG_PRINT_L0("Bd0> Target system found\r\n");
*/
+ led2 = ON; // Check target OK
+
+ /* Set basic function default setting */
+ baseOperation.sv_JS_OpeMode = 0;
+ baseOperation.sv_JS_OpeMode = 0;
+ baseOperation.sv_WinchValid = 0;
/*
**************************************************
@@ -1855,7 +2097,7 @@
I2C_cmd[4] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100>>8)&0xFF); // Cable diameter upper
I2C_cmd[5] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100)&0xFF); // Cable diameter lower
I2C_cmd[6] = setValue.winchCtrl.sv_WRS_RResolution; // Resolver resolution
- i2c.write(i2c_addr_resolver, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ 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");
@@ -1870,15 +2112,17 @@
DEBUG_PRINT_L0( "Bd0> Initializing completed !\r\n");
DEBUG_PRINT_L0( "Bd0> ------------------------\r\n");
+ led4 = OFF;
+ led3 = OFF;
+ led2 = OFF;
led1 = ON; // Initializing is OK then Power Indicator LED ON
I2C_cmd[4] = 0x00;
I2C_cmd[5] = 0x01;
- i2c.write(i2c_addr_handy, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
+ i2c.write(I2C_ADDRESS_HANDY, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
while( true ) {
Thread::wait(10);
-
// -----------------------------------------------------------------
// Communicate with client PC program.
// TCP connection:
@@ -1888,11 +2132,14 @@
tcp_server.accept(tcp_client);
tcp_client.set_blocking(false, 3500); // Timeout after (300) msec
DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
- DEBUG_PRINT_L3("Bd0> TCP Connection from: %s\r\n", tcp_client.get_address());
+ DEBUG_PRINT_L3("Bd0> TCP Connection from: %s\r\n", tcp_client.get_address());
DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
while(true){
+ // --------------------------------------------------------------
+ // Following instructions are blocking when no ethernat access
+ // --------------------------------------------------------------
rcv_data_cnt = tcp_client.receive(dbuffer, sizeof(dbuffer));
DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
if( rcv_data_cnt < 0 ){