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