z ysaito
2018.07.26
Dependencies: EthernetInterface TextLCD USBDevice USBHost3 mbed
Fork of
USBHostHub_HelloWorld
by 
Samuel Mokrani
0_main.cpp
- Committer:
- sayzyas
- Date:
- 2016-04-14
- Revision:
- 14:3a5ae61ab1f4
- Parent:
- 13:2c70c772fe24
- Child:
- 15:01680ed6b799
File content as of revision 14:3a5ae61ab1f4:
/***************************************
* Project: B2
* Title: CrExp B2 Motor Ctrl Main
* Target: LPC1768
* ------------------------------------
*
*
*
* mbed LPC1768
* +-------USB-----+
* GND | | VOUT(3.3V)
* VIN | | VU(5.0V OUT)
* VB | | IF-
* mR | # ### # ### | IF+
* p5 mosi | # # # # # # | Ether RD-
* p6 miso | # # ### ### | Ether RD+
* p7 sck | # # # # # # | Ether TD-
* p8 | # # ### ### | Ether TD+
* p9 tx sdi | | USB D-
* p10 rx scl | | USB D+
* p11 mosi | | CAN rd p30
* p12 miso | | CAN td p29
* p13 tx sck | | sda tx p28
* p14 rx | | scl rx P27
* p15 AIn | | PWM P26
* p16 AIn | | PWM P25
* p16 AIn | | PWM p24
* p18 AIn AOut | | PWM p23
* p19 AIn | | PWM p22
* p20 AIn | | PWM p21
* +---------------+
*
***************************************/
#include "mbed.h"
#include "USBHostGamepad.h"
#include "USBSerial.h"
#include "rtos.h"
#include "EthernetInterface.h"
#include "common.h"
#include "stdio.h"
#include "TextLCD.h"
#include "com_func.h"
// USBSerial serial setting
Serial pc(USBTX, USBRX); // UART
// Digital I/O setting
DigitalOut led1(LED1); // 1:on,0:off System is OK then ON.
DigitalOut led2(LED2); // 1:on,0:off GamePad is connected.
DigitalOut led3(LED3); // 1:on,0:off When got the GamePas switch input then ON
DigitalOut led4(LED4); // 1:on,0:off Access indicator with PC
// I2C setting
I2C i2c_res(p28, p27); // I2C SDA, SCL is not good ???
I2C i2c(p9, p10); // I2C SDA, SCL is good
// Ethernet
EthernetInterface eth;
TCPSocketServer tcp_server; // TCP Server
TCPSocketConnection tcp_client;
UDPSocket udp_server; // UDP Server
Endpoint client;
// LCD
TextLCD lcd(p11, p12, p24, p23, p22, p21); // rs, e, d4-d7
// Local File System
LocalFileSystem local("local"); // Create the local filesystem under the name "local"
// Global
uint32_t flg_gamePad_Connected = 0;
char PC_cmd[11+1] = "&0100000000";
basic_operation_t baseOperation;
char dbuffer[128];
uint8_t motor_speed = 0;
// Global Parameter of setting
setValue_t setValue; // Setting Data
/* Status flag */
/*
0000 0000 : button LI LK RI RK PCW PCCW TU TD
0000 0000 : limit switch
0000 0000 : res
0000 0000 : res
*/
uint32_t flg_exp_status = 0;
Mutex flg_mutex;
Mutex swbtn_OpeMutex;
int swbtn_Opeflg = 0;
int16_t winchCurrentPosition;
Mutex mtx_wcp;
bool flg_ButtonOn = false;
bool flg_lsw_valid = false;
int flg_JS_shape_mode = 0;
int flg_JS_ope_mode = 0;
int motor1_current_pct;
int motor2_current_pct;
uint8_t limitSw_Sts = 0;
char motorLock_sts = '\0';
/* Prototype */
void write_Setting_lfs(void);
int read_Setting_lfs(void);
void dsp_console_setting_value(void);
void winchMovingControl( int, char*, int, winchData_t*, int, char* );
extern void dspSetValue2Console( Serial*, setValue_t * );
extern void lcd_out( TextLCD* ,int, int, char* );
// ============================================================
// Read motor current
// ============================================================
void read_motorCurrent(
int i2c_addr,
char* I2C_readcmd,
char* I2C_res,
int NumberOfI2Cdata
){
i2c.read(i2c_addr, I2C_res, NumberOfI2Cdata);
/*
DEBUG_PRINT_L0(" ++++++++++++++++++++++++++++++\r\n" );
DEBUG_PRINT_L0(" Read Motor1 Current [%d]\r\n", I2C_res[0] );
DEBUG_PRINT_L0(" Read Motor2 Current [%d]\r\n", I2C_res[1] );
DEBUG_PRINT_L0(" Read [%d]\r\n", I2C_res[2] );
DEBUG_PRINT_L0(" ++++++++++++++++++++++++++++++\r\n" );
*/
}
// ============================================================
// Send Status to PC
// ============================================================
void sendStatus2PC( char *cmd, int32_t numberOfCmd ){
int i;
led4 = 1;
for ( i = 0; i < numberOfCmd; i++ ) {
pc.putc(*cmd++);
}
led4 = 0;
}
// ============================================================
// Read winch current position from Resolver.
// ============================================================
int16_t ReadWinchCurrentPosition( int32_t i2c_addr )
{
char I2C_data[2];
int16_t res_position = 0;
int rts;
rts = i2c.read(i2c_addr, I2C_data, 2); // Read
mtx_wcp.lock();
if( rts == 0 ){
res_position = (I2C_data[1] << 8) | I2C_data[0];
}
else{
res_position = -1;
}
// winchCurrentPosition = res_position;
mtx_wcp.unlock();
return res_position;
}
char I2C_res[NumberOfI2CCommand+1] = "\0";
// ============================================================
// Button control
// ============================================================
void onControl(
uint8_t btn00, uint8_t btn01, uint8_t btn02, uint8_t btn03,
uint8_t btn04, uint8_t btn05, uint8_t btn06, uint8_t btn07,
uint8_t btn08, uint8_t btn09, uint8_t btn10, uint8_t btn11,
uint8_t btn12, uint8_t btn13, uint8_t btn14, uint8_t btn15,
uint16_t gamePadVID, uint16_t gamePadPID
){
/* ** OLD type ***
* I2C Command (7byte)
* 0: '#' // Preamble
* 1: '0' // Target upper
* 2: '1' // Target lower
* 3: '0' // Command 1
* 4: '1/3' // Command 2
* 5: '0/1'
*/
/* New Type 15.11.06 ~
[0] :
[1] :
[2] :
[3] :
[4] : motor 1 direction (A=Forward, B=Reverse, F=Stop)
[5] : motor 1 speed
[6] : motor 2 direction (A=Forward, B=Reverse, F=Stop)
[7] : motor 2 speed <-- New added
[8] : N/F
[9] : N/F
*/
char I2C_cmd[NumberOfI2CCommand+1] = "#0100000000000";
char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
uint8_t btnStatus_RFK = 0;
uint8_t btnStatus_RFI = 0;
uint8_t btnStatus_LBK = 0;
uint8_t btnStatus_LBI = 0;
uint8_t btnStatus_WUP = 0;
uint8_t btnStatus_WDN = 0;
uint8_t btnStatus_RJSFwdRvs = 0; // R-JS Fwd/Rvs
uint8_t btnStatus_RJSLftRgt = 0; // R-JS Left/Light
uint8_t btnStatus_LJSFwdRvs = 0; // L-JS Fwd/Rvs
uint8_t btnStatus_LJSLftRgt = 0; // L-Js Left/Right
uint8_t btnStatus_Start = 0; // Guide button status for ELECOM GamePad
uint8_t btnStatus_CrossUp = 0;
uint8_t btnStatus_CrossDn = 0;
uint8_t btnStatus_CrossRt = 0;
uint8_t btnStatus_CrossLt = 0;
// For JS Ope mode B
uint8_t btnID_RFK = 0;
uint8_t btnID_RFI = 0;
uint8_t btnID_LBK = 0;
uint8_t btnID_LBI = 0;
uint8_t btnID_RFLBI = 0; // RF-I and LB-I both button on same time
uint8_t btnID_RFLBK = 0; // RF-K and LB-K both button on same time
uint8_t btnID_WUP = 0;
uint8_t btnID_WDN = 0;
uint8_t btnID_Start = 0; // Guide button ID for ELECOM GamePad
uint8_t btnID_JS_SD = 0; // JS mode Single / Dual
uint8_t btnID_JD_IK = 0; // JS mode I-Shape / KO-Shape
uint8_t btnID_CrossUp = 0;
uint8_t btnID_CrossDn = 0;
uint8_t btnID_CrossRt = 0;
uint8_t btnID_CrossLt = 0;
if( gamePadVID == GAMEPAD_VID_RSTHANDY ){ // NRst own original controller
DEBUG_PRINT_L4("Bd0> [Rst HDY] ");
btnID_WDN = 0x10;
btnID_WUP = 0x20;
btnID_RFK = 0x01;
btnID_RFI = 0x02;
btnID_LBK = 0x04;
btnID_LBI = 0x08;
btnID_CrossUp = 0;
btnID_CrossDn = 4;
btnID_CrossRt = 2;
btnID_CrossLt = 6;
// ---- This is temporary setting ------------------------------
btnID_RFLBI = 192; // RF-I and LB-I both button on same time
btnID_RFLBK = 48; // RF-K and LB-K both button on same time
// --------------------------------------------------------------
btnID_JS_SD = 1; // JS mode Single or Dual
btnID_JD_IK = 2; // JS mode I-Shape KO-Shape
if ( gamePadPID == GAMEPAD_PID_RSTHANDY ){
btnStatus_WDN = btn04;
btnStatus_WUP = btn04;
btnStatus_RFK = btn04;
btnStatus_RFI = btn04;
btnStatus_LBK = btn04;
btnStatus_LBI = btn04;
btnStatus_RJSFwdRvs = btn03; // Assign analog js to this sw input by LPC1768mbed
btnStatus_RJSLftRgt = btn02; // Assign analog js to this sw input by LPC1768mbed
btnStatus_LJSFwdRvs = btn01; // Assign analog js to this sw input by LPC1768mbed
btnStatus_LJSLftRgt = btn00; // Assign analog js to this sw input by LPC1768mbed
btnStatus_CrossUp = btn06;
btnStatus_CrossDn = btn06;
btnStatus_CrossRt = btn06;
btnStatus_CrossLt = btn06;
btnStatus_Start = btn05; //
}
}
else if (gamePadVID == GAMEPAD_VID_ELECOM ){
DEBUG_PRINT_L4("Bd0> [ELECOM] ");
btnID_WDN = 4;
btnID_WUP = 2;
btnID_RFK = 32;
btnID_RFI = 128;
btnID_LBK = 16;
btnID_LBI = 64;
// ---------------------
btnID_RFLBI = 192; // RF-I and LB-I both button on same time
btnID_RFLBK = 48; // RF-K and LB-K both button on same time
// ---------------------
btnID_Start = 8; // Guide button ID for ELECOM GamePad
btnID_CrossUp = 0;
btnID_CrossDn = 4;
btnID_CrossRt = 2;
btnID_CrossLt = 6;
if ( gamePadPID == GAMEPAD_PID_ELECOM_JCU3613M ){
btnStatus_WDN = btn04;
btnStatus_WUP = btn04;
btnStatus_RFK = btn04;
btnStatus_RFI = btn04;
btnStatus_LBK = btn04;
btnStatus_LBI = btn04;
btnStatus_RJSFwdRvs = btn03;
btnStatus_RJSLftRgt = btn02;
btnStatus_LJSFwdRvs = btn01;
btnStatus_LJSLftRgt = btn00;
btnStatus_Start = btn05; // Guide button status for ELECOM GamePad
btnStatus_CrossUp = btn06;
btnStatus_CrossDn = btn06;
btnStatus_CrossRt = btn06;
btnStatus_CrossLt = btn06;
}
}
else if( gamePadVID == GAMEPAD_VID_LOGICOOL ){
btnID_WDN = 40;
btnID_WUP = 136;
btnID_RFK = 2;
btnID_RFI = 8;
btnID_LBK = 1;
btnID_LBI = 4;
btnID_Start = 32; // Guide button ID for ELECOM GamePad
btnID_CrossUp = 0;
btnID_CrossDn = 4;
btnID_CrossRt = 2;
btnID_CrossLt = 6;
if ( gamePadPID == GAMEPAD_PID_LOGICOOL_F710 ){
DEBUG_PRINT_L4("Bd0> [LOGI F710] ");
btnStatus_WDN = btn05;
btnStatus_WUP = btn05;
btnStatus_RFK = btn06;
btnStatus_RFI = btn06;
btnStatus_LBK = btn06;
btnStatus_LBI = btn06;
}
else if ( gamePadPID == GAMEPAD_PID_LOGICOOL_F310 ){
DEBUG_PRINT_L4("Bd0> [LOGI F310] ");
btnStatus_WDN = btn04;
btnStatus_WUP = btn04;
btnStatus_RFK = btn05;
btnStatus_RFI = btn05;
btnStatus_LBK = btn05;
btnStatus_LBI = btn05;
btnStatus_RJSFwdRvs = btn03;
btnStatus_RJSLftRgt = btn02;
btnStatus_LJSFwdRvs = btn01;
btnStatus_LJSLftRgt = btn00;
btnStatus_Start = btn05; // Guide button status for ELECOM GamePad
btnStatus_CrossUp = btn04;
btnStatus_CrossDn = btn04;
btnStatus_CrossRt = btn04;
btnStatus_CrossLt = btn04;
}
}
else if ( gamePadVID == GAMEPAD_VID_SANWA){
DEBUG_PRINT_L4("Bd0> [SANWA] ");
btnID_WDN = 2;
btnID_WUP = 4;
btnID_RFK = 2;
btnID_RFI = 1;
btnID_LBK = 128;
btnID_LBI = 64;
// ---------------------
btnID_RFLBI = 80; // RF-I and LB-I both button on same time
btnID_RFLBK = 40; // RF-K and LB-K both button on same time
// ---------------------
btnID_CrossUp = 0;
btnID_CrossDn = 255;
btnID_CrossRt = 0;
btnID_CrossLt = 255;
if ( gamePadPID == GAMEPAD_PID_SANWA_JYP70US ){
btnStatus_WDN = btn05;
btnStatus_WUP = btn05;
btnStatus_RFK = btn06;
btnStatus_RFI = btn06;
btnStatus_LBK = btn05;
btnStatus_LBI = btn05;
btnStatus_RJSFwdRvs = btn03; // Assign analog js to this sw input by LPC1768mbed
btnStatus_RJSLftRgt = btn02; // Assign analog js to this sw input by LPC1768mbed
btnStatus_LJSFwdRvs = btn01; // Assign analog js to this sw input by LPC1768mbed
btnStatus_LJSLftRgt = btn00; // Assign analog js to this sw input by LPC1768mbed
btnStatus_CrossUp = btn01;
btnStatus_CrossDn = btn01;
btnStatus_CrossRt = btn00;
btnStatus_CrossLt = btn00;
}
}
#ifdef __DISP_GAMAPAD_STATUS_ALL__ // For Debug
// DEBUG_PRINT_BTN(" Btn 00:%d, 01:%d, 02:%d, 03:%d, 04:%d, 05:%d, 06:%d, 07:%d, 08:%d | VID=0x%04x, PID=0x%04x\r\n",
// btn00,btn01,btn02,btn03,btn04,btn05,btn06,btn07,btn08,
// gamePadVID, gamePadPID);
DEBUG_PRINT_SW("Bd0> -- Button Status -------------------------------\r\n");
DEBUG_PRINT_SW("Bd0> 00(%02x) 01(%02x) 02(%02x) 03(%02x)\r\n", btn00,btn01,btn02,btn03);
DEBUG_PRINT_SW("Bd0> 04(%02x) 05(%02x) 06(%02x) 07(%02x) 08(%02x)\r\n", btn04,btn05,btn06,btn07,btn08);
DEBUG_PRINT_SW("Bd0> 09(%02x) 10(%02x) 11(%02x) 12(%02x) 13(%02x) 14(%02x) 15(%02x)\r\n", btn09,btn10,btn11,btn12,btn13,btn14,btn15);
DEBUG_PRINT_SW("Bd0> ------------------------------------------------\r\n");
#endif
I2C_cmd[I2C_CP_M1_DIR] = '\0';
I2C_cmd[I2C_CP_M1_SPEED] = '\0';
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = '\0';
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = '\0';
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = '\0';
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = '\0';
I2C_cmd[I2C_CP_M2_DIR] = '\0';
I2C_cmd[I2C_CP_M2_SPEED] = '\0';
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = '\0';
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = '\0';
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = '\0';
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = '\0';
int tmpSpeed = 0;
if (swbtn_Opeflg == 1){
Thread::wait(1);
}
else{
if( flg_lsw_valid == true ){
I2C_cmd[1] = 'V';
}
else{
I2C_cmd[1] = '0';
}
if( gamePadVID == GAMEPAD_VID_RSTHANDY ){ // NRst own original controller
flg_exp_status &= 0xFFFFFFF0;
if(!( btnStatus_Start & 0x01 )){ // I-Shape
flg_mutex.lock();
baseOperation.sv_JS_ShapeMode = 0;
baseOperation.sv_WinchValid = 0;
flg_mutex.unlock();
flg_exp_status |= 0x00000001;
}
else{ // KO-Shape
flg_mutex.lock();
baseOperation.sv_JS_ShapeMode = 1;
flg_mutex.unlock();
flg_exp_status |= 0x00000002;
}
if(!(btnStatus_Start & 0x02 )){ // Tfm,crawler part valid
flg_mutex.lock();
baseOperation.sv_WinchValid = 0;
flg_mutex.unlock();
flg_exp_status |= 0x00000004;
}
else{ // Winch part valid
flg_mutex.lock();
baseOperation.sv_WinchValid = 1;
flg_mutex.unlock();
flg_exp_status |= 0x00000008;
}
DEBUG_PRINT_L4( "-----------------------------\r\n" );
DEBUG_PRINT_L4( "%d : %d\r\n",btnStatus_Start, flg_exp_status );
DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
DEBUG_PRINT_L4( "-----------------------------\r\n" );
}
else{
/*
*
* GamePad software switch
* Cross button Up on : JS shape mode I
* Cross button Down on : JS shape mode KO
* Cross button Right on: Winch part valid
* Cross button Left on : Crawlerm, Transform part valid
* 7 6 5 4 3 2 1 0
* +-+-+-+-+-+-+-+-+
* |x|x|x|x|x|x|x|o| 1: I-Shape JSmode, 2: K-Shape JSmode, 4: Left part(Crawler, Tfm) part valid, 8: Winch part valid
* +-+-+-+-+-+-+-+-+
*/
if ( btnStatus_CrossUp == btnID_CrossUp ){ // I Shape
flg_mutex.lock();
baseOperation.sv_JS_ShapeMode = 0;
flg_mutex.unlock();
DEBUG_PRINT_L4( "--------------------------------\r\n" );
DEBUG_PRINT_L4( " I\r\n" );
DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
DEBUG_PRINT_L4( "--------------------------------\r\n" );
flg_exp_status |= 0x00000001;
}
else if( btnStatus_CrossDn == btnID_CrossDn ){ // KO Shape
flg_mutex.lock();
baseOperation.sv_JS_ShapeMode = 1;
flg_mutex.unlock();
DEBUG_PRINT_L4( "-------------------------\r\n" );
DEBUG_PRINT_L4( " KO\r\n" );
DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
DEBUG_PRINT_L4( "-------------------------\r\n" );
flg_exp_status |= 0x00000002;
}
else if( btnStatus_CrossRt == btnID_CrossLt ){ // Valid Part : Crawler (Left)
flg_mutex.lock();
baseOperation.sv_WinchValid = 0;
flg_mutex.unlock();
DEBUG_PRINT_L4( "-----------------------------\r\n" );
DEBUG_PRINT_L4( " Tfm, Crawler\r\n" );
DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
DEBUG_PRINT_L4( "-----------------------------\r\n" );
flg_exp_status |= 0x00000004;
}
else if( btnStatus_CrossLt == btnID_CrossRt ){ // Valid Part : Winch (Right)
flg_mutex.lock();
baseOperation.sv_WinchValid = 1;
flg_mutex.unlock();
DEBUG_PRINT_L4( "-----------------------------\r\n" );
DEBUG_PRINT_L4( " Winch\r\n" );
DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
DEBUG_PRINT_L4( "-----------------------------\r\n" );
flg_exp_status |= 0x00000008;
}
else{
flg_exp_status &= 0xFFFFFFF0;
}
}
/*
// ====================================
// TRANSFORM Motor Control
// ====================================
* 7 6 5 4 3 2 1 0
* +-+-+-+-+-+-+-+-+
* |o|x|x|x|x|x|x|x| 1: RF-I, 2: RF-K, 4: LB-I, 8: LB-K
* +-+-+-+-+-+-+-+-+
*/
if ((btnStatus_RFK==btnID_RFLBK)&&(baseOperation.sv_WinchValid==0)){ // Both sw on
flg_ButtonOn = true;
DEBUG_PRINT_L3( "Bd0> BTN RF-K & LB-K\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_F; // Speed
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor1 FWD
I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_F; // Speed
#endif
flg_exp_status |= 0x30000000;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
#ifdef __READ_TFM_MOTOR_CURRENT__
read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
limitSw_Sts = I2C_res[3];
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
#endif // __IIC_COMAMND_SEND__
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if ((btnStatus_RFI== btnID_RFLBI)&&(baseOperation.sv_WinchValid==0)){ // Both sw on
flg_ButtonOn = true;
DEBUG_PRINT_L3( "Bd0> BTN RF-I & LB-I\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 FWD
I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_F; // Speed
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor1 FWD
I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_F; // Speed
#endif
flg_exp_status |= 0x10000000;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
#ifdef __READ_TFM_MOTOR_CURRENT__
read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
limitSw_Sts = I2C_res[3];
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
#endif // __IIC_COMAMND_SEND__
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if ((btnStatus_RFK==btnID_RFK)&&(baseOperation.sv_WinchValid==0)){ // RF KO
flg_ButtonOn = true;
DEBUG_PRINT_L3( "Bd0> BTN RF-K\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_F; // Speed
#endif
flg_exp_status |= 0x10000000;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
#ifdef __READ_TFM_MOTOR_CURRENT__
read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
limitSw_Sts = I2C_res[3];
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
#endif // __IIC_COMAMND_SEND__
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if ((btnStatus_RFI==btnID_RFI)&&(baseOperation.sv_WinchValid==0)){ // RF I
DEBUG_PRINT_L3( "Bd0> BTN RF-I\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_R; // Speed
#endif
flg_exp_status |= 0x20000000;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
#ifdef __READ_TFM_MOTOR_CURRENT__
read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
limitSw_Sts = I2C_res[3];
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
#endif // __READ_TFM_MOTOR_CURRENT__
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if ((btnStatus_LBK==btnID_LBK)&&(baseOperation.sv_WinchValid==0)){ // LB KO
flg_ButtonOn = true;
DEBUG_PRINT_L3( "Bd0> BTN LB-K\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 FWD
I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_F; // Speed
#endif
flg_exp_status |= 0x40000000;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
#ifdef __READ_TFM_MOTOR_CURRENT__
read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
limitSw_Sts = I2C_res[3];
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
#endif // __READ_TFM_MOTOR_CURRENT__
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if ((btnStatus_LBI==btnID_LBI)&&(baseOperation.sv_WinchValid==0)) { // LB I
flg_ButtonOn = true;
DEBUG_PRINT_L3( "Bd0> BTN LB-I\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor1 RVS
I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_R; // Speed
#endif
flg_exp_status |= 0x80000000;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
#ifdef __READ_TFM_MOTOR_CURRENT__
read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
limitSw_Sts = I2C_res[3];
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
#endif // __READ_TFM_MOTOR_CURRENT__
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
// ====================================
// ALL motor off commmand packet send
// -- This part isn't operated in case of general game controller, because no event. --
// ====================================
else if (baseOperation.sv_WinchValid==0){
led3 = 0;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP;
I2C_cmd[I2C_CP_M1_SPEED] = 0;
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP;
I2C_cmd[I2C_CP_M2_SPEED] = 0;
Thread::wait(5);
#endif
flg_exp_status &= 0x0FFFFFFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
#ifdef __READ_TFM_MOTOR_CURRENT__
read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
limitSw_Sts = I2C_res[3];
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
#endif // __READ_TFM_MOTOR_CURRENT__
}
/*/ ====================================
// Crawler Moving Control
// ====================================
// JoyStick mode 1: Independence mode ( Dual JoyStick mode )
//
// *** ***
// * L * * R *
// *** ***
// F 4 1
//
// R 8 2
//
// Forward move 5
// Reverce move a
// Right rotation 6
// Left rotation 9
* 7 6 5 4 3 2 1 0
* +-+-+-+-+-+-+-+-+
* |x|x|o|x|x|x|x|x| 1: R Fwd, 2: R Rvs, 4: L Fwd, 8: L Rvs
* +-+-+-+-+-+-+-+-+
*/
if( baseOperation.sv_JS_OpeMode == 1 ){
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_R & 0xFF );
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_R & 0xFF );
if ((btnStatus_LJSFwdRvs<setValue.crawlerCtrl.sv_LBCM_dzc-setValue.crawlerCtrl.sv_LBCM_dzl)&&(baseOperation.sv_WinchValid==0)){
flg_ButtonOn = true;
led3 = 1;
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 FWD
tmpSpeed = ( setValue.crawlerCtrl.sv_LBCM_dzc+1 - btnStatus_LJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc * setValue.crawlerCtrl.sv_LBCM_srto_F / 100; // Speed
I2C_cmd[I2C_CP_M2_SPEED] = (char)tmpSpeed;
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Fwd (Speed=%d)\r\n", tmpSpeed);
// Read motor current from target
read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
flg_exp_status |= 0x00400000;
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if ((btnStatus_LJSFwdRvs>setValue.crawlerCtrl.sv_LBCM_dzc+setValue.crawlerCtrl.sv_LBCM_dzu)&&(baseOperation.sv_WinchValid==0)){
flg_ButtonOn = true;
led3 = 1;
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 RVS
tmpSpeed = ( btnStatus_LJSFwdRvs - setValue.crawlerCtrl.sv_LBCM_dzc ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc+1 * setValue.crawlerCtrl.sv_LBCM_srto_F / 100; // Speed I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = (char)tmpSpeed;
I2C_cmd[I2C_CP_M2_SPEED] = (char)tmpSpeed;
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Rvs (Speed=%d)\r\n", tmpSpeed);
// Read motor current from target
read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
flg_exp_status |= 0x00800000;
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if (baseOperation.sv_WinchValid==0) {
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Stop
I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed=0
flg_exp_status &= 0xFF3FFFFF;
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
}
if ((btnStatus_RJSFwdRvs<setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)&&(baseOperation.sv_WinchValid==0)){
flg_ButtonOn = true;
led3 = 1;
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
tmpSpeed = (( 128 - btnStatus_RJSFwdRvs ) * 100 / 127 ) * setValue.crawlerCtrl.sv_RFCM_srto_R / 100;
I2C_cmd[I2C_CP_M1_SPEED] = (char)tmpSpeed;
DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Fwd (Speed=%d)\r\n", tmpSpeed);
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
// Read motor current from target
read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
flg_exp_status |= 0x00100000;
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if ((btnStatus_RJSFwdRvs>setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu)&&(baseOperation.sv_WinchValid==0)){
flg_ButtonOn = true;
led3 = 1;
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
tmpSpeed = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc+1 * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
I2C_cmd[I2C_CP_M1_SPEED] = (char)tmpSpeed;
DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Rvs (Speed=%d)\r\n", tmpSpeed);
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
// Read motor current from target
read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
flg_exp_status |= 0x00200000;
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if (baseOperation.sv_WinchValid==0) {
DEBUG_PRINT_L2("***** MOTOR STOP ****\r\n");
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Stop
I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed=0
flg_exp_status &= 0xFFCFFFFF;
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
}
led3 = 0;
}
/* JoyStick mode 0: Syncronous mode ( Single JoyStick mode )
//
// *** ****
// * X * * LR *
// *** ****
// F 4 1
//
// R 8 2
//
// Forward move 5
// Reverce move a
// Right rotation 6
// Left rotation 9
* 7 6 5 4 3 2 1 0
* +-+-+-+-+-+-+-+-+
* |x|x|o|x|x|x|x|x| 1: R Fwd, 2: R Rvs, 4: L Fwd, 8: L Rvs
* +-+-+-+-+-+-+-+-+
*/
else{ // Single JoyStick mode
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_R & 0xFF );
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_R & 0xFF );
if(
( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl ) &&
( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50) &&
( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50 ) &&
( baseOperation.sv_WinchValid == 0)
){
flg_ButtonOn = true;
led3 = 1;
if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Reverse
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
}
else{
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Fwd
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
}
I2C_cmd[I2C_CP_M1_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
I2C_cmd[I2C_CP_M2_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
DEBUG_PRINT_L3( "Bd0> Single Mode: Dir1 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_RVS_CNTTH_U], I2C_cmd[I2C_CP_M2_SPEED]);
flg_exp_status |= 0x00500000;
//flg_exp_status |= 0x00400000; // 0x00500000
// Read motor current from target
read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if(
( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu ) &&
( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50 ) &&
( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
( baseOperation.sv_WinchValid == 0)
){
flg_ButtonOn = true;
led3 = 1;
if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Reverse
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor1 Reverse
}
else{
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Rvs
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
}
I2C_cmd[I2C_CP_M1_SPEED] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
I2C_cmd[I2C_CP_M2_SPEED] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
DEBUG_PRINT_L3( "Bd0> Single Mode: Dir2 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_RVS_CNTTH_U], I2C_cmd[I2C_CP_M2_SPEED]);
flg_exp_status |= 0x00600000;
//flg_exp_status |= 0x00400000; // 0x00600000
// Read motor current from target
read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if(
( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu ) &&
( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50) &&
( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
( baseOperation.sv_WinchValid == 0)
){
flg_ButtonOn = true;
led3 = 1;
if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Rvs
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
}
else{
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Rvs
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
}
I2C_cmd[I2C_CP_M1_SPEED] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
I2C_cmd[I2C_CP_M2_SPEED] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
DEBUG_PRINT_L3( "Bd0> Single Mode: Dir3 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_RVS_CNTTH_U], I2C_cmd[I2C_CP_M2_SPEED]);
flg_exp_status |= 0x00A00000;
//flg_exp_status |= 0x00800000; // 0x00A00000
// Read motor current from target
read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if(
( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzu ) &&
( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50) &&
( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
( baseOperation.sv_WinchValid == 0)
){
flg_ButtonOn = true;
led3 = 1;
if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Reverse
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor1 Reverse
}
else{
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Fwd
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
}
I2C_cmd[I2C_CP_M1_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
I2C_cmd[I2C_CP_M2_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
DEBUG_PRINT_L3( "Bd0> Single Mode: Dir4 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_RVS_CNTTH_U], I2C_cmd[I2C_CP_M2_SPEED]);
flg_exp_status |= 0x00900000;
//flg_exp_status |= 0x00800000; // 0x00900000
// Read motor current from target
read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
motorLock_sts = I2C_res[4];
if( motorLock_sts == '1' ){
motor1_current_pct = 999;
}
else{
motor1_current_pct = I2C_res[0];
}
if( motorLock_sts == '2' ){
motor2_current_pct = 999;
}
else{
motor2_current_pct = I2C_res[1];
}
DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
// ====================================
// ALL motor off commmand packet send
// ====================================
else if (baseOperation.sv_WinchValid==0) {
led3 = 0;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Fwd
I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed=0
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 Rvs
I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed=0
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
// Thread::wait(5);
#endif
flg_exp_status &= 0xFF0FFFFF;
// I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_F;
// I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_R;
// I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_F;
// I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_R;
}
}
/*
* ====================================
* Winch Motor Control
*
* 7 6 5 4 3 2 1 0
* +-+-+-+-+-+-+-+-+
* |x|o|x|x|x|x|x|x| 1: W Down, 2: W Up, 4: -, 8: -
* +-+-+-+-+-+-+-+-+
*/
if ((btnStatus_WDN == btnID_WDN) && (baseOperation.sv_WinchValid == 1)){ // Winch Down (FWD)
flg_ButtonOn = true;
DEBUG_PRINT_L3( "Bd0> BTN W-DN(Fwd)\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_F; // Speed
// I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 FWD
// I2C_cmd[I2C_CP_M2_SPEED] = setValue.winchCtrl.sv_WRM_hsrto_F; // Speed
#endif
flg_exp_status |= 0x01000000;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_R & 0xFF );
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_R & 0xFF );
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if (( btnStatus_WUP == btnID_WUP ) && (baseOperation.sv_WinchValid == 1)) { // Winch Up (Rvs)
flg_ButtonOn = true;
DEBUG_PRINT_L3( "Bd0> BTN W-UP(Rvs)\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_R; // Speed
// I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 RVS
// I2C_cmd[I2C_CP_M2_SPEED] = setValue.winchCtrl.sv_WRM_hsrto_R; // Speed
#endif
flg_exp_status |= 0x02000000;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_R & 0xFF );
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_R & 0xFF );
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
// ====================================
// ALL motor off commmand packet send
// ====================================
else if (baseOperation.sv_WinchValid == 1){
led3 = 0;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP;
I2C_cmd[I2C_CP_M1_SPEED] = 0;
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP;
I2C_cmd[I2C_CP_M2_SPEED] = 0;
Thread::wait(5);
#endif
flg_exp_status &= 0xF0FFFFFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_R & 0xFF );
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_F >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_F & 0xFF );
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_R >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_R & 0xFF );
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
}
}
}
uint32_t getc_fromHost( uint8_t *c ){
uint32_t rts = 0;
if(pc.readable()){
*c = pc.getc();
rts = 0;
}
else{
rts = 1;
}
return rts;
}
// **************************************************************
// TASK: Hoat Interface Task
//
// **************************************************************
int first_counter = 0;
void clientPC_interface_task(void const *)
{
int rcv_data_cnt;
//winchData_t winchData;
char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
// winchData_t winchData;
// int16_t winchCurrentPosition;
int16_t winchTempPosition;
int cnt = 0;
while(1){
// DEBUG_PRINT("\r\nWaiting for UDP packet...\r\n");
rcv_data_cnt = udp_server.receiveFrom(client, dbuffer, sizeof(dbuffer));
Thread::wait(10);
if( rcv_data_cnt < 0 ){
DEBUG_PRINT_L0("Bd0> ## Receive packet fail ##\r\n");
}
else{
dbuffer[rcv_data_cnt] = '\0';
led4 = 1;
if(!strcmp( dbuffer, "Hello Z\r\n" )){
DEBUG_PRINT_L2("Bd0> Hello Z Packet received from client by UDP\n");
char snd_data[] = "Hello I'm CrExoB2";
udp_server.sendTo(client, snd_data, sizeof(snd_data));
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if(!strcmp( dbuffer, "status\r\n")){
DEBUG_PRINT_L2("Bd0> Hello Status req received from client by UDP\r\n");
strcpy(dbuffer,"XXXX\r\n");
udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if(!strcmp( dbuffer, "set_jss")){ // Single JS mode
baseOperation.sv_JS_OpeMode = 0;
}
else if(!strcmp( dbuffer, "set_jsd")){ // Dual JS mode
baseOperation.sv_JS_OpeMode = 1;
}
else if(!strcmp( dbuffer, "lsw_valid")){ // Limit swich detection valid
flg_lsw_valid = true;
}
else if(!strcmp( dbuffer, "lsw_invalid")){ // Limit swich detection invalid
flg_lsw_valid = false;
}
else if(!strcmp( dbuffer, "Hello")){
DEBUG_PRINT_L2( "Bd0> Hello Packet received from [ 0x%02x ]\r\n", flg_exp_status );
/* ***************************************** */
/* Read Winch Current Position from Resolver */
/* ***************************************** */
if (baseOperation.sv_WinchValid == 1){ // read winch current position operation is valid in case of winch part is valid.
winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
if( winchTempPosition != -1 ){
mtx_wcp.lock();
winchCurrentPosition = winchTempPosition;
mtx_wcp.unlock();
}
}
Thread::wait(10);
first_counter++;
if( first_counter > 10 ) {
sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts); // Winch down
first_counter = 10;
}
else{
sprintf( dbuffer,"JSMD %03d %03d 0000 000\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid );
}
// -------------------------------------
// Crawler Moving
// -------------------------------------
if( flg_exp_status & 0x00F00000 ){
// Forward move 5
// Reverce move a
// Right rotation 6
// Left rotation 9
if((flg_exp_status & 0x00F00000) == 0x00500000 ){
// 01234 5678 9012 34569
sprintf( dbuffer, "BCFW %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00a00000 ){
sprintf( dbuffer, "BCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00600000 ){
sprintf( dbuffer, "BCRR %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00900000 ){
sprintf( dbuffer, "BCLR %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00800000 ){
sprintf( dbuffer, "LCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00400000 ){
sprintf( dbuffer, "LCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00200000 ){
sprintf( dbuffer, "RCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00100000 ){
sprintf( dbuffer, "RCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
Thread::wait(10);
}
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
pc.printf("\t\t\t S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
// -------------------------------------
// Transform
// -------------------------------------
else if( flg_exp_status & 0x20000000 ){
sprintf(dbuffer,"RF2I %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // RF Crawler Tfm I
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
pc.printf("\t\t\t S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x10000000 ){
sprintf(dbuffer,"RF2K %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // RF Crawler Tfm K
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x80000000 ){
sprintf(dbuffer,"LB2I %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // LB Crawler Tfm I
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x40000000 ){
sprintf(dbuffer,"LB2K %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // LB Crawler Tfm K
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
// -------------------------------------
// Winch Moving
// -------------------------------------
else if( flg_exp_status & 0x01000000 ){ // Wincd down (FWD)
sprintf(dbuffer,"WCDN %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x02000000 ){ // Winch up (RVS)
sprintf(dbuffer,"WCUP %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x0000000f ){
if( cnt == 3 ){
sprintf( dbuffer,"JSMD %03d %03d 0000 000\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid );
cnt = 0;
}
else{
sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
}
cnt++;
DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else{
sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
/* Send data to client PC */
udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
Thread::wait(10);
// ----------------------------------------------------------------------------
// Read target(transform controller) status in each time.
read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 4 );
motor1_current_pct = I2C_res[0];
motor2_current_pct = I2C_res[1];
limitSw_Sts = I2C_res[3];
// DEBUG_PRINT_L2( "Bd0> X: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
// DEBUG_PRINT_L2( "Bd0> X: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
// DEBUG_PRINT_L2( "Bd0> X: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
// ----------------------------------------------------------------------------
}
}
// Thread::wait(100); // When this period seto to short time, gamepad command can not get. 50msec OK
led4 = 0;
// Thread::wait(100); // When this period seto to short time, gamepad command can not get. 50msec OK
}
}
// **************************************************************
// TASK: GamaPad Task
//
// **************************************************************
void gamepad_task(void const *)
{
char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
// int counter = 0;
// USB HOST GAMEPAD
USBHostGamepad gamepad;
led1 = 1;
while(1) {
// try to connect a USB Gamepad
while(!gamepad.connect()) {
flg_gamePad_Connected = 0;
led2 = OFF;
// led1 = 1;
Thread::wait(500);
}
// when connected, attach handler called on gamepad event
gamepad.attachEvent(onControl);
// wait until the Gamepad is disconnected
while(gamepad.connected()) {
flg_gamePad_Connected = 1;
led2 = !led2;
led2 = ON;
// led1 = 1;
// Send status to Handy Ctrl controller, but currently this is only for Main Controller.
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = 0x00;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = 0x01;
i2c.write(I2C_ADDRESS_HANDY, I2C_cmd, NumberOfI2CCommand);
// DEBUG_PRINT_L2("Bd0> Send Handy Controller(%02x) >>>>>>>>>>\r\n",I2C_ADDRESS_HANDY );
Thread::wait(500);
}
}
}
// ======================================================================
// Read setting value from lpcal file system of mbed
// ======================================================================
bool read_LFS( setValue_t* setValue ){
FILE *fp;
char *fname = "/local/set.txt";
char s[150];
int c;
int data;
bool rts;
flg_mutex.lock();
fp = fopen(fname, "r");
if( fp != NULL ){ // Open "set.txt" on the local file system for writing
c = getc(fp);
if( c != '#' ){
pc.printf( "#### ERROR This is not a setting file ####\r\n");
rts = false;
}
else{
fgets( s, 100, fp );
pc.printf( "%s", s );
fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WDM_ith_F=data; pc.printf("%04d",setValue->winchCtrl.sv_WDM_ith_F); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WDM_ith_R=data; pc.printf("%04d",setValue->winchCtrl.sv_WDM_ith_R); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRM_ith_F=data; pc.printf("%04d",setValue->winchCtrl.sv_WRM_ith_F); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRM_ith_R=data; pc.printf("%04d",setValue->winchCtrl.sv_WRM_ith_R); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_hsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_hsrto_F); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_hsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_hsrto_R); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_lsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_lsrto_F); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_lsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_lsrto_R); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_hsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_hsrto_F); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_hsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_hsrto_R); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_lsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_lsrto_F); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_lsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_lsrto_R); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRS_DramDmrx100=data; pc.printf("%04d",setValue->winchCtrl.sv_WRS_DramDmrx100); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRS_CCableDmrx100=data; pc.printf("%04d",setValue->winchCtrl.sv_WRS_CCableDmrx100); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRS_RResolution=data; pc.printf("%03d",setValue->winchCtrl.sv_WRS_RResolution); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->winchCtrl.reserved=data; pc.printf("%03d",setValue->winchCtrl.reserved); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_RFTM_ith_F=data; pc.printf("%04d",setValue->tfmCtrl.sv_RFTM_ith_F); fgets(s, 100, fp ); pc.printf("%s",s);
fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_RFTM_ith_R=data; pc.printf("%04d",setValue->tfmCtrl.sv_RFTM_ith_R); fgets(s, 100, fp ); pc.printf("%s",s);
fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_LBTM_ith_F=data; pc.printf("%04d",setValue->tfmCtrl.sv_LBTM_ith_F); fgets(s, 100, fp ); pc.printf("%s",s);
fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_LBTM_ith_R=data; pc.printf("%04d",setValue->tfmCtrl.sv_LBTM_ith_R); fgets(s, 100, fp ); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_RFTM_srto_F=data; pc.printf("%03d",setValue->tfmCtrl.sv_RFTM_srto_F); fgets(s, 100, fp ); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_RFTM_srto_R=data; pc.printf("%03d",setValue->tfmCtrl.sv_RFTM_srto_R); fgets(s, 100, fp ); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_LBTM_srto_F=data; pc.printf("%03d",setValue->tfmCtrl.sv_LBTM_srto_F); fgets(s, 100, fp ); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_LBTM_srto_R=data; pc.printf("%03d",setValue->tfmCtrl.sv_LBTM_srto_R); fgets(s, 100, fp ); pc.printf("%s",s);
fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_RFCM_ith_F=data; pc.printf("%04d",setValue->crawlerCtrl.sv_RFCM_ith_F); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_RFCM_ith_R=data; pc.printf("%04d",setValue->crawlerCtrl.sv_RFCM_ith_R); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_LBCM_ith_F=data; pc.printf("%04d",setValue->crawlerCtrl.sv_LBCM_ith_F); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_LBCM_ith_R=data; pc.printf("%04d",setValue->crawlerCtrl.sv_LBCM_ith_R); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_srto_F=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_srto_F); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_srto_R=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_srto_R); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_srto_F=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_srto_F); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_srto_R=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_srto_R); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_dzu=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_dzu); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_dzc=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_dzc); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_dzl=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_dzl); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_dzu=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_dzu); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_dzc=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_dzc); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_dzl=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_dzl); fgets(s,100,fp); pc.printf("%s",s );
fscanf(fp,"%03d",&data);setValue->crawlerCtrl.reserved1=data; pc.printf("%03d",setValue->crawlerCtrl.reserved1); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->crawlerCtrl.reserved2=data; pc.printf("%03d",setValue->crawlerCtrl.reserved2); fgets(s,100,fp); pc.printf("%s",s);
}
fclose(fp);
rts = true;
}
else{
pc.printf( "#### ERROR local file open error ####\r\n");
rts = false;
}
flg_mutex.unlock();
return rts;
}
bool write_LFS( setValue_t* setValue ){
FILE *fp;
char *fname = "/local/set.txt";
bool rts = true;
pc.printf("write setting data to setting file \r\n ");
fp = fopen(fname, "w"); // Open "out.txt" on the local file system for writing
if( fp != NULL ){
fprintf(fp, "#### B2 DebrisServayor Setting ####\n");
fclose(fp);
}
else{
rts = false;
pc.printf("File open error (0)\r\n");
}
Thread::wait(50);
fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
if( fp != NULL ){
pc.printf("Writing ... %04d #WDM F-C th\n", setValue->winchCtrl.sv_WDM_ith_F);
fprintf(fp, "%04d #WDM F-C th\n", setValue->winchCtrl.sv_WDM_ith_F);
pc.printf("Writing ... %04d #WDM R-C th\n", setValue->winchCtrl.sv_WDM_ith_R);
fprintf(fp, "%04d #WDM R-C th\n", setValue->winchCtrl.sv_WDM_ith_R);
pc.printf("Writing ... %04d #WM2 F-C th\n", setValue->winchCtrl.sv_WRM_ith_F);
fprintf(fp, "%04d #WM2 F-C th\n", setValue->winchCtrl.sv_WRM_ith_F);
pc.printf("Writing ... %04d #WM2 R-C th\n", setValue->winchCtrl.sv_WRM_ith_R);
fprintf(fp, "%04d #WM2 R-C th\n", setValue->winchCtrl.sv_WRM_ith_R);
fclose(fp);
}
else{
pc.printf("File open error (1)\r\n");
}
Thread::wait(50);
fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
if( fp != NULL ){
pc.printf("Writing ... %03d #WDM F-HS\n", setValue->winchCtrl.sv_WDM_hsrto_F);
fprintf(fp, "%03d #WDM F-HS\n", setValue->winchCtrl.sv_WDM_hsrto_F);
pc.printf("Writing ... %03d #WDM R-HS\n", setValue->winchCtrl.sv_WDM_hsrto_R);
fprintf(fp, "%03d #WDM R-HS\n", setValue->winchCtrl.sv_WDM_hsrto_R);
pc.printf("Writing ... %03d #WDM F-LS\n", setValue->winchCtrl.sv_WDM_lsrto_F);
fprintf(fp, "%03d #WDM F-LS\n", setValue->winchCtrl.sv_WDM_lsrto_F);
pc.printf("Writing ... %03d #WDM R-LS\n", setValue->winchCtrl.sv_WDM_lsrto_R);
fprintf(fp, "%03d #WDM R-LS\n", setValue->winchCtrl.sv_WDM_lsrto_R);
fclose(fp);
}
else{
rts = false;
pc.printf("File open error (2)\r\n");
}
Thread::wait(50);
fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
if( fp != NULL ){
pc.printf("Writing ... %03d #WM2 F-HS\n", setValue->winchCtrl.sv_WRM_hsrto_F);
fprintf(fp, "%03d #WM2 F-HS\n", setValue->winchCtrl.sv_WRM_hsrto_F);
pc.printf("Writing ... %03d #WM2 R-HS\n", setValue->winchCtrl.sv_WRM_hsrto_R);
fprintf(fp, "%03d #WM2 R-HS\n", setValue->winchCtrl.sv_WRM_hsrto_R);
pc.printf("Writing ... %03d #WM2 F-LS\n", setValue->winchCtrl.sv_WRM_lsrto_F);
fprintf(fp, "%03d #WM2 F-LS\n", setValue->winchCtrl.sv_WRM_lsrto_F);
pc.printf("Writing ... %03d #WM2 R-LS\n", setValue->winchCtrl.sv_WRM_lsrto_R);
fprintf(fp, "%03d #WM2 R-LS\n", setValue->winchCtrl.sv_WRM_lsrto_R);
fclose(fp);
}
else{
rts = false;
pc.printf("File open error (3)\r\n");
}
Thread::wait(50);
fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
if( fp != NULL ){
pc.printf("Writing ... %04d #WD D-DMx100\n", setValue->winchCtrl.sv_WRS_DramDmrx100);
fprintf(fp, "%04d #WD D-DMx100\n", setValue->winchCtrl.sv_WRS_DramDmrx100);
pc.printf("Writing ... %04d #WCC Dx100\n", setValue->winchCtrl.sv_WRS_CCableDmrx100);
fprintf(fp, "%04d #WCC Dx100\n", setValue->winchCtrl.sv_WRS_CCableDmrx100);
pc.printf("Writing ... %03d #R-Res\n", setValue->winchCtrl.sv_WRS_RResolution);
fprintf(fp, "%03d #R-Res\n", setValue->winchCtrl.sv_WRS_RResolution);
pc.printf("Writing ... %03d #Res\n", setValue->winchCtrl.reserved);
fprintf(fp, "%03d #res\n", setValue->winchCtrl.reserved);
fclose(fp);
}
else{
rts = false;
pc.printf("File open error (4)\r\n");
}
Thread::wait(50);
fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
if( fp != NULL ){
pc.printf("Writing ... %04d #R-TFM F-C\n", setValue->tfmCtrl.sv_RFTM_ith_F);
fprintf(fp, "%04d #R-TFM F-C\n", setValue->tfmCtrl.sv_RFTM_ith_F);
pc.printf("Writing ... %04d #R-TFM R-C\n", setValue->tfmCtrl.sv_RFTM_ith_R);
fprintf(fp, "%04d #R-TFM R-C\n", setValue->tfmCtrl.sv_RFTM_ith_R);
pc.printf("Writing ... %04d #L-TFM F-C\n", setValue->tfmCtrl.sv_LBTM_ith_F);;
fprintf(fp, "%04d #L-TFM F-C\n", setValue->tfmCtrl.sv_LBTM_ith_F);;
pc.printf("Writing ... %04d #L-TFM R-C\n", setValue->tfmCtrl.sv_LBTM_ith_R);
fprintf(fp, "%04d #L-TFM R-C\n", setValue->tfmCtrl.sv_LBTM_ith_R);
fclose(fp);
}
else{
rts = false;
pc.printf("File open error (5)\r\n");
}
Thread::wait(50);
fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
if( fp != NULL ){
pc.printf("Writing ... %03d #R-TFM F-S\n", setValue->tfmCtrl.sv_RFTM_srto_F);
fprintf(fp, "%03d #R-TFM F-S\n", setValue->tfmCtrl.sv_RFTM_srto_F);
pc.printf("Writing ... %03d #R-TFM R-S\n", setValue->tfmCtrl.sv_RFTM_srto_R);
fprintf(fp, "%03d #R-TFM R-S\n", setValue->tfmCtrl.sv_RFTM_srto_R);
pc.printf("Writing ... %03d #L-TFM F-S\n", setValue->tfmCtrl.sv_LBTM_srto_F);
fprintf(fp, "%03d #L-TFM F-S\n", setValue->tfmCtrl.sv_LBTM_srto_F);
pc.printf("Writing ... %03d #L-TFM R-S\n", setValue->tfmCtrl.sv_LBTM_srto_R);
fprintf(fp, "%03d #L-TFM R-S\n", setValue->tfmCtrl.sv_LBTM_srto_R);
fclose(fp);
}
else{
rts = false;
pc.printf("File open error (6)\r\n");
}
Thread::wait(50);
fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
if( fp != NULL ){
pc.printf("Writing ... %04d #RC F-C th\n", setValue->crawlerCtrl.sv_RFCM_ith_F);
fprintf(fp, "%04d #RC F-C th\n", setValue->crawlerCtrl.sv_RFCM_ith_F);
pc.printf("Writing ... %04d #RC R-C th\n", setValue->crawlerCtrl.sv_RFCM_ith_R);
fprintf(fp, "%04d #RC R-C th\n", setValue->crawlerCtrl.sv_RFCM_ith_R);
pc.printf("Writing ... %04d #LC F-C th\n", setValue->crawlerCtrl.sv_LBCM_ith_F);
fprintf(fp, "%04d #LC F-C th\n", setValue->crawlerCtrl.sv_LBCM_ith_F);
pc.printf("Writing ... %04d #LC R-C th\n", setValue->crawlerCtrl.sv_LBCM_ith_R);
fprintf(fp, "%04d #LC R-C th\n", setValue->crawlerCtrl.sv_LBCM_ith_R);
fclose(fp);
}
else{
rts = false;
pc.printf("File open error (7)\r\n");
}
Thread::wait(50);
fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
if( fp != NULL ){
pc.printf("Writing ... %03d #RC F-S\n", setValue->crawlerCtrl.sv_RFCM_srto_F);
fprintf(fp, "%03d #RC F-S\n", setValue->crawlerCtrl.sv_RFCM_srto_F);
pc.printf("Writing ... %03d #RC R-S\n", setValue->crawlerCtrl.sv_RFCM_srto_R);
fprintf(fp, "%03d #RC R-S\n", setValue->crawlerCtrl.sv_RFCM_srto_R);
pc.printf("Writing ... %03d #LC F-S\n", setValue->crawlerCtrl.sv_LBCM_srto_F);
fprintf(fp, "%03d #LC F-S\n", setValue->crawlerCtrl.sv_LBCM_srto_F);
pc.printf("Writing ... %03d #LC R-S\n", setValue->crawlerCtrl.sv_LBCM_srto_R);
fprintf(fp, "%03d #LC R-S\n", setValue->crawlerCtrl.sv_LBCM_srto_R);
fclose(fp);
}
else{
rts = false;
pc.printf("File open error (8)\r\n");
}
fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
if( fp != NULL ){
pc.printf("Writing ... %03d #RJS U\n", setValue->crawlerCtrl.sv_RFCM_dzu);
fprintf(fp, "%03d #RJS U\n", setValue->crawlerCtrl.sv_RFCM_dzu);
pc.printf("Writing ... %03d #RJS C\n", setValue->crawlerCtrl.sv_RFCM_dzc);
fprintf(fp, "%03d #RJS C\n", setValue->crawlerCtrl.sv_RFCM_dzc);
pc.printf("Writing ... %03d #RJS L\n", setValue->crawlerCtrl.sv_RFCM_dzl);
fprintf(fp, "%03d #RJS L\n", setValue->crawlerCtrl.sv_RFCM_dzl);
pc.printf("Writing ... %03d #LJS U\n", setValue->crawlerCtrl.sv_LBCM_dzu);
fprintf(fp, "%03d #LJS U\n", setValue->crawlerCtrl.sv_LBCM_dzu);
fclose(fp);
}
else{
rts = false;
pc.printf("File open error (9)\r\n");
}
fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
if( fp != NULL ){
pc.printf("Writing ... %03d #LJS C\n", setValue->crawlerCtrl.sv_LBCM_dzc);
fprintf(fp, "%03d #LJS C\n", setValue->crawlerCtrl.sv_LBCM_dzc);
pc.printf("Writing ... %03d #LJS L\n", setValue->crawlerCtrl.sv_LBCM_dzl);
fprintf(fp, "%03d #LJS L\n", setValue->crawlerCtrl.sv_LBCM_dzl);
pc.printf("Writing ... %03d #res\n", setValue->crawlerCtrl.reserved1);
fprintf(fp, "%03d #res\n", setValue->crawlerCtrl.reserved1);
pc.printf("Writing ... %03d #res\n", setValue->crawlerCtrl.reserved2);
fprintf(fp, "%03d #res\n", setValue->crawlerCtrl.reserved2);
fclose(fp);
}
else{
rts = false;
pc.printf("File open error (10)\r\n");
}
pc.printf("settig file write completed \r\n ");
return rts;
}
// ======================================================================
// Read Network setting value from lpcal file system of mbed
// ======================================================================
int read_NetSetting_lfs( char* ip_address, char* subnet_mask, char* gateway )
{
FILE *rfp;
DEBUG_PRINT_L3("Bd0> Read Network Setting data from local file system \r\n");
rfp = fopen("/local/net.txt", "r"); // Open local file "set.txt" for writing
if(!rfp){
DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
return _FAIL_;
}
else{
Thread::wait(50);
fscanf(rfp, "%s", ip_address);
fscanf(rfp, "%s", subnet_mask);
fscanf(rfp, "%s", gateway);
fclose(rfp);
return _OK_;
}
}
// ======================================================================
// Winch control function
// ======================================================================
void winchMovingControl(
int mode, // Operationg mode: Relative / Abslute
char* dbufferP, // Date buffer pointer
int dbuffer_s, // Date buffer size
winchData_t* winchDataP, // Winch data structure pointer
int winchData_s, // Winch data structure size
char* I2C_cmd
){
int rcv_data_cnt;
// int moving_data;
int man_speed;
int cnt;
bool flg_stop_operation = false;
int16_t winchTempPosition;
char I2C_read[NumberOfI2CCommand+1];
char I2C_readcmd[NumberOfI2CCommand+1];
// if (hwbtn_Opeflg == 1){
// Thread::wait(1);
// }
// else{
if( flg_ButtonOn == true ) {Thread::wait(2);}
I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_F;
I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_R;
I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_F;
I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_R;
if( mode == WINCH_POSITION_CLEAR ){
led3 = ON;
DEBUG_PRINT_L3("Bd0> === WINCH_POSITION_CLEAR ===\r\n");
rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
if( rcv_data_cnt < 0 ){
DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
// tcp_client.send( (char*)winchDataP, winchData_s);
// break;
}
else{
DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
// if( !strcmp( dbuffer, "WinchStepDnOf" ) ){
// break;
// }
}
I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Zero clear
i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
led3 = OFF;
}
else if (( mode == WINCH_MMODE_RELATIVE ) || ( mode == WINCH_MMODE_ABSOLUTE )){
if ( mode == WINCH_MMODE_RELATIVE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_RELATIVE === \r\n");
if ( mode == WINCH_MMODE_ABSOLUTE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_ABSOLUTE === \r\n");
rcv_data_cnt = tcp_client.receive( dbuffer, dbuffer_s);
*(dbufferP+rcv_data_cnt) = '\0';
winchDataP->operation = '\r\n';
DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
// Copy received data to Winch data structure.
memcpy( winchDataP, (winchData_t *)dbuffer, winchData_s);
// winchDataP->dt_WinchDstPosition += winchDataP->dt_WinchCntPosition;
DEBUG_PRINT_L3("Bd0> Winch Rtv Move [ From %04d >>> To %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
swbtn_OpeMutex.lock();
swbtn_Opeflg = 1;
swbtn_OpeMutex.unlock();
cnt = 0;
while( true ){
while( true ){
led3 = ON;
////winchDataP->dt_WinchCntPosition = res_position; // Current position.
DEBUG_PRINT_L3("Bd0> == Winch Position ==============\r\n");
DEBUG_PRINT_L3("Bd0> CURRENT : %d\r\n", winchDataP->dt_WinchCntPosition );
DEBUG_PRINT_L3("Bd0> DESTINATION: %d\r\n", winchDataP->dt_WinchDstPosition );
DEBUG_PRINT_L3("Bd0> ================================\r\n");
tcp_client.send( (char*)winchDataP, winchData_s);
DEBUG_PRINT_L3("Bd0> Send Winch Rtv data [ %04d >>>> %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
if( rcv_data_cnt < 0 ){
DEBUG_PRINT_L3("##ERROR## Data receive\r\n" );
// tcp_client.send( (char*)winchDataP, winchData_s);
break;
}
else{
DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
if( !strcmp( dbuffer, "WinchRtvStop" ) ){
flg_stop_operation = true;
break;
}
}
// Forward rotation : winch down
if( winchDataP->dt_WinchCntPosition < winchDataP->dt_WinchDstPosition ){
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
I2C_cmd[I2C_CP_M1_SPEED] = (setValue.winchCtrl.sv_WDM_lsrto_F >> 1); // very slow speed
}
else if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_lsrto_F; // slow speed
}
else{
I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_F; // normal speed
}
}
// Reverse rotation : winch up
else if ( winchDataP->dt_WinchCntPosition > winchDataP->dt_WinchDstPosition ){
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
I2C_cmd[I2C_CP_M1_SPEED] = (setValue.winchCtrl.sv_WDM_lsrto_R >> 1); // very slow speed
}
else if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_lsrto_R; // slow speed
}
else{
I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_R; // normal speed
}
}
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
// Read winch current position from Resolver.
winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
if( winchTempPosition != -1 ){
mtx_wcp.lock();
winchCurrentPosition = winchTempPosition;
mtx_wcp.unlock();
}
mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
mtx_wcp.unlock();
winchDataP->operation = 0x00;
//i2c.read(I2C_ADDRESS_RESOLVER, I2C_resdata, 2); // Read
//res_position = (I2C_resdata[1] << 8) | I2C_resdata[0];
// --------------------------------------
// Read motor current
// --------------------------------------
read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
winchDataP->dt_WinchMotor1Current = I2C_read[0]; // Motor current set
winchDataP->dt_WinchMotor2Current = I2C_read[1]; // Motor current set
winchDataP->operation = I2C_read[2];
DEBUG_PRINT_L3("Bd0> 15: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
led3 = OFF;
if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) == 0 ){
// if( winchDataP->dt_WinchCntPosition == winchDataP->dt_WinchDstPosition ){
DEBUG_PRINT_L3( "Bd0> Current:%d -> Destination:%d\r\n" , winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition);
break;
}
Thread::wait(2); // Time interval for program debugging
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
}
DEBUG_PRINT_L3( "Bd0> ! Winch Stop\r\n" );
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 STOP
I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 STOP
I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
// Thread::wait(1000); // Time interval for program debugging
winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
if( winchTempPosition != -1 ){
mtx_wcp.lock();
winchCurrentPosition = winchTempPosition;
mtx_wcp.unlock();
}
mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
mtx_wcp.unlock();
winchDataP->operation = 0x00;
DEBUG_PRINT_L3("Bd0> Check destination is same to setting point or not: %d\r\n", winchCurrentPosition);
if( winchDataP->dt_WinchDstPosition == winchCurrentPosition ){
cnt++;
DEBUG_PRINT_L3("Bd0> Destination is same to setting point, then stop operation\r\n" );
winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
if( winchTempPosition != -1 ){
mtx_wcp.lock();
winchCurrentPosition = winchTempPosition;
mtx_wcp.unlock();
}
if ( cnt >= 5 ){
break; // When final destination == set point , then break. else adjust position again.
}
}
// Force Stop by Stop button
if( flg_stop_operation == true ){
DEBUG_PRINT_L3("Bd0> Winch auto operation force stop\r\n" );
flg_stop_operation = false;
break;
}
}
/*
Thread::wait(300); // Time interval for program debugging
winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
if( winchTempPosition != -1 ){
mtx_wcp.lock();
winchCurrentPosition = winchTempPosition;
mtx_wcp.unlock();
}
mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
mtx_wcp.unlock();
winchDataP->operation = 0x00;
DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
tcp_client.send( (char*)winchDataP, winchData_s);
Thread::wait(300); // Time interval for program debugging
winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
if( winchTempPosition != -1 ){
mtx_wcp.lock();
winchCurrentPosition = winchTempPosition;
mtx_wcp.unlock();
}
mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
mtx_wcp.unlock();
winchDataP->operation = 0x00;
DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
tcp_client.send( (char*)winchDataP, winchData_s);
*/
Thread::wait(300); // Time interval for program debugging
winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
if( winchTempPosition != -1 ){
mtx_wcp.lock();
winchCurrentPosition = winchTempPosition;
mtx_wcp.unlock();
}
mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
mtx_wcp.unlock();
winchDataP->operation = 0x77;
DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
tcp_client.send( (char*)winchDataP, winchData_s);
swbtn_OpeMutex.lock();
swbtn_Opeflg = 0;
swbtn_OpeMutex.unlock();
led3 = OFF;
}
// ----------------------------------------------------------
// In case of commad received from PC by TCP connection.
// In case of hard ware button pushed is by gamepad task
// ----------------------------------------------------------
else if (( mode == WINCH_STEPDOWN_BTN_ON )||( mode == WINCH_U_STEPDOWN_BTN_ON )) {
if ( mode == WINCH_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPDOWN_BTN_ON ===\r\n" );
if ( mode == WINCH_U_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPDOWN_BTN_ON ===\r\n" );
swbtn_OpeMutex.lock();
swbtn_Opeflg = 1;
swbtn_OpeMutex.unlock();
while( 1 ){
led3 = ON;
DEBUG_PRINT_L3("Bd0> WINCH_STEPDOWN_BTN_ON\r\n");
rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
if( rcv_data_cnt < 0 ){
DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
// tcp_client.send( (char*)winchDataP, winchData_s);
break;
}
else{
DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
if(( !strcmp( dbuffer, "WinchStepDnOf" ))||( !strcmp( dbuffer, "WinchuStepDnOf" )) ){
break;
}
}
if ( mode == WINCH_U_STEPDOWN_BTN_ON ) man_speed = 50;
else man_speed = 100;
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
I2C_cmd[I2C_CP_M1_SPEED] = man_speed; // Speed
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
led3 = OFF;
read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
winchDataP->dt_WinchMotor1Current = I2C_read[0];
winchDataP->dt_WinchMotor2Current = I2C_read[1];
winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
DEBUG_PRINT_L3( "Bd0> 16: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
if( winchDataP->operation == 0x88 ){
winchDataP->dt_WinchMotor1Current = 0xFF;
}
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
if( winchTempPosition != -1 ){
mtx_wcp.lock();
winchCurrentPosition = winchTempPosition;
mtx_wcp.unlock();
}
mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition;
mtx_wcp.unlock();
winchDataP->operation = 0x00;
// ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x00 );
tcp_client.send( (char*)winchDataP, winchData_s);
DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
// Thread::wait(2); // Time interval for program debugging
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
}
DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 FWD
I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 FWD
I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
if( winchTempPosition != -1 ){
mtx_wcp.lock();
winchCurrentPosition = winchTempPosition;
mtx_wcp.unlock();
}
mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition;
mtx_wcp.unlock();
winchDataP->operation = 0x77;
//ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x77 );
tcp_client.send( (char*)winchDataP, winchData_s);
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
swbtn_OpeMutex.lock();
swbtn_Opeflg = 0;
swbtn_OpeMutex.unlock();
led3 = OFF;
}
// ----------------------------------------------------------
// In case of commad received from PC by TCP connection.
// In case of hard ware button pushed is by gamepad task
// ----------------------------------------------------------
else if (( mode == WINCH_STEPUP_BTN_ON )||( mode == WINCH_U_STEPUP_BTN_ON )) {
if ( mode == WINCH_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPUP_BTN_ON ===\r\n" );
if ( mode == WINCH_U_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPUP_BTN_ON ===\r\n" );
swbtn_OpeMutex.lock();
swbtn_Opeflg = 1;
swbtn_OpeMutex.unlock();
while( 1 ){
led3 = ON;
DEBUG_PRINT_L3("Bd0> WINCH_STEPUP_BTN_ON\r\n");
rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
if( rcv_data_cnt < 0 ){
DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
// tcp_client.send( (char*)winchDataP, winchData_s);
break;
}
else{
DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
if(
( !strcmp( dbuffer, "WinchStepUpOf" ))||(!strcmp( dbuffer, "WinchuStepUpOf" )) ){
break;
}
}
if ( mode == WINCH_U_STEPUP_BTN_ON ) man_speed = 50;
else man_speed = 100;
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 FWD
I2C_cmd[I2C_CP_M1_SPEED] = man_speed; // Speed
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
led3 = OFF;
read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
winchDataP->dt_WinchMotor1Current = I2C_read[0];
winchDataP->dt_WinchMotor2Current = I2C_read[1];
winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
DEBUG_PRINT_L3( "Bd0> 17: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
if( winchDataP->operation == 0x88 ){
winchDataP->dt_WinchMotor1Current = 0xFF;
}
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
if( winchTempPosition != -1 ){
mtx_wcp.lock();
winchCurrentPosition = winchTempPosition;
mtx_wcp.unlock();
}
mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition;
mtx_wcp.unlock();
winchDataP->operation = 0x00;
// ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x00 );
tcp_client.send( (char*)winchDataP, winchData_s);
DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
// Thread::wait(2); // Time interval for program debugging
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
}
DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 FWD
I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed
I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 FWD
I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
if( flg_ButtonOn == false ){
winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
if( winchTempPosition != -1 ){
mtx_wcp.lock();
winchCurrentPosition = winchTempPosition;
mtx_wcp.unlock();
}
mtx_wcp.lock();
winchDataP->dt_WinchCntPosition = winchCurrentPosition;
mtx_wcp.unlock();
winchDataP->operation = 0x77;
// ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x77 );
tcp_client.send( (char*)winchDataP, winchData_s);
}
i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
swbtn_OpeMutex.lock();
swbtn_Opeflg = 0;
swbtn_OpeMutex.unlock();
led3 = OFF;
}
else {
// DEBUG_PRINT_L3("STEPSTEPSTEPSTEPSTEP\r\n");
}
// }
}
// **********************************************************************
//
// Main Function of this program
//
// **********************************************************************
int main()
{
Mutex file_access_mutex;
setValue_t new_setValue; // Setting Data
winchData_t winchData;
char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
// char I2C_res[NumberOfI2CCommand+1] = "\0";
char ip_address[20];
char subnet_mask[20];
char gateway[20];
// char* ip_address;
// char* subnet_mask;
// char* gateway;
int ret;
int try_cnt;
int rcv_data_cnt;
bool flg_ethernet = false;
char ttt[20];
// Set UART(USB) baudrate
pc.baud(115200);
cf_led_demo( &led1, &led2, &led3, &led4, 10, 15 );
DEBUG_PRINT_L0("\r\n");
DEBUG_PRINT_L0("Bd0> +--------------------------------------------------------------\r\n");
DEBUG_PRINT_L0("Bd0> | Project: B2 Crawler Explorer for 1F-1 PCV internal inspection\r\n");
DEBUG_PRINT_L0("Bd0> |---------\r\n");
DEBUG_PRINT_L0("Bd0> | This is: Main Control Program of Main Controller\r\n");
DEBUG_PRINT_L0("Bd0> | Target MCU: mbed LPC1768\r\n");
DEBUG_PRINT_L0("Bd0> | Letest update: %s\r\n", LatestUpDate);
DEBUG_PRINT_L0("Bd0> | Program Revision: %s\r\n", ProgramRevision);
DEBUG_PRINT_L0("Bd0> | Author: %s\r\n", Author);
DEBUG_PRINT_L0("Bd0> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
DEBUG_PRINT_L0("Bd0> ---------------------------------------------------------------\r\n");
sprintf( ttt, "%s", ProgramRevision );
DEBUG_PRINT_L0("Bd0> Start ststem initializing ...\r\n");
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
DEBUG_PRINT_L0("Bd0> 1. Initalizing Ethernet ...\r\n");
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
read_NetSetting_lfs( ip_address, subnet_mask, gateway );
// ip_address = "192.168.3.24";
// subnet_mask = "255.255.255.0";
// gateway = "192.168.3.1";
DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
DEBUG_PRINT_L0("Bd0> ip address : %s\r\n", ip_address);
DEBUG_PRINT_L0("Bd0> subnet mask : %s\r\n", subnet_mask);
DEBUG_PRINT_L0("Bd0> default gateway: %s\r\n", gateway);
DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
#ifdef __ETERNET_DHCP__
ret = eth.init(); // Use DHCP
#else // __ETERNET_DHCP__
ret = eth.init(
ip_address, // const char* ip,
subnet_mask, // const char* mask,
gateway // const char* gateway
);
#endif // __ETERNET_DHCP__
if( ret == 0 ){
DEBUG_PRINT_L0("Bd0> Eternet init ... OK\r\n");
ret = eth.connect();
if( ret == 0 ){
cf_led_onoff( &led1,&led2,&led3,&led4, false, false, false, true );
DEBUG_PRINT_L0("Bd0> Eternat connect ... OK\r\n");
DEBUG_PRINT_L0("Bd0> [ IP Address : %s ]\r\n", eth.getIPAddress());
udp_server.bind(UDP_SERVER_PORT);
tcp_server.bind(TCP_SERVER_PORT);
tcp_server.listen();
flg_ethernet = true;
led4 = ON; // Ethernet OK
}
else{
cf_led_error( &led1,&led2,&led3,&led4 );
DEBUG_PRINT_L0("Bd0> ***ERROR*** Eternat connect Fali\r\n");
}
}
else{
DEBUG_PRINT_L0("Bd0> *** ERROR*** Eternat init Fail\r\n");
}
Thread::wait(50);
//---------------------------------------------------
// Read CrExp setting value from Local File System
// setting file "SET.DAT".
// When error occured, LED1 will be blinking shortly.
//---------------------------------------------------
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
DEBUG_PRINT_L0("Bd0> 2. Read setting value from LFS\r\n");
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
#ifdef __CREATE_SETTING_FILE__
write_LFS(&setValue); // Create and set setting file.
#endif // __CREATE_SETTING_FILE__
// --------------------------------------------------------------------
// Read setting from local file system and set to internal structure
// --------------------------------------------------------------------
try_cnt = LFS_READ_COUNT;
while( 1 ){
if( read_LFS(&setValue) == true ) break;
else try_cnt -= 1;
if( try_cnt == 0 ){
DEBUG_PRINT_L0("Bd0> ***ERROR*** LFS read error\r\n");
while(1){
led1 = !led1;
Thread::wait(30);
}
}
}
DEBUG_PRINT_L0("Bd0> LFS read OK\r\n");
led3 = ON; // Setting Data Read OK
#ifdef __TARGET_BOARD_CHECK__
//---------------------------------------------------
// Checking Targer LCXpresso824-MAX board here.
// Send Hello Packet and waiting reply message from
// target.
// When error occured, LED1 will blinking slowly.
//---------------------------------------------------
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
DEBUG_PRINT_L0("Vd0> 3. Check the target controler\r\n");
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
try_cnt = TARGET_CHECK_COUNT;
while(1){
// Check each target motor control 824 board here
i2c.read(I2C_ADDRESS_WINCH, I2C_res, NumberOfI2CCommand);
if( I2C_res[4] == 'S' ){
DEBUG_PRINT_L0("Bd0> Try count : %02d/%02d\r\n", TARGET_CHECK_COUNT - try_cnt, TARGET_CHECK_COUNT );
DEBUG_PRINT_L0("Bd0> Return from (0x%02x) : '%c'\r\n", I2C_ADDRESS_WINCH, I2C_res[4]);
break;
}
else try_cnt -= 1;
if( try_cnt == 0 ){
DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_WINCH);
led1 = OFF;
while(1){
led1 = !led1; // ON
Thread::wait(80);
}
}
}
try_cnt = TARGET_CHECK_COUNT;
while(1){
// Check each target motor control 824 board here
i2c.read(I2C_ADDRESS_TRANSFORM, I2C_res, NumberOfI2CCommand);
if( I2C_res[4] == 'S' ){
DEBUG_PRINT_L0("Bd0> Try count: %02d/%02d\r\n", TARGET_CHECK_COUNT - try_cnt, TARGET_CHECK_COUNT );
DEBUG_PRINT_L0("Bd0> Return from (0x%02x): '%c'\r\n", I2C_ADDRESS_TRANSFORM, I2C_res[4]);
break;
}
else try_cnt -= 1;
if( try_cnt == 0 ){
DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_TRANSFORM);
led1 = OFF;
while(1){
led2 = !led2; // ON
Thread::wait(80);
}
}
}
try_cnt = TARGET_CHECK_COUNT;
while(1){
// Check each target motor control 824 board here
i2c.read(I2C_ADDRESS_CRAWLER, I2C_res, NumberOfI2CCommand);
if( I2C_res[4] == 'S' ){
DEBUG_PRINT_L0("Bd0> Try count : %02d/%02d\r\n", TARGET_CHECK_COUNT - try_cnt, TARGET_CHECK_COUNT );
DEBUG_PRINT_L0("Bd0> Return from (0x%02x) : '%c'\r\n", I2C_ADDRESS_CRAWLER, I2C_res[4]);
break;
}
else try_cnt -= 1;
if( try_cnt == 0 ){
DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_CRAWLER);
led1 = OFF;
while(1){
led3 = !led3; // ON
Thread::wait(80);
}
}
}
DEBUG_PRINT_L0("Bd0> -------------------\r\n");
DEBUG_PRINT_L0("Bd0> Target system found\r\n");
DEBUG_PRINT_L0("Bd0> -------------------\r\n");
#endif // __TARGET_BOARD_CHECK__
led2 = ON; // Check target OK
/* Set basic function default setting */
baseOperation.sv_JS_OpeMode = 0;
baseOperation.sv_JS_OpeMode = 0;
baseOperation.sv_WinchValid = 0;
/*
**************************************************
Send Calculation Data to Resolver Controller
**************************************************
*/
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
DEBUG_PRINT_L0("Bd0> 4. Send the Calculation base data to Resolver Controller");
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
I2C_cmd[I2C_CP_WDRAM_DIA_UPPER] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100>>8)&0xFF); // Dram diameter upper
I2C_cmd[I2C_CP_WDRAM_DIA_LOWER] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100)&0xFF); // Dram diameter lower
I2C_cmd[I2C_CP_CCABLE_DIA_UPPER] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100>>8)&0xFF); // Cable diameter upper
I2C_cmd[I2C_CP_CCABLE_DIA_LOWER] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100)&0xFF); // Cable diameter lower
I2C_cmd[I2C_CP_RESOLVER_RESO] = setValue.winchCtrl.sv_WRS_RResolution; // Resolver resolution
for( int j = 0; j < NumberOfI2CCommand; j++)
DEBUG_PRINT_L0("%02x ", I2C_cmd[j]);
DEBUG_PRINT_L0( "\r\n" );
i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
DEBUG_PRINT_L0(" ... done\r\n");
// Thread (
// void(*task)(void const *argument),
// void *argument=NULL,
// osPriority priority=osPriorityNormal,
// uint32_t stack_size=DEFAULT_STACK_SIZE,
// unsigned char *stack_pointer=NULL
//)
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
DEBUG_PRINT_L0("Bd0> 5. Start the task\r\n");
/* Max thread count is (may be ..) 2, How can I increase this , I don't know ?? */
DEBUG_PRINT_L0("Bd0> Start host interface task ... ");
// Thread thread_hif(clientPC_interface_task, NULL, osPriorityHigh, 128*4);
Thread thread_hif(clientPC_interface_task, NULL, osPriorityNormal, 128*4);
DEBUG_PRINT_L0("\r\nBd0> Start host gamepad task ... ");
Thread thread_gpd(gamepad_task, NULL, osPriorityNormal, 128*4);
DEBUG_PRINT_L0("\r\n");
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
DEBUG_PRINT_L0( "Bd0> ----------------------------------\r\n");
DEBUG_PRINT_L0( "Bd0> >>>> Initializing completed ! <<<<\r\n");
DEBUG_PRINT_L0( "Bd0> ----------------------------------\r\n");
led4 = OFF;
led3 = OFF;
led2 = OFF;
led1 = ON; // Initializing is OK then Power Indicator LED ON
I2C_cmd[4] = 0x00;
I2C_cmd[5] = 0x01;
i2c.write(I2C_ADDRESS_HANDY, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
while( true ) {
Thread::wait(10);
// -----------------------------------------------------------------
// Communicate with client PC program.
// TCP connection:
// -----------------------------------------------------------------
if( flg_ethernet == true ) // in case of Ethernet OK
{
tcp_server.accept(tcp_client);
tcp_client.set_blocking(false, 3500); // Timeout after (300) msec
DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
DEBUG_PRINT_L3("Bd0> TCP Connection from: %s\r\n", tcp_client.get_address());
DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
while(true){
// --------------------------------------------------------------
// Following instructions are blocking when no ethernat access
// --------------------------------------------------------------
rcv_data_cnt = tcp_client.receive(dbuffer, sizeof(dbuffer));
DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
if( rcv_data_cnt < 0 ){
// DEBUG_PRINT("## TCP Receive packet fail ##\r\n");
break;
}
else{
if( !strcmp( dbuffer, "WinchPositionClear" ) ){
winchMovingControl(
WINCH_POSITION_CLEAR,
dbuffer,
sizeof(dbuffer),
&winchData,
sizeof( winchData ),
I2C_cmd
);
}
else if( !strcmp( dbuffer, "WinchRtvStart" ) ){
winchMovingControl(
WINCH_MMODE_RELATIVE,
dbuffer,
sizeof(dbuffer),
&winchData,
sizeof( winchData ),
I2C_cmd
);
}
else if( !strcmp( dbuffer, "WinchAbsStart" ) ){
winchMovingControl(
WINCH_MMODE_ABSOLUTE,
dbuffer,
sizeof(dbuffer),
&winchData,
sizeof( winchData ),
I2C_cmd
);
}
else if( !strcmp( dbuffer, "WinchStepUpOn" ) ){
winchMovingControl(
WINCH_STEPUP_BTN_ON,
dbuffer,
sizeof(dbuffer),
&winchData,
sizeof( winchData ),
I2C_cmd
);
}
else if( !strcmp( dbuffer, "WinchStepUpOf" ) ){
winchMovingControl(
WINCH_STEPUP_BTN_OFF,
dbuffer,
sizeof(dbuffer),
&winchData,
sizeof( winchData ),
I2C_cmd
);
}
else if( !strcmp( dbuffer, "WinchStepDnOn" ) ){
winchMovingControl(
WINCH_STEPDOWN_BTN_ON,
dbuffer,
sizeof(dbuffer),
&winchData,
sizeof( winchData ),
I2C_cmd
);
}
else if( !strcmp( dbuffer, "WinchStepDnOf" ) ){
winchMovingControl(
WINCH_STEPDOWN_BTN_OFF,
dbuffer,
sizeof(dbuffer),
&winchData,
sizeof( winchData ),
I2C_cmd
);
}
else if( !strcmp( dbuffer, "WinchuStepUpOn" ) ){
winchMovingControl(
WINCH_U_STEPUP_BTN_ON,
dbuffer,
sizeof(dbuffer),
&winchData,
sizeof( winchData ),
I2C_cmd
);
}
else if( !strcmp( dbuffer, "WinchuStepUpOf" ) ){
winchMovingControl(
WINCH_U_STEPUP_BTN_OFF,
dbuffer,
sizeof(dbuffer),
&winchData,
sizeof( winchData ),
I2C_cmd
);
}
else if( !strcmp( dbuffer, "WinchuStepDnOn" ) ){
winchMovingControl(
WINCH_U_STEPDOWN_BTN_ON,
dbuffer,
sizeof(dbuffer),
&winchData,
sizeof( winchData ),
I2C_cmd
);
}
else if( !strcmp( dbuffer, "WinchuStepDnOf" ) ){
winchMovingControl(
WINCH_U_STEPDOWN_BTN_OFF,
dbuffer,
sizeof(dbuffer),
&winchData,
sizeof( winchData ),
I2C_cmd
);
}
else if( !strcmp( dbuffer, "SetValue" ) ){
DEBUG_PRINT_L3("Bd0> SetValue Request from client\r\n");
Thread::wait(200);
rcv_data_cnt = tcp_client.receive( (char*)&new_setValue, sizeof(new_setValue));
Thread::wait(200);
// DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
// dspSetValue2Console( &pc, &new_setValue );
// DEBUG_PRINT_L0("Bd0> write setting file to local file sysytem\r\n");
thread_hif.terminate();
thread_gpd.terminate();
// file_access_mutex.lock();
write_LFS(&new_setValue);
// file_access_mutex.unlock();
Thread::wait(500);
DEBUG_PRINT_L0("Bd0> SetValue instruction is over\r\n");
}
else if(!strcmp( dbuffer, "GetValue" )){
DEBUG_PRINT_L3("Bd0> GetValue request from TCP client\r\n");
flg_mutex.lock(); // This is very important
memcpy( &new_setValue, &setValue, sizeof( new_setValue ) );
flg_mutex.unlock(); // This is very important
dspSetValue2Console( &pc, &new_setValue );
tcp_client.send_all( (char*)&new_setValue, sizeof(new_setValue) );
DEBUG_PRINT_L2("(%d)\r\n", sizeof(new_setValue));
}
}
if( rcv_data_cnt <= 0 ) break;
}
tcp_client.close();
}
}
}