Pro_B2_0ZNR_B2DebrisSurvayor_1768LAN - 2018.07.26

Users » sayzyas » Code » Pro_B2_0ZNR_B2DebrisSurvayor_1768LAN

z ysaito / Mbed 2 deprecated Pro_B2_0ZNR_B2DebrisSurvayor_1768LAN

2018.07.26

Dependencies: EthernetInterface TextLCD USBDevice USBHost3 mbed

Fork of USBHostHub_HelloWorld by Samuel Mokrani

0_main.cpp@13:2c70c772fe24, 2016-03-28 (annotated)

Committer:: sayzyas
Date:: Mon Mar 28 00:07:19 2016 +0000
Revision:: 13:2c70c772fe24
Parent:: 12:3e6b6fcf540b
Child:: 14:3a5ae61ab1f4

Rev20160325

Who changed what in which revision?

User	Revision	Line number	New contents of line
sayzyas	11:ff06edc0219c	1
sayzyas	10:a2bd7d07c7f8	2	/***************************************
sayzyas	10:a2bd7d07c7f8	3	* Project: B2
sayzyas	10:a2bd7d07c7f8	4	* Title: CrExp B2 Motor Ctrl Main
sayzyas	10:a2bd7d07c7f8	5	* Target: LPC1768
sayzyas	10:a2bd7d07c7f8	6	* ------------------------------------
sayzyas	10:a2bd7d07c7f8	7	*
sayzyas	10:a2bd7d07c7f8	8	*
sayzyas	10:a2bd7d07c7f8	9	*
sayzyas	10:a2bd7d07c7f8	10	* mbed LPC1768
sayzyas	10:a2bd7d07c7f8	11	* +-------USB-----+
sayzyas	10:a2bd7d07c7f8	12	* GND \| \| VOUT(3.3V)
sayzyas	10:a2bd7d07c7f8	13	* VIN \| \| VU(5.0V OUT)
sayzyas	10:a2bd7d07c7f8	14	* VB \| \| IF-
sayzyas	10:a2bd7d07c7f8	15	* mR \| # ### # ### \| IF+
sayzyas	10:a2bd7d07c7f8	16	* p5 mosi \| # # # # # # \| Ether RD-
sayzyas	10:a2bd7d07c7f8	17	* p6 miso \| # # ### ### \| Ether RD+
sayzyas	10:a2bd7d07c7f8	18	* p7 sck \| # # # # # # \| Ether TD-
sayzyas	10:a2bd7d07c7f8	19	* p8 \| # # ### ### \| Ether TD+
sayzyas	10:a2bd7d07c7f8	20	* p9 tx sdi \| \| USB D-
sayzyas	10:a2bd7d07c7f8	21	* p10 rx scl \| \| USB D+
sayzyas	10:a2bd7d07c7f8	22	* p11 mosi \| \| CAN rd p30
sayzyas	10:a2bd7d07c7f8	23	* p12 miso \| \| CAN td p29
sayzyas	10:a2bd7d07c7f8	24	* p13 tx sck \| \| sda tx p28
sayzyas	10:a2bd7d07c7f8	25	* p14 rx \| \| scl rx P27
sayzyas	10:a2bd7d07c7f8	26	* p15 AIn \| \| PWM P26
sayzyas	10:a2bd7d07c7f8	27	* p16 AIn \| \| PWM P25
sayzyas	10:a2bd7d07c7f8	28	* p16 AIn \| \| PWM p24
sayzyas	10:a2bd7d07c7f8	29	* p18 AIn AOut \| \| PWM p23
sayzyas	10:a2bd7d07c7f8	30	* p19 AIn \| \| PWM p22
sayzyas	10:a2bd7d07c7f8	31	* p20 AIn \| \| PWM p21
sayzyas	10:a2bd7d07c7f8	32	* +---------------+
sayzyas	10:a2bd7d07c7f8	33	*
sayzyas	10:a2bd7d07c7f8	34	***************************************/
sayzyas	10:a2bd7d07c7f8	35	#include "mbed.h"
sayzyas	10:a2bd7d07c7f8	36	#include "USBHostGamepad.h"
sayzyas	10:a2bd7d07c7f8	37	#include "USBSerial.h"
sayzyas	10:a2bd7d07c7f8	38	#include "rtos.h"
sayzyas	10:a2bd7d07c7f8	39	#include "EthernetInterface.h"
sayzyas	10:a2bd7d07c7f8	40	#include "common.h"
sayzyas	10:a2bd7d07c7f8	41	#include "stdio.h"
sayzyas	10:a2bd7d07c7f8	42	#include "TextLCD.h"
sayzyas	10:a2bd7d07c7f8	43	#include "com_func.h"
sayzyas	10:a2bd7d07c7f8	44
sayzyas	10:a2bd7d07c7f8	45	// USBSerial serial setting
sayzyas	10:a2bd7d07c7f8	46	Serial pc(USBTX, USBRX); // UART
sayzyas	10:a2bd7d07c7f8	47	// Digital I/O setting
sayzyas	13:2c70c772fe24	48	DigitalOut led1(LED1); // 1:on,0:off System is OK then ON.
sayzyas	13:2c70c772fe24	49	DigitalOut led2(LED2); // 1:on,0:off GamePad is connected.
sayzyas	13:2c70c772fe24	50	DigitalOut led3(LED3); // 1:on,0:off When got the GamePas switch input then ON
sayzyas	13:2c70c772fe24	51	DigitalOut led4(LED4); // 1:on,0:off Access indicator with PC
sayzyas	10:a2bd7d07c7f8	52	// I2C setting
sayzyas	13:2c70c772fe24	53	I2C i2c_res(p28, p27); // I2C SDA, SCL is not good ???
sayzyas	10:a2bd7d07c7f8	54	I2C i2c(p9, p10); // I2C SDA, SCL is good
sayzyas	13:2c70c772fe24	55
sayzyas	10:a2bd7d07c7f8	56	// Ethernet
sayzyas	10:a2bd7d07c7f8	57	EthernetInterface eth;
sayzyas	10:a2bd7d07c7f8	58	TCPSocketServer tcp_server; // TCP Server
sayzyas	10:a2bd7d07c7f8	59	TCPSocketConnection tcp_client;
sayzyas	10:a2bd7d07c7f8	60	UDPSocket udp_server; // UDP Server
sayzyas	10:a2bd7d07c7f8	61	Endpoint client;
sayzyas	10:a2bd7d07c7f8	62	// LCD
sayzyas	10:a2bd7d07c7f8	63	TextLCD lcd(p11, p12, p24, p23, p22, p21); // rs, e, d4-d7
sayzyas	10:a2bd7d07c7f8	64	// Local File System
sayzyas	10:a2bd7d07c7f8	65	LocalFileSystem SettingFile("local"); // Create the local filesystem under the name "local"
sayzyas	10:a2bd7d07c7f8	66
sayzyas	10:a2bd7d07c7f8	67	// Global
sayzyas	13:2c70c772fe24	68	uint32_t flg_gamePad_Connected = 0;
sayzyas	13:2c70c772fe24	69	char PC_cmd[11+1] = "&0100000000";
sayzyas	13:2c70c772fe24	70	basic_operation_t baseOperation;
sayzyas	13:2c70c772fe24	71	setValue_t setValue; // Setting Data
sayzyas	13:2c70c772fe24	72	char dbuffer[128];
sayzyas	13:2c70c772fe24	73	uint8_t motor_speed = 0;
sayzyas	10:a2bd7d07c7f8	74	/* Status flag */
sayzyas	10:a2bd7d07c7f8	75	/*
sayzyas	10:a2bd7d07c7f8	76	0000 0000 : button LI LK RI RK PCW PCCW TU TD
sayzyas	10:a2bd7d07c7f8	77	0000 0000 : limit switch
sayzyas	10:a2bd7d07c7f8	78	0000 0000 : res
sayzyas	10:a2bd7d07c7f8	79	0000 0000 : res
sayzyas	10:a2bd7d07c7f8	80	*/
sayzyas	10:a2bd7d07c7f8	81	uint32_t flg_exp_status = 0;
sayzyas	10:a2bd7d07c7f8	82	Mutex flg_mutex;
sayzyas	10:a2bd7d07c7f8	83	Mutex swbtn_OpeMutex;
sayzyas	10:a2bd7d07c7f8	84	int swbtn_Opeflg = 0;
sayzyas	10:a2bd7d07c7f8	85
sayzyas	13:2c70c772fe24	86	int16_t winchCurrentPosition;
sayzyas	13:2c70c772fe24	87
sayzyas	13:2c70c772fe24	88	Mutex mtx_wcp;
sayzyas	13:2c70c772fe24	89
sayzyas	10:a2bd7d07c7f8	90	bool flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	91
sayzyas	10:a2bd7d07c7f8	92	// Mutex hwbtn_OpeMutex;
sayzyas	10:a2bd7d07c7f8	93	// int hwbtn_Opeflg = 0;
sayzyas	10:a2bd7d07c7f8	94
sayzyas	10:a2bd7d07c7f8	95	int flg_JS_shape_mode = 0;
sayzyas	10:a2bd7d07c7f8	96	int flg_JS_ope_mode = 0;
sayzyas	10:a2bd7d07c7f8	97	uint8_t motor1_current_pct;
sayzyas	10:a2bd7d07c7f8	98	uint8_t motor2_current_pct;
sayzyas	13:2c70c772fe24	99	uint8_t limitSw_Sts = 0;
sayzyas	10:a2bd7d07c7f8	100
sayzyas	10:a2bd7d07c7f8	101
sayzyas	10:a2bd7d07c7f8	102	/* Prototype */
sayzyas	10:a2bd7d07c7f8	103	void write_Setting_lfs(void);
sayzyas	10:a2bd7d07c7f8	104	int read_Setting_lfs(void);
sayzyas	10:a2bd7d07c7f8	105	void dsp_console_setting_value(void);
sayzyas	10:a2bd7d07c7f8	106	void winchMovingControl( int, char, int, winchData_t, int, char* );
sayzyas	10:a2bd7d07c7f8	107	extern void dspSetValue2Console( Serial, setValue_t );
sayzyas	10:a2bd7d07c7f8	108	extern void lcd_out( TextLCD* ,int, int, char* );
sayzyas	10:a2bd7d07c7f8	109
sayzyas	10:a2bd7d07c7f8	110	// ============================================================
sayzyas	10:a2bd7d07c7f8	111	// Read motor current
sayzyas	10:a2bd7d07c7f8	112	// ============================================================
sayzyas	10:a2bd7d07c7f8	113	void read_motorCurrent(
sayzyas	10:a2bd7d07c7f8	114	int i2c_addr,
sayzyas	11:ff06edc0219c	115	char* I2C_readcmd,
sayzyas	10:a2bd7d07c7f8	116	char* I2C_res,
sayzyas	10:a2bd7d07c7f8	117	int NumberOfI2Cdata
sayzyas	10:a2bd7d07c7f8	118	){
sayzyas	11:ff06edc0219c	119	i2c.read(i2c_addr, I2C_res, NumberOfI2Cdata); // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
sayzyas	10:a2bd7d07c7f8	120	/*
sayzyas	11:ff06edc0219c	121	DEBUG_PRINT_L0(" ++++++++++++++++++++++++++++++\r\n" );
sayzyas	11:ff06edc0219c	122	DEBUG_PRINT_L0(" Read Motor1 Current [%d]\r\n", I2C_res[0] );
sayzyas	11:ff06edc0219c	123	DEBUG_PRINT_L0(" Read Motor2 Current [%d]\r\n", I2C_res[1] );
sayzyas	11:ff06edc0219c	124	DEBUG_PRINT_L0(" Read [%d]\r\n", I2C_res[2] );
sayzyas	11:ff06edc0219c	125	DEBUG_PRINT_L0(" ++++++++++++++++++++++++++++++\r\n" );
sayzyas	10:a2bd7d07c7f8	126	*/
sayzyas	10:a2bd7d07c7f8	127	}
sayzyas	10:a2bd7d07c7f8	128
sayzyas	10:a2bd7d07c7f8	129	// ============================================================
sayzyas	10:a2bd7d07c7f8	130	// Send stop command to motor specific controller
sayzyas	10:a2bd7d07c7f8	131	// ============================================================
sayzyas	10:a2bd7d07c7f8	132	void stop_motor( int32_t i2c_addr, char* i2c_cmd, int32_t flg ){
sayzyas	10:a2bd7d07c7f8	133	if (flg == 1 ){
sayzyas	10:a2bd7d07c7f8	134	i2c_cmd[3] = MOTOR_STP;
sayzyas	10:a2bd7d07c7f8	135	i2c_cmd[4] = MOTOR_STP;
sayzyas	10:a2bd7d07c7f8	136	}
sayzyas	10:a2bd7d07c7f8	137	i2c.write(i2c_addr, i2c_cmd, 6 ); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	138	}
sayzyas	10:a2bd7d07c7f8	139
sayzyas	10:a2bd7d07c7f8	140	// ============================================================
sayzyas	10:a2bd7d07c7f8	141	// Send Status to PC
sayzyas	10:a2bd7d07c7f8	142	// ============================================================
sayzyas	10:a2bd7d07c7f8	143	void sendStatus2PC( char *cmd, int32_t numberOfCmd ){
sayzyas	10:a2bd7d07c7f8	144	int i;
sayzyas	10:a2bd7d07c7f8	145	led4 = 1;
sayzyas	10:a2bd7d07c7f8	146	for ( i = 0; i < numberOfCmd; i++ ) {
sayzyas	10:a2bd7d07c7f8	147	pc.putc(*cmd++);
sayzyas	10:a2bd7d07c7f8	148	}
sayzyas	10:a2bd7d07c7f8	149	led4 = 0;
sayzyas	10:a2bd7d07c7f8	150	}
sayzyas	10:a2bd7d07c7f8	151
sayzyas	10:a2bd7d07c7f8	152	// ============================================================
sayzyas	10:a2bd7d07c7f8	153	// Read winch current position from Resolver.
sayzyas	10:a2bd7d07c7f8	154	// ============================================================
sayzyas	10:a2bd7d07c7f8	155	int16_t ReadWinchCurrentPosition( int32_t i2c_addr )
sayzyas	10:a2bd7d07c7f8	156	{
sayzyas	10:a2bd7d07c7f8	157	char I2C_data[2];
sayzyas	11:ff06edc0219c	158	int16_t res_position = 0;
sayzyas	11:ff06edc0219c	159	int rts;
sayzyas	10:a2bd7d07c7f8	160
sayzyas	11:ff06edc0219c	161	rts = i2c.read(i2c_addr, I2C_data, 2); // Read
sayzyas	13:2c70c772fe24	162	mtx_wcp.lock();
sayzyas	11:ff06edc0219c	163	if( rts == 0 ){
sayzyas	11:ff06edc0219c	164	res_position = (I2C_data[1] << 8) \| I2C_data[0];
sayzyas	11:ff06edc0219c	165	}
sayzyas	11:ff06edc0219c	166	else{
sayzyas	13:2c70c772fe24	167	res_position = -1;
sayzyas	11:ff06edc0219c	168	}
sayzyas	11:ff06edc0219c	169	// winchCurrentPosition = res_position;
sayzyas	13:2c70c772fe24	170	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	171	return res_position;
sayzyas	10:a2bd7d07c7f8	172	}
sayzyas	10:a2bd7d07c7f8	173
sayzyas	11:ff06edc0219c	174
sayzyas	11:ff06edc0219c	175	char I2C_res[NumberOfI2CCommand+1] = "\0";
sayzyas	10:a2bd7d07c7f8	176	// ============================================================
sayzyas	10:a2bd7d07c7f8	177	// Button control
sayzyas	10:a2bd7d07c7f8	178	// ============================================================
sayzyas	10:a2bd7d07c7f8	179	void onControl(
sayzyas	10:a2bd7d07c7f8	180	uint8_t btn00, uint8_t btn01, uint8_t btn02, uint8_t btn03,
sayzyas	10:a2bd7d07c7f8	181	uint8_t btn04, uint8_t btn05, uint8_t btn06, uint8_t btn07,
sayzyas	10:a2bd7d07c7f8	182	uint8_t btn08, uint8_t btn09, uint8_t btn10, uint8_t btn11,
sayzyas	10:a2bd7d07c7f8	183	uint8_t btn12, uint8_t btn13, uint8_t btn14, uint8_t btn15,
sayzyas	10:a2bd7d07c7f8	184	uint16_t gamePadVID, uint16_t gamePadPID
sayzyas	10:a2bd7d07c7f8	185	){
sayzyas	10:a2bd7d07c7f8	186	/* OLD type *
sayzyas	10:a2bd7d07c7f8	187	* I2C Command (7byte)
sayzyas	10:a2bd7d07c7f8	188	* 0: '#' // Preamble
sayzyas	10:a2bd7d07c7f8	189	* 1: '0' // Target upper
sayzyas	10:a2bd7d07c7f8	190	* 2: '1' // Target lower
sayzyas	10:a2bd7d07c7f8	191	* 3: '0' // Command 1
sayzyas	10:a2bd7d07c7f8	192	* 4: '1/3' // Command 2
sayzyas	10:a2bd7d07c7f8	193	* 5: '0/1'
sayzyas	10:a2bd7d07c7f8	194	*/
sayzyas	10:a2bd7d07c7f8	195
sayzyas	10:a2bd7d07c7f8	196	/* New Type 15.11.06 ~
sayzyas	10:a2bd7d07c7f8	197	[0] :
sayzyas	10:a2bd7d07c7f8	198	[1] :
sayzyas	10:a2bd7d07c7f8	199	[2] :
sayzyas	10:a2bd7d07c7f8	200	[3] :
sayzyas	10:a2bd7d07c7f8	201	[4] : motor 1 direction (A=Forward, B=Reverse, F=Stop)
sayzyas	10:a2bd7d07c7f8	202	[5] : motor 1 speed
sayzyas	10:a2bd7d07c7f8	203	[6] : motor 2 direction (A=Forward, B=Reverse, F=Stop)
sayzyas	10:a2bd7d07c7f8	204	[7] : motor 2 speed <-- New added
sayzyas	10:a2bd7d07c7f8	205	[8] : N/F
sayzyas	10:a2bd7d07c7f8	206	[9] : N/F
sayzyas	10:a2bd7d07c7f8	207	*/
sayzyas	10:a2bd7d07c7f8	208	char I2C_cmd[NumberOfI2CCommand+1] = "#010000";
sayzyas	11:ff06edc0219c	209	char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
sayzyas	10:a2bd7d07c7f8	210
sayzyas	10:a2bd7d07c7f8	211	uint8_t btnStatus_RFK = 0;
sayzyas	10:a2bd7d07c7f8	212	uint8_t btnStatus_RFI = 0;
sayzyas	10:a2bd7d07c7f8	213	uint8_t btnStatus_LBK = 0;
sayzyas	10:a2bd7d07c7f8	214	uint8_t btnStatus_LBI = 0;
sayzyas	10:a2bd7d07c7f8	215	uint8_t btnStatus_WUP = 0;
sayzyas	10:a2bd7d07c7f8	216	uint8_t btnStatus_WDN = 0;
sayzyas	10:a2bd7d07c7f8	217	uint8_t btnStatus_RJSFwdRvs = 0; // R-JS Fwd/Rvs
sayzyas	10:a2bd7d07c7f8	218	uint8_t btnStatus_RJSLftRgt = 0; // R-JS Left/Light
sayzyas	10:a2bd7d07c7f8	219	uint8_t btnStatus_LJSFwdRvs = 0; // L-JS Fwd/Rvs
sayzyas	10:a2bd7d07c7f8	220	uint8_t btnStatus_LJSLftRgt = 0; // L-Js Left/Right
sayzyas	10:a2bd7d07c7f8	221
sayzyas	10:a2bd7d07c7f8	222	uint8_t btnStatus_Start = 0; // Guide button status for ELECOM GamePad
sayzyas	10:a2bd7d07c7f8	223	uint8_t btnStatus_CrossUp = 0;
sayzyas	10:a2bd7d07c7f8	224	uint8_t btnStatus_CrossDn = 0;
sayzyas	10:a2bd7d07c7f8	225	uint8_t btnStatus_CrossRt = 0;
sayzyas	10:a2bd7d07c7f8	226	uint8_t btnStatus_CrossLt = 0;
sayzyas	10:a2bd7d07c7f8	227
sayzyas	10:a2bd7d07c7f8	228	// For JS Ope mode B
sayzyas	10:a2bd7d07c7f8	229	uint8_t btnID_RFK = 0;
sayzyas	10:a2bd7d07c7f8	230	uint8_t btnID_RFI = 0;
sayzyas	10:a2bd7d07c7f8	231	uint8_t btnID_LBK = 0;
sayzyas	10:a2bd7d07c7f8	232	uint8_t btnID_LBI = 0;
sayzyas	13:2c70c772fe24	233	uint8_t btnID_RFLBI = 0; // RF-I and LB-I both button on same time
sayzyas	13:2c70c772fe24	234	uint8_t btnID_RFLBK = 0; // RF-K and LB-K both button on same time
sayzyas	10:a2bd7d07c7f8	235	uint8_t btnID_WUP = 0;
sayzyas	10:a2bd7d07c7f8	236	uint8_t btnID_WDN = 0;
sayzyas	10:a2bd7d07c7f8	237
sayzyas	10:a2bd7d07c7f8	238	uint8_t btnID_Start = 0; // Guide button ID for ELECOM GamePad
sayzyas	10:a2bd7d07c7f8	239	uint8_t btnID_JS_SD = 0; // JS mode Single / Dual
sayzyas	10:a2bd7d07c7f8	240	uint8_t btnID_JD_IK = 0; // JS mode I-Shape / KO-Shape
sayzyas	10:a2bd7d07c7f8	241
sayzyas	10:a2bd7d07c7f8	242	uint8_t btnID_CrossUp = 0;
sayzyas	10:a2bd7d07c7f8	243	uint8_t btnID_CrossDn = 0;
sayzyas	10:a2bd7d07c7f8	244	uint8_t btnID_CrossRt = 0;
sayzyas	10:a2bd7d07c7f8	245	uint8_t btnID_CrossLt = 0;
sayzyas	10:a2bd7d07c7f8	246
sayzyas	10:a2bd7d07c7f8	247	if( gamePadVID == GAMEPAD_VID_RSTHANDY ){ // NRst own original controller
sayzyas	10:a2bd7d07c7f8	248	DEBUG_PRINT_L4("Bd0> [Rst HDY] ");
sayzyas	10:a2bd7d07c7f8	249	btnID_WDN = 0x10;
sayzyas	10:a2bd7d07c7f8	250	btnID_WUP = 0x20;
sayzyas	10:a2bd7d07c7f8	251	btnID_RFK = 0x01;
sayzyas	10:a2bd7d07c7f8	252	btnID_RFI = 0x02;
sayzyas	10:a2bd7d07c7f8	253	btnID_LBK = 0x04;
sayzyas	10:a2bd7d07c7f8	254	btnID_LBI = 0x08;
sayzyas	10:a2bd7d07c7f8	255	btnID_CrossUp = 0;
sayzyas	10:a2bd7d07c7f8	256	btnID_CrossDn = 4;
sayzyas	10:a2bd7d07c7f8	257	btnID_CrossRt = 2;
sayzyas	10:a2bd7d07c7f8	258	btnID_CrossLt = 6;
sayzyas	13:2c70c772fe24	259	// ---- This is temporary setting ------------------------------
sayzyas	13:2c70c772fe24	260	btnID_RFLBI = 192; // RF-I and LB-I both button on same time
sayzyas	13:2c70c772fe24	261	btnID_RFLBK = 48; // RF-K and LB-K both button on same time
sayzyas	13:2c70c772fe24	262	// --------------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	263	btnID_JS_SD = 1; // JS mode Single or Dual
sayzyas	10:a2bd7d07c7f8	264	btnID_JD_IK = 2; // JS mode I-Shape KO-Shape
sayzyas	10:a2bd7d07c7f8	265	if ( gamePadPID == GAMEPAD_PID_RSTHANDY ){
sayzyas	10:a2bd7d07c7f8	266	btnStatus_WUP = btn04;
sayzyas	10:a2bd7d07c7f8	267	btnStatus_WDN = btn04;
sayzyas	10:a2bd7d07c7f8	268	btnStatus_RFK = btn04;
sayzyas	10:a2bd7d07c7f8	269	btnStatus_RFI = btn04;
sayzyas	10:a2bd7d07c7f8	270	btnStatus_LBK = btn04;
sayzyas	10:a2bd7d07c7f8	271	btnStatus_LBI = btn04;
sayzyas	10:a2bd7d07c7f8	272	btnStatus_RJSFwdRvs = btn03; // Assign analog js to this sw input by LPC1768mbed
sayzyas	10:a2bd7d07c7f8	273	btnStatus_RJSLftRgt = btn02; // Assign analog js to this sw input by LPC1768mbed
sayzyas	10:a2bd7d07c7f8	274	btnStatus_LJSFwdRvs = btn01; // Assign analog js to this sw input by LPC1768mbed
sayzyas	10:a2bd7d07c7f8	275	btnStatus_LJSLftRgt = btn00; // Assign analog js to this sw input by LPC1768mbed
sayzyas	10:a2bd7d07c7f8	276	btnStatus_Start = btn05; //
sayzyas	10:a2bd7d07c7f8	277	btnStatus_CrossUp = btn06;
sayzyas	10:a2bd7d07c7f8	278	btnStatus_CrossDn = btn06;
sayzyas	10:a2bd7d07c7f8	279	btnStatus_CrossRt = btn06;
sayzyas	10:a2bd7d07c7f8	280	btnStatus_CrossLt = btn06;
sayzyas	10:a2bd7d07c7f8	281	}
sayzyas	10:a2bd7d07c7f8	282	}
sayzyas	10:a2bd7d07c7f8	283	else if (gamePadVID == GAMEPAD_VID_ELECOM ){
sayzyas	10:a2bd7d07c7f8	284	DEBUG_PRINT_L4("Bd0> [ELECOM] ");
sayzyas	10:a2bd7d07c7f8	285	btnID_WUP = 2;
sayzyas	10:a2bd7d07c7f8	286	btnID_WDN = 4;
sayzyas	10:a2bd7d07c7f8	287	btnID_RFK = 32;
sayzyas	10:a2bd7d07c7f8	288	btnID_RFI = 128;
sayzyas	10:a2bd7d07c7f8	289	btnID_LBK = 16;
sayzyas	10:a2bd7d07c7f8	290	btnID_LBI = 64;
sayzyas	13:2c70c772fe24	291	// ---------------------
sayzyas	13:2c70c772fe24	292	btnID_RFLBI = 192; // RF-I and LB-I both button on same time
sayzyas	13:2c70c772fe24	293	btnID_RFLBK = 48; // RF-K and LB-K both button on same time
sayzyas	13:2c70c772fe24	294	// ---------------------
sayzyas	10:a2bd7d07c7f8	295	btnID_Start = 8; // Guide button ID for ELECOM GamePad
sayzyas	10:a2bd7d07c7f8	296	btnID_CrossUp = 0;
sayzyas	10:a2bd7d07c7f8	297	btnID_CrossDn = 4;
sayzyas	10:a2bd7d07c7f8	298	btnID_CrossRt = 2;
sayzyas	10:a2bd7d07c7f8	299	btnID_CrossLt = 6;
sayzyas	10:a2bd7d07c7f8	300	if ( gamePadPID == GAMEPAD_PID_ELECOM_JCU3613M ){
sayzyas	10:a2bd7d07c7f8	301	btnStatus_WUP = btn04;
sayzyas	10:a2bd7d07c7f8	302	btnStatus_WDN = btn04;
sayzyas	10:a2bd7d07c7f8	303	btnStatus_RFK = btn04;
sayzyas	10:a2bd7d07c7f8	304	btnStatus_RFI = btn04;
sayzyas	10:a2bd7d07c7f8	305	btnStatus_LBK = btn04;
sayzyas	10:a2bd7d07c7f8	306	btnStatus_LBI = btn04;
sayzyas	10:a2bd7d07c7f8	307	btnStatus_RJSFwdRvs = btn03;
sayzyas	10:a2bd7d07c7f8	308	btnStatus_RJSLftRgt = btn02;
sayzyas	10:a2bd7d07c7f8	309	btnStatus_LJSFwdRvs = btn01;
sayzyas	10:a2bd7d07c7f8	310	btnStatus_LJSLftRgt = btn00;
sayzyas	10:a2bd7d07c7f8	311	btnStatus_Start = btn05; // Guide button status for ELECOM GamePad
sayzyas	10:a2bd7d07c7f8	312	btnStatus_CrossUp = btn06;
sayzyas	10:a2bd7d07c7f8	313	btnStatus_CrossDn = btn06;
sayzyas	10:a2bd7d07c7f8	314	btnStatus_CrossRt = btn06;
sayzyas	10:a2bd7d07c7f8	315	btnStatus_CrossLt = btn06;
sayzyas	10:a2bd7d07c7f8	316	}
sayzyas	10:a2bd7d07c7f8	317	}
sayzyas	10:a2bd7d07c7f8	318	else if( gamePadVID == GAMEPAD_VID_LOGICOOL ){
sayzyas	10:a2bd7d07c7f8	319	btnID_WUP = 136;
sayzyas	10:a2bd7d07c7f8	320	btnID_WDN = 40;
sayzyas	10:a2bd7d07c7f8	321	btnID_RFK = 2;
sayzyas	10:a2bd7d07c7f8	322	btnID_RFI = 8;
sayzyas	10:a2bd7d07c7f8	323	btnID_LBK = 1;
sayzyas	10:a2bd7d07c7f8	324	btnID_LBI = 4;
sayzyas	10:a2bd7d07c7f8	325	btnID_Start = 32; // Guide button ID for ELECOM GamePad
sayzyas	10:a2bd7d07c7f8	326	btnID_CrossUp = 0;
sayzyas	10:a2bd7d07c7f8	327	btnID_CrossDn = 4;
sayzyas	10:a2bd7d07c7f8	328	btnID_CrossRt = 2;
sayzyas	10:a2bd7d07c7f8	329	btnID_CrossLt = 6;
sayzyas	10:a2bd7d07c7f8	330	if ( gamePadPID == GAMEPAD_PID_LOGICOOL_F710 ){
sayzyas	10:a2bd7d07c7f8	331	DEBUG_PRINT_L4("Bd0> [LOGI F710] ");
sayzyas	10:a2bd7d07c7f8	332	btnStatus_WUP = btn05;
sayzyas	10:a2bd7d07c7f8	333	btnStatus_WDN = btn05;
sayzyas	10:a2bd7d07c7f8	334	btnStatus_RFK = btn06;
sayzyas	10:a2bd7d07c7f8	335	btnStatus_RFI = btn06;
sayzyas	10:a2bd7d07c7f8	336	btnStatus_LBK = btn06;
sayzyas	10:a2bd7d07c7f8	337	btnStatus_LBI = btn06;
sayzyas	10:a2bd7d07c7f8	338	}
sayzyas	10:a2bd7d07c7f8	339	else if ( gamePadPID == GAMEPAD_PID_LOGICOOL_F310 ){
sayzyas	10:a2bd7d07c7f8	340	DEBUG_PRINT_L4("Bd0> [LOGI F310] ");
sayzyas	10:a2bd7d07c7f8	341	btnStatus_WUP = btn04;
sayzyas	10:a2bd7d07c7f8	342	btnStatus_WDN = btn04;
sayzyas	10:a2bd7d07c7f8	343	btnStatus_RFK = btn05;
sayzyas	10:a2bd7d07c7f8	344	btnStatus_RFI = btn05;
sayzyas	10:a2bd7d07c7f8	345	btnStatus_LBK = btn05;
sayzyas	10:a2bd7d07c7f8	346	btnStatus_LBI = btn05;
sayzyas	10:a2bd7d07c7f8	347	btnStatus_RJSFwdRvs = btn03;
sayzyas	10:a2bd7d07c7f8	348	btnStatus_RJSLftRgt = btn02;
sayzyas	10:a2bd7d07c7f8	349	btnStatus_LJSFwdRvs = btn01;
sayzyas	10:a2bd7d07c7f8	350	btnStatus_LJSLftRgt = btn00;
sayzyas	10:a2bd7d07c7f8	351	btnStatus_RJSFwdRvs = btn03;
sayzyas	10:a2bd7d07c7f8	352	btnStatus_RJSLftRgt = btn02;
sayzyas	10:a2bd7d07c7f8	353	btnStatus_LJSFwdRvs = btn01;
sayzyas	10:a2bd7d07c7f8	354	btnStatus_LJSLftRgt = btn00;
sayzyas	10:a2bd7d07c7f8	355	btnStatus_Start = btn05; // Guide button status for ELECOM GamePad
sayzyas	10:a2bd7d07c7f8	356	btnStatus_CrossUp = btn04;
sayzyas	10:a2bd7d07c7f8	357	btnStatus_CrossDn = btn04;
sayzyas	10:a2bd7d07c7f8	358	btnStatus_CrossRt = btn04;
sayzyas	10:a2bd7d07c7f8	359	btnStatus_CrossLt = btn04;
sayzyas	10:a2bd7d07c7f8	360	}
sayzyas	10:a2bd7d07c7f8	361	}
sayzyas	10:a2bd7d07c7f8	362	else if ( gamePadVID == GAMEPAD_VID_SANWA){
sayzyas	10:a2bd7d07c7f8	363	DEBUG_PRINT_L4("Bd0> [SANWA] ");
sayzyas	10:a2bd7d07c7f8	364	btnID_WUP = 4;
sayzyas	10:a2bd7d07c7f8	365	btnID_WDN = 2;
sayzyas	10:a2bd7d07c7f8	366	btnID_RFK = 2;
sayzyas	10:a2bd7d07c7f8	367	btnID_RFI = 1;
sayzyas	10:a2bd7d07c7f8	368	btnID_LBK = 128;
sayzyas	10:a2bd7d07c7f8	369	btnID_LBI = 64;
sayzyas	10:a2bd7d07c7f8	370	if ( gamePadPID == GAMEPAD_PID_SANWA_JYP70US ){
sayzyas	10:a2bd7d07c7f8	371	btnStatus_WUP = btn05;
sayzyas	10:a2bd7d07c7f8	372	btnStatus_WDN = btn05;
sayzyas	10:a2bd7d07c7f8	373	btnStatus_RFK = btn06;
sayzyas	10:a2bd7d07c7f8	374	btnStatus_RFI = btn06;
sayzyas	10:a2bd7d07c7f8	375	btnStatus_LBK = btn05;
sayzyas	10:a2bd7d07c7f8	376	btnStatus_LBI = btn05;
sayzyas	10:a2bd7d07c7f8	377	}
sayzyas	10:a2bd7d07c7f8	378	}
sayzyas	10:a2bd7d07c7f8	379
sayzyas	10:a2bd7d07c7f8	380	#ifdef __DISP_GAMAPAD_STATUS_ALL__ // For Debug
sayzyas	10:a2bd7d07c7f8	381	// 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",
sayzyas	10:a2bd7d07c7f8	382	// btn00,btn01,btn02,btn03,btn04,btn05,btn06,btn07,btn08,
sayzyas	10:a2bd7d07c7f8	383	// gamePadVID, gamePadPID);
sayzyas	13:2c70c772fe24	384	DEBUG_PRINT_L4("Bd0> -- Button Status -------------------------------\r\n");
sayzyas	13:2c70c772fe24	385	DEBUG_PRINT_L4("Bd0> 00(%02x) 01(%02x) 02(%02x) 03(%02x)\r\n", btn00,btn01,btn02,btn03);
sayzyas	13:2c70c772fe24	386	DEBUG_PRINT_L4("Bd0> 04(%02x) 05(%02x) 06(%02x) 07(%02x) 08(%02x)\r\n", btn04,btn05,btn06,btn07,btn08);
sayzyas	13:2c70c772fe24	387	DEBUG_PRINT_L4("Bd0> ------------------------------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	388	#endif
sayzyas	10:a2bd7d07c7f8	389
sayzyas	10:a2bd7d07c7f8	390	I2C_cmd[4] = '\0';
sayzyas	10:a2bd7d07c7f8	391	I2C_cmd[5] = '\0';
sayzyas	10:a2bd7d07c7f8	392	I2C_cmd[6] = '\0';
sayzyas	10:a2bd7d07c7f8	393	I2C_cmd[7] = '\0';
sayzyas	10:a2bd7d07c7f8	394	I2C_cmd[8] = '\0';
sayzyas	10:a2bd7d07c7f8	395	I2C_cmd[9] = '\0';
sayzyas	10:a2bd7d07c7f8	396
sayzyas	10:a2bd7d07c7f8	397	int tmpSpeed = 0;
sayzyas	10:a2bd7d07c7f8	398
sayzyas	10:a2bd7d07c7f8	399	if (swbtn_Opeflg == 1){
sayzyas	10:a2bd7d07c7f8	400	Thread::wait(1);
sayzyas	10:a2bd7d07c7f8	401	}
sayzyas	10:a2bd7d07c7f8	402	else{
sayzyas	10:a2bd7d07c7f8	403	if( gamePadVID == GAMEPAD_VID_RSTHANDY ){ // NRst own original controller
sayzyas	10:a2bd7d07c7f8	404	flg_exp_status &= 0xFFFFFFF0;
sayzyas	10:a2bd7d07c7f8	405	if(!( btnStatus_Start & 0x01 )){ // I-Shape
sayzyas	11:ff06edc0219c	406	flg_mutex.lock();
sayzyas	13:2c70c772fe24	407	baseOperation.sv_JS_ShapeMode = 0;
sayzyas	13:2c70c772fe24	408	baseOperation.sv_WinchValid = 0;
sayzyas	11:ff06edc0219c	409	flg_mutex.unlock();
sayzyas	10:a2bd7d07c7f8	410	flg_exp_status \|= 0x00000001;
sayzyas	10:a2bd7d07c7f8	411	}
sayzyas	10:a2bd7d07c7f8	412	else{ // KO-Shape
sayzyas	11:ff06edc0219c	413	flg_mutex.lock();
sayzyas	13:2c70c772fe24	414	baseOperation.sv_JS_ShapeMode = 1;
sayzyas	11:ff06edc0219c	415	flg_mutex.unlock();
sayzyas	10:a2bd7d07c7f8	416	flg_exp_status \|= 0x00000002;
sayzyas	10:a2bd7d07c7f8	417	}
sayzyas	10:a2bd7d07c7f8	418
sayzyas	13:2c70c772fe24	419	if(!(btnStatus_Start & 0x02 )){ // Tfm,crawler part valid
sayzyas	11:ff06edc0219c	420	flg_mutex.lock();
sayzyas	13:2c70c772fe24	421	baseOperation.sv_WinchValid = 0;
sayzyas	11:ff06edc0219c	422	flg_mutex.unlock();
sayzyas	10:a2bd7d07c7f8	423	flg_exp_status \|= 0x00000004;
sayzyas	10:a2bd7d07c7f8	424	}
sayzyas	13:2c70c772fe24	425	else{ // Winch part valid
sayzyas	11:ff06edc0219c	426	flg_mutex.lock();
sayzyas	13:2c70c772fe24	427	baseOperation.sv_WinchValid = 1;
sayzyas	11:ff06edc0219c	428	flg_mutex.unlock();
sayzyas	10:a2bd7d07c7f8	429	flg_exp_status \|= 0x00000008;
sayzyas	10:a2bd7d07c7f8	430	}
sayzyas	11:ff06edc0219c	431	DEBUG_PRINT_L4( "-----------------------------\r\n" );
sayzyas	11:ff06edc0219c	432	DEBUG_PRINT_L4( "%d : %d\r\n",btnStatus_Start, flg_exp_status );
sayzyas	13:2c70c772fe24	433	DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
sayzyas	13:2c70c772fe24	434	DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
sayzyas	11:ff06edc0219c	435	DEBUG_PRINT_L4( "-----------------------------\r\n" );
sayzyas	10:a2bd7d07c7f8	436	}
sayzyas	10:a2bd7d07c7f8	437	else{
sayzyas	10:a2bd7d07c7f8	438	/*
sayzyas	10:a2bd7d07c7f8	439	*
sayzyas	10:a2bd7d07c7f8	440	* GamePad software switch
sayzyas	10:a2bd7d07c7f8	441	* [Guide] + [B] : JS shape mode toggle: I <--> KO
sayzyas	10:a2bd7d07c7f8	442	* [Guide] + [X} : JS Operation mode toggle: Dual <--> Single
sayzyas	10:a2bd7d07c7f8	443	* 7 6 5 4 3 2 1 0
sayzyas	10:a2bd7d07c7f8	444	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	445	* \|x\|x\|x\|x\|x\|x\|x\|o\| 1: I-Shape JSmode, 2: K-Shape JSmode, 4: Single JSmode, 8: Dual JSmode
sayzyas	10:a2bd7d07c7f8	446	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	447	*/
sayzyas	13:2c70c772fe24	448	if ( btnStatus_CrossUp == btnID_CrossUp ){ // I Shape
sayzyas	13:2c70c772fe24	449	flg_mutex.lock();
sayzyas	13:2c70c772fe24	450	baseOperation.sv_JS_ShapeMode = 0;
sayzyas	13:2c70c772fe24	451	flg_mutex.unlock();
sayzyas	13:2c70c772fe24	452	DEBUG_PRINT_L4( "--------------------------------\r\n" );
sayzyas	13:2c70c772fe24	453	DEBUG_PRINT_L4( " I\r\n" );
sayzyas	13:2c70c772fe24	454	DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
sayzyas	13:2c70c772fe24	455	DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
sayzyas	13:2c70c772fe24	456	DEBUG_PRINT_L4( "--------------------------------\r\n" );
sayzyas	13:2c70c772fe24	457	flg_exp_status \|= 0x00000001;
sayzyas	10:a2bd7d07c7f8	458	}
sayzyas	13:2c70c772fe24	459	else if( btnStatus_CrossDn == btnID_CrossDn ){ // KO Shape
sayzyas	13:2c70c772fe24	460	flg_mutex.lock();
sayzyas	13:2c70c772fe24	461	baseOperation.sv_JS_ShapeMode = 1;
sayzyas	13:2c70c772fe24	462	flg_mutex.unlock();
sayzyas	13:2c70c772fe24	463	DEBUG_PRINT_L4( "-------------------------\r\n" );
sayzyas	13:2c70c772fe24	464	DEBUG_PRINT_L4( " KO\r\n" );
sayzyas	13:2c70c772fe24	465	DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
sayzyas	13:2c70c772fe24	466	DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
sayzyas	13:2c70c772fe24	467	DEBUG_PRINT_L4( "-------------------------\r\n" );
sayzyas	13:2c70c772fe24	468	flg_exp_status \|= 0x00000002;
sayzyas	13:2c70c772fe24	469	}
sayzyas	13:2c70c772fe24	470	else if( btnStatus_CrossRt == btnID_CrossLt ){ // Valid Part : Crawler (Left)
sayzyas	13:2c70c772fe24	471	flg_mutex.lock();
sayzyas	13:2c70c772fe24	472	baseOperation.sv_WinchValid = 0;
sayzyas	13:2c70c772fe24	473	flg_mutex.unlock();
sayzyas	13:2c70c772fe24	474	DEBUG_PRINT_L4( "-----------------------------\r\n" );
sayzyas	13:2c70c772fe24	475	DEBUG_PRINT_L4( " Tfm, Crawler\r\n" );
sayzyas	13:2c70c772fe24	476	DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
sayzyas	13:2c70c772fe24	477	DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
sayzyas	13:2c70c772fe24	478	DEBUG_PRINT_L4( "-----------------------------\r\n" );
sayzyas	13:2c70c772fe24	479	flg_exp_status \|= 0x00000004;
sayzyas	13:2c70c772fe24	480	}
sayzyas	13:2c70c772fe24	481	else if( btnStatus_CrossLt == btnID_CrossRt ){ // Valid Part : Winch (Right)
sayzyas	13:2c70c772fe24	482	flg_mutex.lock();
sayzyas	13:2c70c772fe24	483	baseOperation.sv_WinchValid = 1;
sayzyas	13:2c70c772fe24	484	flg_mutex.unlock();
sayzyas	13:2c70c772fe24	485	DEBUG_PRINT_L4( "-----------------------------\r\n" );
sayzyas	13:2c70c772fe24	486	DEBUG_PRINT_L4( " Winch\r\n" );
sayzyas	13:2c70c772fe24	487	DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
sayzyas	13:2c70c772fe24	488	DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
sayzyas	13:2c70c772fe24	489	DEBUG_PRINT_L4( "-----------------------------\r\n" );
sayzyas	13:2c70c772fe24	490	flg_exp_status \|= 0x00000008;
sayzyas	13:2c70c772fe24	491	}
sayzyas	10:a2bd7d07c7f8	492	else{
sayzyas	10:a2bd7d07c7f8	493	flg_exp_status &= 0xFFFFFFF0;
sayzyas	13:2c70c772fe24	494	}
sayzyas	13:2c70c772fe24	495	}
sayzyas	13:2c70c772fe24	496	/*
sayzyas	13:2c70c772fe24	497	// ====================================
sayzyas	13:2c70c772fe24	498	// TRANSFORM Motor Control
sayzyas	13:2c70c772fe24	499	// ====================================
sayzyas	13:2c70c772fe24	500	* 7 6 5 4 3 2 1 0
sayzyas	13:2c70c772fe24	501	* +-+-+-+-+-+-+-+-+
sayzyas	13:2c70c772fe24	502	* \|o\|x\|x\|x\|x\|x\|x\|x\| 1: RF-I, 2: RF-K, 4: LB-I, 8: LB-K
sayzyas	13:2c70c772fe24	503	* +-+-+-+-+-+-+-+-+
sayzyas	13:2c70c772fe24	504	*/
sayzyas	13:2c70c772fe24	505
sayzyas	13:2c70c772fe24	506	if ((btnStatus_RFK==btnID_RFLBK)&&(baseOperation.sv_WinchValid==0)){ // Both sw on
sayzyas	13:2c70c772fe24	507	flg_ButtonOn = true;
sayzyas	13:2c70c772fe24	508	DEBUG_PRINT_L3( "Bd0> BTN RF-K & LB-K\r\n" );
sayzyas	13:2c70c772fe24	509	led3 = 1;
sayzyas	13:2c70c772fe24	510	#ifdef __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	511	I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
sayzyas	13:2c70c772fe24	512	I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_F; // Speed
sayzyas	13:2c70c772fe24	513	I2C_cmd[6] = MOTOR_FWD; // Motor1 FWD
sayzyas	13:2c70c772fe24	514	I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_F; // Speed
sayzyas	13:2c70c772fe24	515	#endif
sayzyas	13:2c70c772fe24	516	flg_exp_status \|= 0x30000000;
sayzyas	13:2c70c772fe24	517	I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
sayzyas	13:2c70c772fe24	518	I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
sayzyas	13:2c70c772fe24	519	I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
sayzyas	13:2c70c772fe24	520	I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
sayzyas	13:2c70c772fe24	521	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	522	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	523	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 4 );
sayzyas	13:2c70c772fe24	524	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	525	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	526	limitSw_Sts = I2C_res[3];
sayzyas	13:2c70c772fe24	527	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	528	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	529	DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	13:2c70c772fe24	530	#endif // __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	531	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	532	flg_ButtonOn = false;
sayzyas	13:2c70c772fe24	533	}
sayzyas	13:2c70c772fe24	534
sayzyas	13:2c70c772fe24	535	else if ((btnStatus_RFI== btnID_RFLBI)&&(baseOperation.sv_WinchValid==0)){ // Both sw on
sayzyas	13:2c70c772fe24	536	flg_ButtonOn = true;
sayzyas	13:2c70c772fe24	537	DEBUG_PRINT_L3( "Bd0> BTN RF-I & LB-I\r\n" );
sayzyas	13:2c70c772fe24	538	led3 = 1;
sayzyas	13:2c70c772fe24	539	#ifdef __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	540	I2C_cmd[2] = MOTOR_RVS; // Motor1 FWD
sayzyas	13:2c70c772fe24	541	I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_F; // Speed
sayzyas	13:2c70c772fe24	542	I2C_cmd[6] = MOTOR_RVS; // Motor1 FWD
sayzyas	13:2c70c772fe24	543	I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_F; // Speed
sayzyas	13:2c70c772fe24	544	#endif
sayzyas	13:2c70c772fe24	545	flg_exp_status \|= 0x10000000;
sayzyas	13:2c70c772fe24	546	I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
sayzyas	13:2c70c772fe24	547	I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
sayzyas	13:2c70c772fe24	548	I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
sayzyas	13:2c70c772fe24	549	I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
sayzyas	13:2c70c772fe24	550	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	551	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	552	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 4 );
sayzyas	13:2c70c772fe24	553	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	554	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	555	limitSw_Sts = I2C_res[3];
sayzyas	13:2c70c772fe24	556	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	557	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	558	DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	13:2c70c772fe24	559	#endif // __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	560	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	561	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	562	}
sayzyas	13:2c70c772fe24	563
sayzyas	13:2c70c772fe24	564	else if ((btnStatus_RFK==btnID_RFK)&&(baseOperation.sv_WinchValid==0)){ // RF KO
sayzyas	13:2c70c772fe24	565	flg_ButtonOn = true;
sayzyas	13:2c70c772fe24	566	DEBUG_PRINT_L3( "Bd0> BTN RF-K\r\n" );
sayzyas	13:2c70c772fe24	567	led3 = 1;
sayzyas	13:2c70c772fe24	568	#ifdef __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	569	I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
sayzyas	13:2c70c772fe24	570	I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_F; // Speed
sayzyas	13:2c70c772fe24	571	#endif
sayzyas	13:2c70c772fe24	572	flg_exp_status \|= 0x10000000;
sayzyas	13:2c70c772fe24	573	I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
sayzyas	13:2c70c772fe24	574	I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
sayzyas	13:2c70c772fe24	575	I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
sayzyas	13:2c70c772fe24	576	I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
sayzyas	13:2c70c772fe24	577	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	578	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	579	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 4 );
sayzyas	13:2c70c772fe24	580	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	581	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	582	limitSw_Sts = I2C_res[3];
sayzyas	13:2c70c772fe24	583	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	584	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	585	DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	13:2c70c772fe24	586	#endif // __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	587	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	588	flg_ButtonOn = false;
sayzyas	13:2c70c772fe24	589	}
sayzyas	13:2c70c772fe24	590	else if ((btnStatus_RFI==btnID_RFI)&&(baseOperation.sv_WinchValid==0)){ // RF I
sayzyas	13:2c70c772fe24	591	DEBUG_PRINT_L3( "Bd0> BTN RF-I\r\n" );
sayzyas	13:2c70c772fe24	592	led3 = 1;
sayzyas	13:2c70c772fe24	593	#ifdef __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	594	I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
sayzyas	13:2c70c772fe24	595	I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_R; // Speed
sayzyas	13:2c70c772fe24	596	#endif
sayzyas	13:2c70c772fe24	597	flg_exp_status \|= 0x20000000;
sayzyas	13:2c70c772fe24	598	I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
sayzyas	13:2c70c772fe24	599	I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
sayzyas	13:2c70c772fe24	600	I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
sayzyas	13:2c70c772fe24	601	I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
sayzyas	13:2c70c772fe24	602	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	603	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	604	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 4 );
sayzyas	13:2c70c772fe24	605	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	606	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	607	limitSw_Sts = I2C_res[3];
sayzyas	13:2c70c772fe24	608	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	609	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	610	DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	13:2c70c772fe24	611	#endif // __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	612	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	613	flg_ButtonOn = false;
sayzyas	13:2c70c772fe24	614	}
sayzyas	13:2c70c772fe24	615
sayzyas	13:2c70c772fe24	616	else if ((btnStatus_LBK==btnID_LBK)&&(baseOperation.sv_WinchValid==0)){ // LB KO
sayzyas	13:2c70c772fe24	617	flg_ButtonOn = true;
sayzyas	13:2c70c772fe24	618	DEBUG_PRINT_L3( "Bd0> BTN LB-K\r\n" );
sayzyas	13:2c70c772fe24	619	led3 = 1;
sayzyas	13:2c70c772fe24	620	#ifdef __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	621	I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
sayzyas	13:2c70c772fe24	622	I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_F; // Speed
sayzyas	13:2c70c772fe24	623	#endif
sayzyas	13:2c70c772fe24	624	flg_exp_status \|= 0x40000000;
sayzyas	13:2c70c772fe24	625
sayzyas	13:2c70c772fe24	626	I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
sayzyas	13:2c70c772fe24	627	I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
sayzyas	13:2c70c772fe24	628	I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
sayzyas	13:2c70c772fe24	629	I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
sayzyas	13:2c70c772fe24	630	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	631	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	632	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 4 );
sayzyas	13:2c70c772fe24	633	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	634	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	635	limitSw_Sts = I2C_res[3];
sayzyas	13:2c70c772fe24	636	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	637	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	638	DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	13:2c70c772fe24	639	#endif // __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	640	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	641	flg_ButtonOn = false;
sayzyas	13:2c70c772fe24	642	}
sayzyas	13:2c70c772fe24	643	else if ((btnStatus_LBI==btnID_LBI)&&(baseOperation.sv_WinchValid==0)) { // LB I
sayzyas	13:2c70c772fe24	644	flg_ButtonOn = true;
sayzyas	13:2c70c772fe24	645	DEBUG_PRINT_L3( "Bd0> BTN LB-I\r\n" );
sayzyas	13:2c70c772fe24	646	led3 = 1;
sayzyas	13:2c70c772fe24	647	#ifdef __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	648	I2C_cmd[6] = MOTOR_RVS; // Motor1 RVS
sayzyas	13:2c70c772fe24	649	I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_R; // Speed
sayzyas	13:2c70c772fe24	650	#endif
sayzyas	13:2c70c772fe24	651	flg_exp_status \|= 0x80000000;
sayzyas	13:2c70c772fe24	652
sayzyas	13:2c70c772fe24	653	I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
sayzyas	13:2c70c772fe24	654	I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
sayzyas	13:2c70c772fe24	655	I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
sayzyas	13:2c70c772fe24	656	I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
sayzyas	13:2c70c772fe24	657	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	658	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	659	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 4 );
sayzyas	13:2c70c772fe24	660	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	661	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	662	limitSw_Sts = I2C_res[3];
sayzyas	13:2c70c772fe24	663	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	664	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	665	DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	13:2c70c772fe24	666	#endif // __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	667	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	668	flg_ButtonOn = false;
sayzyas	13:2c70c772fe24	669	}
sayzyas	13:2c70c772fe24	670	// ====================================
sayzyas	13:2c70c772fe24	671	// ALL motor off commmand packet send
sayzyas	13:2c70c772fe24	672	// -- This part isn't operated in case of general game controller, because no event. --
sayzyas	13:2c70c772fe24	673	// ====================================
sayzyas	13:2c70c772fe24	674	else if (baseOperation.sv_WinchValid==0){
sayzyas	13:2c70c772fe24	675	led3 = 0;
sayzyas	13:2c70c772fe24	676	#ifdef __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	677	I2C_cmd[2] = MOTOR_STP;
sayzyas	13:2c70c772fe24	678	I2C_cmd[3] = 0;
sayzyas	13:2c70c772fe24	679	I2C_cmd[6] = MOTOR_STP;
sayzyas	13:2c70c772fe24	680	I2C_cmd[7] = 0;
sayzyas	13:2c70c772fe24	681	Thread::wait(5);
sayzyas	13:2c70c772fe24	682	#endif
sayzyas	13:2c70c772fe24	683	flg_exp_status &= 0x0FFFFFFF;
sayzyas	13:2c70c772fe24	684	I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
sayzyas	13:2c70c772fe24	685	I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
sayzyas	13:2c70c772fe24	686	I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
sayzyas	13:2c70c772fe24	687	I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
sayzyas	13:2c70c772fe24	688	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	689	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	690
sayzyas	13:2c70c772fe24	691	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	692	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 4 );
sayzyas	13:2c70c772fe24	693	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	694	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	695	limitSw_Sts = I2C_res[3];
sayzyas	13:2c70c772fe24	696	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	697	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	698	DEBUG_PRINT_L2( "Bd0> 3: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	13:2c70c772fe24	699	#endif // __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	700
sayzyas	13:2c70c772fe24	701	}
sayzyas	13:2c70c772fe24	702
sayzyas	10:a2bd7d07c7f8	703	/*/ ====================================
sayzyas	10:a2bd7d07c7f8	704	// Crawler Moving Control
sayzyas	10:a2bd7d07c7f8	705	// ====================================
sayzyas	10:a2bd7d07c7f8	706	// JoyStick mode 1: Dual JoyStick mode
sayzyas	10:a2bd7d07c7f8	707	//
sayzyas	10:a2bd7d07c7f8	708	// * *
sayzyas	10:a2bd7d07c7f8	709	// * L * * R *
sayzyas	10:a2bd7d07c7f8	710	// * *
sayzyas	10:a2bd7d07c7f8	711	// F 4 1
sayzyas	10:a2bd7d07c7f8	712	//
sayzyas	10:a2bd7d07c7f8	713	// R 8 2
sayzyas	10:a2bd7d07c7f8	714	//
sayzyas	10:a2bd7d07c7f8	715	// Forward move 5
sayzyas	10:a2bd7d07c7f8	716	// Reverce move a
sayzyas	10:a2bd7d07c7f8	717	// Right rotation 6
sayzyas	10:a2bd7d07c7f8	718	// Left rotation 9
sayzyas	10:a2bd7d07c7f8	719	* 7 6 5 4 3 2 1 0
sayzyas	10:a2bd7d07c7f8	720	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	721	* \|x\|x\|o\|x\|x\|x\|x\|x\| 1: R Fwd, 2: R Rvs, 4: L Fwd, 8: L Rvs
sayzyas	10:a2bd7d07c7f8	722	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	723	*/
sayzyas	13:2c70c772fe24	724	if( baseOperation.sv_JS_OpeMode == 1 ){
sayzyas	11:ff06edc0219c	725	I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
sayzyas	11:ff06edc0219c	726	I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
sayzyas	11:ff06edc0219c	727	I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
sayzyas	11:ff06edc0219c	728	I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
sayzyas	13:2c70c772fe24	729	if ((btnStatus_LJSFwdRvs<setValue.crawlerCtrl.sv_LBCM_dzc-setValue.crawlerCtrl.sv_LBCM_dzl)&&(baseOperation.sv_WinchValid==0)){
sayzyas	10:a2bd7d07c7f8	730	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	731	led3 = 1;
sayzyas	10:a2bd7d07c7f8	732	I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
sayzyas	10:a2bd7d07c7f8	733	tmpSpeed = ( setValue.crawlerCtrl.sv_LBCM_dzc+1 - btnStatus_LJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc * setValue.crawlerCtrl.sv_LBCM_sli_F / 100; // Speed
sayzyas	10:a2bd7d07c7f8	734	I2C_cmd[7] = (char)tmpSpeed;
sayzyas	13:2c70c772fe24	735	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	11:ff06edc0219c	736	DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Fwd (Speed=%d)\r\n", tmpSpeed);
sayzyas	13:2c70c772fe24	737	// Read motor current from target
sayzyas	13:2c70c772fe24	738	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 3 );
sayzyas	13:2c70c772fe24	739	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	740	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	741	DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	742	DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	743
sayzyas	11:ff06edc0219c	744	flg_exp_status \|= 0x00400000;
sayzyas	13:2c70c772fe24	745	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	746	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	747	}
sayzyas	13:2c70c772fe24	748	else if ((btnStatus_LJSFwdRvs>setValue.crawlerCtrl.sv_LBCM_dzc+setValue.crawlerCtrl.sv_LBCM_dzu)&&(baseOperation.sv_WinchValid==0)){
sayzyas	10:a2bd7d07c7f8	749	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	750	led3 = 1;
sayzyas	10:a2bd7d07c7f8	751	I2C_cmd[6] = MOTOR_RVS; // Motor2 RVS
sayzyas	10:a2bd7d07c7f8	752	tmpSpeed = ( btnStatus_LJSFwdRvs - setValue.crawlerCtrl.sv_LBCM_dzc ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc+1 * setValue.crawlerCtrl.sv_LBCM_sli_F / 100; // Speed I2C_cmd[5] = (char)tmpSpeed;
sayzyas	10:a2bd7d07c7f8	753	I2C_cmd[7] = (char)tmpSpeed;
sayzyas	13:2c70c772fe24	754	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	11:ff06edc0219c	755	DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Rvs (Speed=%d)\r\n", tmpSpeed);
sayzyas	11:ff06edc0219c	756
sayzyas	13:2c70c772fe24	757	// Read motor current from target
sayzyas	13:2c70c772fe24	758	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 3 );
sayzyas	13:2c70c772fe24	759	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	760	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	761	DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	762	DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	11:ff06edc0219c	763
sayzyas	10:a2bd7d07c7f8	764	flg_exp_status \|= 0x00800000;
sayzyas	13:2c70c772fe24	765	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	766	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	767	}
sayzyas	13:2c70c772fe24	768	else if (baseOperation.sv_WinchValid==0) {
sayzyas	10:a2bd7d07c7f8	769	I2C_cmd[6] = MOTOR_STP; // Stop
sayzyas	10:a2bd7d07c7f8	770	I2C_cmd[7] = 0; // Speed=0
sayzyas	10:a2bd7d07c7f8	771	flg_exp_status &= 0xFF3F000F;
sayzyas	13:2c70c772fe24	772	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	773	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	774	}
sayzyas	10:a2bd7d07c7f8	775
sayzyas	13:2c70c772fe24	776	if ((btnStatus_RJSFwdRvs<setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)&&(baseOperation.sv_WinchValid==0)){
sayzyas	10:a2bd7d07c7f8	777	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	778	led3 = 1;
sayzyas	10:a2bd7d07c7f8	779	I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
sayzyas	10:a2bd7d07c7f8	780	tmpSpeed = (( 128 - btnStatus_RJSFwdRvs ) * 100 / 127 ) * setValue.crawlerCtrl.sv_RFCM_sli_R / 100;
sayzyas	10:a2bd7d07c7f8	781	I2C_cmd[3] = (char)tmpSpeed;
sayzyas	11:ff06edc0219c	782	DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Fwd (Speed=%d)\r\n", tmpSpeed);
sayzyas	13:2c70c772fe24	783	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	11:ff06edc0219c	784
sayzyas	13:2c70c772fe24	785	// Read motor current from target
sayzyas	13:2c70c772fe24	786	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 3 );
sayzyas	13:2c70c772fe24	787	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	788	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	789	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	790	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	11:ff06edc0219c	791
sayzyas	10:a2bd7d07c7f8	792	flg_exp_status \|= 0x00100000;
sayzyas	13:2c70c772fe24	793	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	794	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	795	}
sayzyas	13:2c70c772fe24	796	else if ((btnStatus_RJSFwdRvs>setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu)&&(baseOperation.sv_WinchValid==0)){
sayzyas	10:a2bd7d07c7f8	797	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	798	led3 = 1;
sayzyas	10:a2bd7d07c7f8	799	I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
sayzyas	10:a2bd7d07c7f8	800	tmpSpeed = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc+1 * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
sayzyas	10:a2bd7d07c7f8	801	I2C_cmd[3] = (char)tmpSpeed;
sayzyas	11:ff06edc0219c	802	DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Rvs (Speed=%d)\r\n", tmpSpeed);
sayzyas	13:2c70c772fe24	803	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	11:ff06edc0219c	804
sayzyas	13:2c70c772fe24	805	// Read motor current from target
sayzyas	13:2c70c772fe24	806	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 3 );
sayzyas	13:2c70c772fe24	807	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	808	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	809	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	810	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	11:ff06edc0219c	811
sayzyas	10:a2bd7d07c7f8	812	flg_exp_status \|= 0x00200000;
sayzyas	13:2c70c772fe24	813	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	814	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	815	}
sayzyas	13:2c70c772fe24	816	else if (baseOperation.sv_WinchValid==0) {
sayzyas	13:2c70c772fe24	817	pc.printf("*** MOTOR STOP **\r\n");
sayzyas	10:a2bd7d07c7f8	818	I2C_cmd[2] = MOTOR_STP; // Stop
sayzyas	10:a2bd7d07c7f8	819	I2C_cmd[3] = 0; // Speed=0
sayzyas	10:a2bd7d07c7f8	820	flg_exp_status &= 0xFFCFFFFF;
sayzyas	13:2c70c772fe24	821	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	822	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	823	}
sayzyas	10:a2bd7d07c7f8	824	led3 = 0;
sayzyas	10:a2bd7d07c7f8	825	}
sayzyas	10:a2bd7d07c7f8	826	/* JoyStick mode 0: Single JoyStick mode
sayzyas	10:a2bd7d07c7f8	827	//
sayzyas	10:a2bd7d07c7f8	828	// * **
sayzyas	10:a2bd7d07c7f8	829	// * X * * LR *
sayzyas	10:a2bd7d07c7f8	830	// * **
sayzyas	10:a2bd7d07c7f8	831	// F 4 1
sayzyas	10:a2bd7d07c7f8	832	//
sayzyas	10:a2bd7d07c7f8	833	// R 8 2
sayzyas	10:a2bd7d07c7f8	834	//
sayzyas	10:a2bd7d07c7f8	835	// Forward move 5
sayzyas	10:a2bd7d07c7f8	836	// Reverce move a
sayzyas	10:a2bd7d07c7f8	837	// Right rotation 6
sayzyas	10:a2bd7d07c7f8	838	// Left rotation 9
sayzyas	10:a2bd7d07c7f8	839	* 7 6 5 4 3 2 1 0
sayzyas	10:a2bd7d07c7f8	840	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	841	* \|x\|x\|o\|x\|x\|x\|x\|x\| 1: R Fwd, 2: R Rvs, 4: L Fwd, 8: L Rvs
sayzyas	10:a2bd7d07c7f8	842	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	843	*/
sayzyas	10:a2bd7d07c7f8	844	else{ // Single JoyStick mode
sayzyas	13:2c70c772fe24	845	if(
sayzyas	13:2c70c772fe24	846	( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl ) &&
sayzyas	13:2c70c772fe24	847	( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50) &&
sayzyas	13:2c70c772fe24	848	( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50 ) &&
sayzyas	13:2c70c772fe24	849	( baseOperation.sv_WinchValid == 0)
sayzyas	13:2c70c772fe24	850	){
sayzyas	10:a2bd7d07c7f8	851	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	852	led3 = 1;
sayzyas	13:2c70c772fe24	853	if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
sayzyas	11:ff06edc0219c	854	I2C_cmd[2] = MOTOR_RVS; // Motor1 Reverse
sayzyas	13:2c70c772fe24	855	I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
sayzyas	11:ff06edc0219c	856	}
sayzyas	11:ff06edc0219c	857	else{
sayzyas	11:ff06edc0219c	858	I2C_cmd[2] = MOTOR_FWD; // Motor1 Fwd
sayzyas	13:2c70c772fe24	859	I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
sayzyas	11:ff06edc0219c	860	}
sayzyas	10:a2bd7d07c7f8	861	I2C_cmd[3] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
sayzyas	10:a2bd7d07c7f8	862	I2C_cmd[7] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
sayzyas	13:2c70c772fe24	863	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	864	DEBUG_PRINT_L3( "Bd0> Single Mode: Dir1 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[2], I2C_cmd[3], I2C_cmd[5], I2C_cmd[7]);
sayzyas	13:2c70c772fe24	865	flg_exp_status \|= 0x00500000;
sayzyas	13:2c70c772fe24	866	//flg_exp_status \|= 0x00400000; // 0x00500000
sayzyas	13:2c70c772fe24	867
sayzyas	13:2c70c772fe24	868	// Read motor current from target
sayzyas	13:2c70c772fe24	869	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 3 );
sayzyas	13:2c70c772fe24	870	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	871	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	872	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	873	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	874
sayzyas	13:2c70c772fe24	875	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	12:3e6b6fcf540b	876	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	877	}
sayzyas	13:2c70c772fe24	878	else if(
sayzyas	13:2c70c772fe24	879	( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu ) &&
sayzyas	13:2c70c772fe24	880	( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50 ) &&
sayzyas	13:2c70c772fe24	881	( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
sayzyas	13:2c70c772fe24	882	( baseOperation.sv_WinchValid == 0)
sayzyas	13:2c70c772fe24	883	){
sayzyas	10:a2bd7d07c7f8	884	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	885	led3 = 1;
sayzyas	13:2c70c772fe24	886	if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
sayzyas	11:ff06edc0219c	887	I2C_cmd[2] = MOTOR_STP; // Motor1 Reverse
sayzyas	11:ff06edc0219c	888	I2C_cmd[6] = MOTOR_STP; // Motor1 Reverse
sayzyas	11:ff06edc0219c	889	}
sayzyas	11:ff06edc0219c	890	else{
sayzyas	11:ff06edc0219c	891	I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
sayzyas	11:ff06edc0219c	892	I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
sayzyas	11:ff06edc0219c	893	}
sayzyas	10:a2bd7d07c7f8	894	I2C_cmd[3] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
sayzyas	10:a2bd7d07c7f8	895	I2C_cmd[7] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
sayzyas	13:2c70c772fe24	896	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	897	DEBUG_PRINT_L3( "Bd0> Single Mode: Dir2 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[2], I2C_cmd[3], I2C_cmd[5], I2C_cmd[7]);
sayzyas	13:2c70c772fe24	898	flg_exp_status \|= 0x00600000;
sayzyas	13:2c70c772fe24	899	//flg_exp_status \|= 0x00400000; // 0x00600000
sayzyas	13:2c70c772fe24	900
sayzyas	13:2c70c772fe24	901	// Read motor current from target
sayzyas	13:2c70c772fe24	902	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 3 );
sayzyas	13:2c70c772fe24	903	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	904	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	905	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	906	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	907
sayzyas	13:2c70c772fe24	908	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	909	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	910	}
sayzyas	13:2c70c772fe24	911	else if(
sayzyas	13:2c70c772fe24	912	( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu ) &&
sayzyas	13:2c70c772fe24	913	( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50) &&
sayzyas	13:2c70c772fe24	914	( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
sayzyas	13:2c70c772fe24	915	( baseOperation.sv_WinchValid == 0)
sayzyas	13:2c70c772fe24	916	){
sayzyas	10:a2bd7d07c7f8	917	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	918	led3 = 1;
sayzyas	13:2c70c772fe24	919	if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
sayzyas	11:ff06edc0219c	920	I2C_cmd[2] = MOTOR_FWD; // Motor1 Rvs
sayzyas	13:2c70c772fe24	921	I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
sayzyas	11:ff06edc0219c	922	}
sayzyas	11:ff06edc0219c	923	else{
sayzyas	11:ff06edc0219c	924	I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
sayzyas	13:2c70c772fe24	925	I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
sayzyas	11:ff06edc0219c	926	}
sayzyas	10:a2bd7d07c7f8	927	I2C_cmd[3] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
sayzyas	10:a2bd7d07c7f8	928	I2C_cmd[7] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
sayzyas	13:2c70c772fe24	929	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	930	DEBUG_PRINT_L3( "Bd0> Single Mode: Dir3 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[2], I2C_cmd[3], I2C_cmd[5], I2C_cmd[7]);
sayzyas	13:2c70c772fe24	931	flg_exp_status \|= 0x00A00000;
sayzyas	13:2c70c772fe24	932	//flg_exp_status \|= 0x00800000; // 0x00A00000
sayzyas	13:2c70c772fe24	933	// Read motor current from target
sayzyas	13:2c70c772fe24	934
sayzyas	13:2c70c772fe24	935	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 3 );
sayzyas	13:2c70c772fe24	936	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	937	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	938	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	939	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	940
sayzyas	13:2c70c772fe24	941	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	942	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	943	}
sayzyas	13:2c70c772fe24	944	else if(
sayzyas	13:2c70c772fe24	945	( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzu ) &&
sayzyas	13:2c70c772fe24	946	( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50) &&
sayzyas	13:2c70c772fe24	947	( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
sayzyas	13:2c70c772fe24	948	( baseOperation.sv_WinchValid == 0)
sayzyas	13:2c70c772fe24	949	){
sayzyas	10:a2bd7d07c7f8	950	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	951	led3 = 1;
sayzyas	13:2c70c772fe24	952	if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
sayzyas	11:ff06edc0219c	953	I2C_cmd[2] = MOTOR_STP; // Motor1 Reverse
sayzyas	11:ff06edc0219c	954	I2C_cmd[6] = MOTOR_STP; // Motor1 Reverse
sayzyas	11:ff06edc0219c	955	}
sayzyas	11:ff06edc0219c	956	else{
sayzyas	11:ff06edc0219c	957	I2C_cmd[2] = MOTOR_FWD; // Motor1 Fwd
sayzyas	11:ff06edc0219c	958	I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
sayzyas	11:ff06edc0219c	959	}
sayzyas	10:a2bd7d07c7f8	960	I2C_cmd[3] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
sayzyas	10:a2bd7d07c7f8	961	I2C_cmd[7] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
sayzyas	13:2c70c772fe24	962	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	963	DEBUG_PRINT_L3( "Bd0> Single Mode: Dir4 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[2], I2C_cmd[3], I2C_cmd[5], I2C_cmd[7]);
sayzyas	13:2c70c772fe24	964	flg_exp_status \|= 0x00900000;
sayzyas	13:2c70c772fe24	965	//flg_exp_status \|= 0x00800000; // 0x00900000
sayzyas	13:2c70c772fe24	966
sayzyas	13:2c70c772fe24	967	// Read motor current from target
sayzyas	13:2c70c772fe24	968	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 3 );
sayzyas	13:2c70c772fe24	969	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	970	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	971	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	972	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	973
sayzyas	13:2c70c772fe24	974	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	975	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	976	}
sayzyas	10:a2bd7d07c7f8	977	// ====================================
sayzyas	10:a2bd7d07c7f8	978	// ALL motor off commmand packet send
sayzyas	10:a2bd7d07c7f8	979	// ====================================
sayzyas	13:2c70c772fe24	980	else if (baseOperation.sv_WinchValid==0) {
sayzyas	10:a2bd7d07c7f8	981	led3 = 0;
sayzyas	10:a2bd7d07c7f8	982	#ifdef __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	983	pc.printf("*** MOTOR STOP **\r\n");
sayzyas	10:a2bd7d07c7f8	984	I2C_cmd[2] = MOTOR_STP; // Motor1 Fwd
sayzyas	10:a2bd7d07c7f8	985	I2C_cmd[3] = 0; // Speed=0
sayzyas	10:a2bd7d07c7f8	986	I2C_cmd[6] = MOTOR_STP; // Motor2 Rvs
sayzyas	10:a2bd7d07c7f8	987	I2C_cmd[7] = 0; // Speed=0
sayzyas	13:2c70c772fe24	988	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	989	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	990	// Thread::wait(5);
sayzyas	10:a2bd7d07c7f8	991	#endif
sayzyas	10:a2bd7d07c7f8	992	flg_exp_status &= 0xFF0FFFFF;
sayzyas	13:2c70c772fe24	993	// I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
sayzyas	13:2c70c772fe24	994	// I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
sayzyas	13:2c70c772fe24	995	// I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
sayzyas	13:2c70c772fe24	996	// I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
sayzyas	10:a2bd7d07c7f8	997	}
sayzyas	10:a2bd7d07c7f8	998	}
sayzyas	13:2c70c772fe24	999
sayzyas	10:a2bd7d07c7f8	1000	/*
sayzyas	10:a2bd7d07c7f8	1001	* ====================================
sayzyas	10:a2bd7d07c7f8	1002	* Winch Motor Control
sayzyas	10:a2bd7d07c7f8	1003	*
sayzyas	10:a2bd7d07c7f8	1004	* 7 6 5 4 3 2 1 0
sayzyas	10:a2bd7d07c7f8	1005	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	1006	* \|x\|o\|x\|x\|x\|x\|x\|x\| 1: W Down, 2: W Up, 4: -, 8: -
sayzyas	10:a2bd7d07c7f8	1007	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	1008	*/
sayzyas	13:2c70c772fe24	1009	if ((btnStatus_WDN == btnID_WDN) && (baseOperation.sv_WinchValid == 1)){ // Winch Down (FWD)
sayzyas	10:a2bd7d07c7f8	1010	flg_ButtonOn = true;
sayzyas	11:ff06edc0219c	1011	DEBUG_PRINT_L3( "Bd0> BTN W-DN(Fwd)\r\n" );
sayzyas	10:a2bd7d07c7f8	1012	led3 = 1;
sayzyas	10:a2bd7d07c7f8	1013	#ifdef __IIC_COMAMND_SEND__
sayzyas	10:a2bd7d07c7f8	1014	I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
sayzyas	10:a2bd7d07c7f8	1015	I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_F; // Speed
sayzyas	10:a2bd7d07c7f8	1016	// I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
sayzyas	10:a2bd7d07c7f8	1017	// I2C_cmd[7] = setValue.winchCtrl.sv_WRM_sli_F; // Speed
sayzyas	10:a2bd7d07c7f8	1018	#endif
sayzyas	10:a2bd7d07c7f8	1019	flg_exp_status \|= 0x01000000;
sayzyas	10:a2bd7d07c7f8	1020	I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
sayzyas	10:a2bd7d07c7f8	1021	I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
sayzyas	10:a2bd7d07c7f8	1022	I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
sayzyas	10:a2bd7d07c7f8	1023	I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
sayzyas	13:2c70c772fe24	1024	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	1025	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	1026	}
sayzyas	13:2c70c772fe24	1027	else if (( btnStatus_WUP == btnID_WUP ) && (baseOperation.sv_WinchValid == 1)) { // Winch Up (Rvs)
sayzyas	10:a2bd7d07c7f8	1028	flg_ButtonOn = true;
sayzyas	11:ff06edc0219c	1029	DEBUG_PRINT_L3( "Bd0> BTN W-UP(Rvs)\r\n" );
sayzyas	10:a2bd7d07c7f8	1030	led3 = 1;
sayzyas	10:a2bd7d07c7f8	1031	#ifdef __IIC_COMAMND_SEND__
sayzyas	10:a2bd7d07c7f8	1032	I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
sayzyas	10:a2bd7d07c7f8	1033	I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_R; // Speed
sayzyas	10:a2bd7d07c7f8	1034	// I2C_cmd[6] = MOTOR_RVS; // Motor2 RVS
sayzyas	10:a2bd7d07c7f8	1035	// I2C_cmd[7] = setValue.winchCtrl.sv_WRM_sli_R; // Speed
sayzyas	10:a2bd7d07c7f8	1036	#endif
sayzyas	10:a2bd7d07c7f8	1037	flg_exp_status \|= 0x02000000;
sayzyas	10:a2bd7d07c7f8	1038	I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
sayzyas	10:a2bd7d07c7f8	1039	I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
sayzyas	10:a2bd7d07c7f8	1040	I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
sayzyas	10:a2bd7d07c7f8	1041	I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
sayzyas	13:2c70c772fe24	1042	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	1043	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	1044	}
sayzyas	10:a2bd7d07c7f8	1045	// ====================================
sayzyas	10:a2bd7d07c7f8	1046	// ALL motor off commmand packet send
sayzyas	10:a2bd7d07c7f8	1047	// ====================================
sayzyas	13:2c70c772fe24	1048	else if (baseOperation.sv_WinchValid == 1){
sayzyas	10:a2bd7d07c7f8	1049	led3 = 0;
sayzyas	10:a2bd7d07c7f8	1050	#ifdef __IIC_COMAMND_SEND__
sayzyas	10:a2bd7d07c7f8	1051	I2C_cmd[2] = MOTOR_STP;
sayzyas	10:a2bd7d07c7f8	1052	I2C_cmd[3] = 0;
sayzyas	10:a2bd7d07c7f8	1053	I2C_cmd[6] = MOTOR_STP;
sayzyas	10:a2bd7d07c7f8	1054	I2C_cmd[7] = 0;
sayzyas	10:a2bd7d07c7f8	1055	Thread::wait(5);
sayzyas	10:a2bd7d07c7f8	1056	#endif
sayzyas	10:a2bd7d07c7f8	1057	flg_exp_status &= 0xF0FFFFFF;
sayzyas	10:a2bd7d07c7f8	1058	I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
sayzyas	10:a2bd7d07c7f8	1059	I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
sayzyas	10:a2bd7d07c7f8	1060	I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
sayzyas	10:a2bd7d07c7f8	1061	I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
sayzyas	13:2c70c772fe24	1062	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	1063	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	1064	}
sayzyas	10:a2bd7d07c7f8	1065	}
sayzyas	10:a2bd7d07c7f8	1066	}
sayzyas	10:a2bd7d07c7f8	1067
sayzyas	10:a2bd7d07c7f8	1068	uint32_t getc_fromHost( uint8_t *c ){
sayzyas	10:a2bd7d07c7f8	1069
sayzyas	10:a2bd7d07c7f8	1070	uint32_t rts = 0;
sayzyas	10:a2bd7d07c7f8	1071
sayzyas	10:a2bd7d07c7f8	1072	if(pc.readable()){
sayzyas	10:a2bd7d07c7f8	1073	*c = pc.getc();
sayzyas	10:a2bd7d07c7f8	1074	rts = 0;
sayzyas	10:a2bd7d07c7f8	1075	}
sayzyas	10:a2bd7d07c7f8	1076	else{
sayzyas	10:a2bd7d07c7f8	1077	rts = 1;
sayzyas	10:a2bd7d07c7f8	1078	}
sayzyas	10:a2bd7d07c7f8	1079	return rts;
sayzyas	10:a2bd7d07c7f8	1080	}
sayzyas	10:a2bd7d07c7f8	1081
sayzyas	10:a2bd7d07c7f8	1082	// **************************************************************
sayzyas	10:a2bd7d07c7f8	1083	// TASK: Hoat Interface Task
sayzyas	10:a2bd7d07c7f8	1084	//
sayzyas	10:a2bd7d07c7f8	1085	// **************************************************************
sayzyas	10:a2bd7d07c7f8	1086	void clientPC_interface_task(void const *)
sayzyas	10:a2bd7d07c7f8	1087	{
sayzyas	10:a2bd7d07c7f8	1088	int rcv_data_cnt;
sayzyas	10:a2bd7d07c7f8	1089	//winchData_t winchData;
sayzyas	10:a2bd7d07c7f8	1090
sayzyas	13:2c70c772fe24	1091	char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
sayzyas	10:a2bd7d07c7f8	1092
sayzyas	10:a2bd7d07c7f8	1093	// winchData_t winchData;
sayzyas	13:2c70c772fe24	1094	// int16_t winchCurrentPosition;
sayzyas	13:2c70c772fe24	1095	int16_t winchTempPosition;
sayzyas	11:ff06edc0219c	1096
sayzyas	11:ff06edc0219c	1097	int cnt = 0;
sayzyas	10:a2bd7d07c7f8	1098
sayzyas	10:a2bd7d07c7f8	1099	while(1){
sayzyas	10:a2bd7d07c7f8	1100
sayzyas	11:ff06edc0219c	1101	// DEBUG_PRINT("\r\nWaiting for UDP packet...\r\n");
sayzyas	10:a2bd7d07c7f8	1102	rcv_data_cnt = udp_server.receiveFrom(client, dbuffer, sizeof(dbuffer));
sayzyas	10:a2bd7d07c7f8	1103	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1104	if( rcv_data_cnt < 0 ){
sayzyas	11:ff06edc0219c	1105	DEBUG_PRINT_L0("Bd0> ## Receive packet fail ##\r\n");
sayzyas	10:a2bd7d07c7f8	1106	}
sayzyas	10:a2bd7d07c7f8	1107	else{
sayzyas	10:a2bd7d07c7f8	1108	dbuffer[rcv_data_cnt] = '\0';
sayzyas	10:a2bd7d07c7f8	1109	led4 = 1;
sayzyas	10:a2bd7d07c7f8	1110
sayzyas	11:ff06edc0219c	1111	if(!strcmp( dbuffer, "Hello Z\r\n" )){
sayzyas	10:a2bd7d07c7f8	1112	DEBUG_PRINT_L2("Bd0> Hello Z Packet received from client by UDP\n");
sayzyas	10:a2bd7d07c7f8	1113	char snd_data[] = "Hello I'm CrExoB2";
sayzyas	10:a2bd7d07c7f8	1114	udp_server.sendTo(client, snd_data, sizeof(snd_data));
sayzyas	10:a2bd7d07c7f8	1115	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1116	}
sayzyas	11:ff06edc0219c	1117	else if(!strcmp( dbuffer, "status\r\n")){
sayzyas	11:ff06edc0219c	1118	DEBUG_PRINT_L2("Bd0> Hello Status req received from client by UDP\r\n");
sayzyas	11:ff06edc0219c	1119	strcpy(dbuffer,"XXXX\r\n");
sayzyas	10:a2bd7d07c7f8	1120	udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
sayzyas	10:a2bd7d07c7f8	1121	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1122	}
sayzyas	10:a2bd7d07c7f8	1123	else if(!strcmp( dbuffer, "Hello")){
sayzyas	11:ff06edc0219c	1124	DEBUG_PRINT_L2( "Bd0> Hello Packet received from [ 0x%02x ]\r\n", flg_exp_status );
sayzyas	10:a2bd7d07c7f8	1125
sayzyas	10:a2bd7d07c7f8	1126	/* ***************************************** */
sayzyas	10:a2bd7d07c7f8	1127	/* Read Winch Current Position from Resolver */
sayzyas	10:a2bd7d07c7f8	1128	/* ***************************************** */
sayzyas	13:2c70c772fe24	1129	if (baseOperation.sv_WinchValid == 1){ // read winch current position operation is valid in case of winch part is valid.
sayzyas	13:2c70c772fe24	1130	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1131	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1132	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1133	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1134	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1135	}
sayzyas	13:2c70c772fe24	1136	}
sayzyas	10:a2bd7d07c7f8	1137	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1138
sayzyas	13:2c70c772fe24	1139	sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts); // Winch down
sayzyas	10:a2bd7d07c7f8	1140	// -------------------------------------
sayzyas	10:a2bd7d07c7f8	1141	// Crawler Moving
sayzyas	10:a2bd7d07c7f8	1142	// -------------------------------------
sayzyas	10:a2bd7d07c7f8	1143	if( flg_exp_status & 0x00F00000 ){
sayzyas	10:a2bd7d07c7f8	1144	// Forward move 5
sayzyas	10:a2bd7d07c7f8	1145	// Reverce move a
sayzyas	10:a2bd7d07c7f8	1146	// Right rotation 6
sayzyas	10:a2bd7d07c7f8	1147	// Left rotation 9
sayzyas	11:ff06edc0219c	1148
sayzyas	10:a2bd7d07c7f8	1149	if((flg_exp_status & 0x00F00000) == 0x00500000 ){
sayzyas	11:ff06edc0219c	1150	// 01234 5678 9012 34569
sayzyas	13:2c70c772fe24	1151	sprintf( dbuffer, "WCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	10:a2bd7d07c7f8	1152	// Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1153	}
sayzyas	10:a2bd7d07c7f8	1154	else if((flg_exp_status & 0x00F00000) == 0x00a00000 ){
sayzyas	13:2c70c772fe24	1155	sprintf( dbuffer, "WCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	10:a2bd7d07c7f8	1156	// Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1157	}
sayzyas	10:a2bd7d07c7f8	1158	else if((flg_exp_status & 0x00F00000) == 0x00600000 ){
sayzyas	13:2c70c772fe24	1159	sprintf( dbuffer, "WCRR %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	10:a2bd7d07c7f8	1160	// Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1161	}
sayzyas	10:a2bd7d07c7f8	1162	else if((flg_exp_status & 0x00F00000) == 0x00900000 ){
sayzyas	13:2c70c772fe24	1163	sprintf( dbuffer, "WCLR %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	10:a2bd7d07c7f8	1164	// Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1165	}
sayzyas	10:a2bd7d07c7f8	1166	else if((flg_exp_status & 0x00F00000) == 0x00800000 ){
sayzyas	13:2c70c772fe24	1167	sprintf( dbuffer, "LCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	10:a2bd7d07c7f8	1168	// Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1169	}
sayzyas	10:a2bd7d07c7f8	1170	else if((flg_exp_status & 0x00F00000) == 0x00400000 ){
sayzyas	13:2c70c772fe24	1171	sprintf( dbuffer, "LCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	10:a2bd7d07c7f8	1172	// Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1173	}
sayzyas	10:a2bd7d07c7f8	1174	else if((flg_exp_status & 0x00F00000) == 0x00200000 ){
sayzyas	13:2c70c772fe24	1175	sprintf( dbuffer, "RCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	10:a2bd7d07c7f8	1176	// Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1177	}
sayzyas	10:a2bd7d07c7f8	1178	else if((flg_exp_status & 0x00F00000) == 0x00100000 ){
sayzyas	13:2c70c772fe24	1179	sprintf( dbuffer, "RCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	10:a2bd7d07c7f8	1180	// Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1181	}
sayzyas	11:ff06edc0219c	1182	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1183	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1184	}
sayzyas	10:a2bd7d07c7f8	1185	// -------------------------------------
sayzyas	10:a2bd7d07c7f8	1186	// Transform
sayzyas	10:a2bd7d07c7f8	1187	// -------------------------------------
sayzyas	10:a2bd7d07c7f8	1188	else if( flg_exp_status & 0x20000000 ){
sayzyas	13:2c70c772fe24	1189	sprintf(dbuffer,"RF2I %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // RF Crawler Tfm I
sayzyas	11:ff06edc0219c	1190	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1191	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1192	}
sayzyas	10:a2bd7d07c7f8	1193	else if( flg_exp_status & 0x10000000 ){
sayzyas	13:2c70c772fe24	1194	sprintf(dbuffer,"RF2K %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // RF Crawler Tfm K
sayzyas	11:ff06edc0219c	1195	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1196	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1197	}
sayzyas	10:a2bd7d07c7f8	1198	else if( flg_exp_status & 0x80000000 ){
sayzyas	13:2c70c772fe24	1199	sprintf(dbuffer,"LB2I %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // LB Crawler Tfm I
sayzyas	11:ff06edc0219c	1200	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1201	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1202	}
sayzyas	10:a2bd7d07c7f8	1203	else if( flg_exp_status & 0x40000000 ){
sayzyas	13:2c70c772fe24	1204	sprintf(dbuffer,"LB2K %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // LB Crawler Tfm K
sayzyas	11:ff06edc0219c	1205	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1206	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1207	}
sayzyas	10:a2bd7d07c7f8	1208	// -------------------------------------
sayzyas	10:a2bd7d07c7f8	1209	// Winch Moving
sayzyas	10:a2bd7d07c7f8	1210	// -------------------------------------
sayzyas	10:a2bd7d07c7f8	1211	else if( flg_exp_status & 0x01000000 ){ // Wincd down (FWD)
sayzyas	13:2c70c772fe24	1212	sprintf(dbuffer,"WCDN %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
sayzyas	11:ff06edc0219c	1213	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1214	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1215	}
sayzyas	10:a2bd7d07c7f8	1216	else if( flg_exp_status & 0x02000000 ){ // Winch up (RVS)
sayzyas	13:2c70c772fe24	1217	sprintf(dbuffer,"WCUP %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
sayzyas	11:ff06edc0219c	1218	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1219	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1220	}
sayzyas	10:a2bd7d07c7f8	1221	else if( flg_exp_status & 0x0000000f ){
sayzyas	13:2c70c772fe24	1222	if( cnt == 3 ){
sayzyas	13:2c70c772fe24	1223	sprintf( dbuffer,"JSMD %03d %03d\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid );
sayzyas	11:ff06edc0219c	1224	cnt = 0;
sayzyas	11:ff06edc0219c	1225	}
sayzyas	11:ff06edc0219c	1226	else{
sayzyas	13:2c70c772fe24	1227	sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
sayzyas	11:ff06edc0219c	1228	}
sayzyas	11:ff06edc0219c	1229	cnt++;
sayzyas	11:ff06edc0219c	1230	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1231	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1232	}
sayzyas	10:a2bd7d07c7f8	1233	else{
sayzyas	11:ff06edc0219c	1234	sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
sayzyas	10:a2bd7d07c7f8	1235	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1236	}
sayzyas	10:a2bd7d07c7f8	1237	/* Send data to client PC */
sayzyas	10:a2bd7d07c7f8	1238	udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
sayzyas	10:a2bd7d07c7f8	1239	Thread::wait(10);
sayzyas	13:2c70c772fe24	1240
sayzyas	13:2c70c772fe24	1241	// ----------------------------------------------------------------------------
sayzyas	13:2c70c772fe24	1242	// Read target(transform controller) status in each time.
sayzyas	13:2c70c772fe24	1243	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 4 );
sayzyas	13:2c70c772fe24	1244	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	1245	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	1246	limitSw_Sts = I2C_res[3];
sayzyas	13:2c70c772fe24	1247	// DEBUG_PRINT_L2( "Bd0> X: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	1248	// DEBUG_PRINT_L2( "Bd0> X: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	1249	// DEBUG_PRINT_L2( "Bd0> X: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	13:2c70c772fe24	1250	// ----------------------------------------------------------------------------
sayzyas	13:2c70c772fe24	1251
sayzyas	10:a2bd7d07c7f8	1252	}
sayzyas	10:a2bd7d07c7f8	1253	}
sayzyas	10:a2bd7d07c7f8	1254	Thread::wait(100); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1255	led4 = 0;
sayzyas	10:a2bd7d07c7f8	1256	Thread::wait(100); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1257	}
sayzyas	10:a2bd7d07c7f8	1258	}
sayzyas	10:a2bd7d07c7f8	1259
sayzyas	10:a2bd7d07c7f8	1260	// **************************************************************
sayzyas	10:a2bd7d07c7f8	1261	// TASK: GamaPad Task
sayzyas	10:a2bd7d07c7f8	1262	//
sayzyas	10:a2bd7d07c7f8	1263	// **************************************************************
sayzyas	10:a2bd7d07c7f8	1264	void gamepad_task(void const *)
sayzyas	10:a2bd7d07c7f8	1265	{
sayzyas	10:a2bd7d07c7f8	1266
sayzyas	10:a2bd7d07c7f8	1267	char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
sayzyas	10:a2bd7d07c7f8	1268
sayzyas	10:a2bd7d07c7f8	1269	// int counter = 0;
sayzyas	10:a2bd7d07c7f8	1270	// USB HOST GAMEPAD
sayzyas	10:a2bd7d07c7f8	1271	USBHostGamepad gamepad;
sayzyas	10:a2bd7d07c7f8	1272
sayzyas	10:a2bd7d07c7f8	1273	led1 = 1;
sayzyas	10:a2bd7d07c7f8	1274
sayzyas	10:a2bd7d07c7f8	1275	while(1) {
sayzyas	10:a2bd7d07c7f8	1276
sayzyas	10:a2bd7d07c7f8	1277	// try to connect a USB Gamepad
sayzyas	10:a2bd7d07c7f8	1278	while(!gamepad.connect()) {
sayzyas	10:a2bd7d07c7f8	1279	flg_gamePad_Connected = 0;
sayzyas	10:a2bd7d07c7f8	1280	led2 = OFF;
sayzyas	10:a2bd7d07c7f8	1281	// led1 = 1;
sayzyas	10:a2bd7d07c7f8	1282	Thread::wait(500);
sayzyas	10:a2bd7d07c7f8	1283	}
sayzyas	10:a2bd7d07c7f8	1284
sayzyas	10:a2bd7d07c7f8	1285	// when connected, attach handler called on gamepad event
sayzyas	10:a2bd7d07c7f8	1286	gamepad.attachEvent(onControl);
sayzyas	10:a2bd7d07c7f8	1287
sayzyas	10:a2bd7d07c7f8	1288	// wait until the Gamepad is disconnected
sayzyas	10:a2bd7d07c7f8	1289	while(gamepad.connected()) {
sayzyas	10:a2bd7d07c7f8	1290	flg_gamePad_Connected = 1;
sayzyas	10:a2bd7d07c7f8	1291	led2 = !led2;
sayzyas	10:a2bd7d07c7f8	1292	led2 = ON;
sayzyas	10:a2bd7d07c7f8	1293	// led1 = 1;
sayzyas	10:a2bd7d07c7f8	1294
sayzyas	11:ff06edc0219c	1295	// Send status to Handy Ctrl controller, but currently this is only for Main Controller.
sayzyas	10:a2bd7d07c7f8	1296	I2C_cmd[4] = 0x00;
sayzyas	10:a2bd7d07c7f8	1297	I2C_cmd[5] = 0x01;
sayzyas	13:2c70c772fe24	1298	i2c.write(I2C_ADDRESS_HANDY, I2C_cmd, NumberOfI2CCommand);
sayzyas	13:2c70c772fe24	1299	// DEBUG_PRINT_L2("Bd0> Send Handy Controller(%02x) >>>>>>>>>>\r\n",I2C_ADDRESS_HANDY );
sayzyas	10:a2bd7d07c7f8	1300
sayzyas	10:a2bd7d07c7f8	1301	Thread::wait(500);
sayzyas	10:a2bd7d07c7f8	1302	}
sayzyas	10:a2bd7d07c7f8	1303	}
sayzyas	10:a2bd7d07c7f8	1304	}
sayzyas	10:a2bd7d07c7f8	1305
sayzyas	10:a2bd7d07c7f8	1306	// ======================================================================
sayzyas	10:a2bd7d07c7f8	1307	// Write setting value from lpcal file system of mbed
sayzyas	10:a2bd7d07c7f8	1308	// ======================================================================
sayzyas	10:a2bd7d07c7f8	1309	void write_Setting_lfs( void )
sayzyas	10:a2bd7d07c7f8	1310	{
sayzyas	10:a2bd7d07c7f8	1311	FILE *wfp;
sayzyas	10:a2bd7d07c7f8	1312
sayzyas	11:ff06edc0219c	1313	flg_mutex.lock();
sayzyas	11:ff06edc0219c	1314	DEBUG_PRINT_L3("\r\nBd0> Local file system write ... ");
sayzyas	10:a2bd7d07c7f8	1315	wfp = fopen("/local/set.dat", "wb"); // Open local file "set.txt" for writing
sayzyas	10:a2bd7d07c7f8	1316	if(!wfp){
sayzyas	11:ff06edc0219c	1317	DEBUG_PRINT_L3("Bd0> !!local file open error!!\r\n");
sayzyas	10:a2bd7d07c7f8	1318	}
sayzyas	10:a2bd7d07c7f8	1319	else{
sayzyas	10:a2bd7d07c7f8	1320	Thread::wait(200);
sayzyas	10:a2bd7d07c7f8	1321	fwrite( &setValue, sizeof( int8_t ),sizeof(setValue), wfp );
sayzyas	10:a2bd7d07c7f8	1322	Thread::wait(200);
sayzyas	10:a2bd7d07c7f8	1323	fclose(wfp);
sayzyas	10:a2bd7d07c7f8	1324
sayzyas	11:ff06edc0219c	1325	DEBUG_PRINT_L3("file writen\r\n");
sayzyas	10:a2bd7d07c7f8	1326	}
sayzyas	11:ff06edc0219c	1327	flg_mutex.unlock();
sayzyas	10:a2bd7d07c7f8	1328	}
sayzyas	10:a2bd7d07c7f8	1329
sayzyas	10:a2bd7d07c7f8	1330	// ======================================================================
sayzyas	10:a2bd7d07c7f8	1331	// Read setting value from lpcal file system of mbed
sayzyas	10:a2bd7d07c7f8	1332	// ======================================================================
sayzyas	10:a2bd7d07c7f8	1333	int read_Setting_lfs( void )
sayzyas	10:a2bd7d07c7f8	1334	{
sayzyas	10:a2bd7d07c7f8	1335	FILE *rfp;
sayzyas	11:ff06edc0219c	1336	int rts;
sayzyas	10:a2bd7d07c7f8	1337
sayzyas	11:ff06edc0219c	1338	//setValue_t lsetValue; // Setting Data
sayzyas	11:ff06edc0219c	1339
sayzyas	11:ff06edc0219c	1340	DEBUG_PRINT_L3("\r\nBd0> Read Setting data from local file system \r\n");
sayzyas	11:ff06edc0219c	1341	DEBUG_PRINT_L3( "Bd0> ---------------------------------------- \r\n");
sayzyas	10:a2bd7d07c7f8	1342	rfp = fopen("/local/set.dat", "rb"); // Open local file "set.txt" for writing
sayzyas	10:a2bd7d07c7f8	1343	if(!rfp){
sayzyas	11:ff06edc0219c	1344	DEBUG_PRINT_L3("Bd0> !!local file open error!!\r\n");
sayzyas	11:ff06edc0219c	1345	rts = _FAIL_;
sayzyas	10:a2bd7d07c7f8	1346	}
sayzyas	10:a2bd7d07c7f8	1347	else{
sayzyas	11:ff06edc0219c	1348	Thread::wait(100);
sayzyas	11:ff06edc0219c	1349	DEBUG_PRINT_L3("fread\r\n");
sayzyas	11:ff06edc0219c	1350	flg_mutex.lock();
sayzyas	10:a2bd7d07c7f8	1351	fread( (int8_t*)&setValue, sizeof(int8_t), sizeof(setValue), rfp );
sayzyas	11:ff06edc0219c	1352	DEBUG_PRINT_L3("fread done\r\n");
sayzyas	10:a2bd7d07c7f8	1353	dspSetValue2Console(&pc, &setValue);
sayzyas	11:ff06edc0219c	1354	flg_mutex.unlock();
sayzyas	10:a2bd7d07c7f8	1355
sayzyas	11:ff06edc0219c	1356	// Thread::wait(500);
sayzyas	11:ff06edc0219c	1357	DEBUG_PRINT_L3("fclose\r\n");
sayzyas	10:a2bd7d07c7f8	1358	fclose(rfp);
sayzyas	11:ff06edc0219c	1359	DEBUG_PRINT_L3("\r\n");
sayzyas	11:ff06edc0219c	1360
sayzyas	11:ff06edc0219c	1361	rts = _OK_;
sayzyas	11:ff06edc0219c	1362	}
sayzyas	11:ff06edc0219c	1363	flg_mutex.unlock();
sayzyas	11:ff06edc0219c	1364	return rts;
sayzyas	11:ff06edc0219c	1365	}
sayzyas	11:ff06edc0219c	1366
sayzyas	11:ff06edc0219c	1367	// ======================================================================
sayzyas	11:ff06edc0219c	1368	// Read setting value from lpcal file system of mbed
sayzyas	11:ff06edc0219c	1369	// ======================================================================
sayzyas	11:ff06edc0219c	1370	int read_Setting2_lfs( void )
sayzyas	11:ff06edc0219c	1371	{
sayzyas	11:ff06edc0219c	1372	FILE *rfp;
sayzyas	11:ff06edc0219c	1373	int rts;
sayzyas	11:ff06edc0219c	1374
sayzyas	11:ff06edc0219c	1375	// setValue_t lsetValue; // Setting Data
sayzyas	11:ff06edc0219c	1376
sayzyas	11:ff06edc0219c	1377	DEBUG_PRINT_L3("\r\nBd0> Read Setting data from local file system \r\n");
sayzyas	11:ff06edc0219c	1378	DEBUG_PRINT_L3( "Bd0> ---------------------------------------- \r\n");
sayzyas	11:ff06edc0219c	1379	rfp = fopen("/local/set.dat", "rb"); // Open local file "set.txt" for writing
sayzyas	11:ff06edc0219c	1380	if(!rfp){
sayzyas	11:ff06edc0219c	1381	DEBUG_PRINT_L3("Bd0> !!local file open error!!\r\n");
sayzyas	11:ff06edc0219c	1382	rts = _FAIL_;
sayzyas	11:ff06edc0219c	1383	}
sayzyas	11:ff06edc0219c	1384	else{
sayzyas	11:ff06edc0219c	1385	Thread::wait(100);
sayzyas	11:ff06edc0219c	1386	DEBUG_PRINT_L3("fread\r\n");
sayzyas	11:ff06edc0219c	1387	flg_mutex.lock();
sayzyas	11:ff06edc0219c	1388	fread( (int8_t*)&setValue, sizeof(int8_t), sizeof(setValue), rfp );
sayzyas	11:ff06edc0219c	1389	DEBUG_PRINT_L3("fread done\r\n");
sayzyas	11:ff06edc0219c	1390	dspSetValue2Console(&pc, &setValue);
sayzyas	11:ff06edc0219c	1391	flg_mutex.unlock();
sayzyas	11:ff06edc0219c	1392
sayzyas	11:ff06edc0219c	1393	// Thread::wait(500);
sayzyas	11:ff06edc0219c	1394	DEBUG_PRINT_L3("fclose\r\n");
sayzyas	11:ff06edc0219c	1395	fclose(rfp);
sayzyas	11:ff06edc0219c	1396	DEBUG_PRINT_L3("\r\n");
sayzyas	11:ff06edc0219c	1397
sayzyas	11:ff06edc0219c	1398	rts = _OK_;
sayzyas	11:ff06edc0219c	1399	}
sayzyas	11:ff06edc0219c	1400	flg_mutex.unlock();
sayzyas	11:ff06edc0219c	1401	return rts;
sayzyas	10:a2bd7d07c7f8	1402	}
sayzyas	10:a2bd7d07c7f8	1403
sayzyas	10:a2bd7d07c7f8	1404	// ======================================================================
sayzyas	10:a2bd7d07c7f8	1405	// Read Network setting value from lpcal file system of mbed
sayzyas	10:a2bd7d07c7f8	1406	// ======================================================================
sayzyas	10:a2bd7d07c7f8	1407	int read_NetSetting_lfs( char* ip_address, char* subnet_mask, char* gateway )
sayzyas	10:a2bd7d07c7f8	1408	{
sayzyas	10:a2bd7d07c7f8	1409	FILE *rfp;
sayzyas	10:a2bd7d07c7f8	1410	// char ip_address[20];
sayzyas	10:a2bd7d07c7f8	1411	// char subnet_mask[20];
sayzyas	10:a2bd7d07c7f8	1412	// char gateway[20];
sayzyas	10:a2bd7d07c7f8	1413
sayzyas	11:ff06edc0219c	1414	DEBUG_PRINT_L3("\r\nBd0> Read Network Setting data from local file system \r\n");
sayzyas	10:a2bd7d07c7f8	1415	rfp = fopen("/local/net.dat", "r"); // Open local file "set.txt" for writing
sayzyas	10:a2bd7d07c7f8	1416	if(!rfp){
sayzyas	11:ff06edc0219c	1417	DEBUG_PRINT_L3("Bd0> !!local file open error!!\r\n");
sayzyas	10:a2bd7d07c7f8	1418	return _FAIL_;
sayzyas	10:a2bd7d07c7f8	1419	}
sayzyas	10:a2bd7d07c7f8	1420	else{
sayzyas	10:a2bd7d07c7f8	1421	Thread::wait(500);
sayzyas	10:a2bd7d07c7f8	1422
sayzyas	10:a2bd7d07c7f8	1423	fscanf(rfp, "%s", ip_address);
sayzyas	10:a2bd7d07c7f8	1424	fscanf(rfp, "%s", subnet_mask);
sayzyas	10:a2bd7d07c7f8	1425	fscanf(rfp, "%s", gateway);
sayzyas	10:a2bd7d07c7f8	1426
sayzyas	11:ff06edc0219c	1427	DEBUG_PRINT_L3("Bd0> ---------------------------------------\r\n");
sayzyas	11:ff06edc0219c	1428	DEBUG_PRINT_L3("Bd0> ip address : %s\r\n", ip_address);
sayzyas	11:ff06edc0219c	1429	DEBUG_PRINT_L3("Bd0> subnet mask : %s\r\n", subnet_mask);
sayzyas	11:ff06edc0219c	1430	DEBUG_PRINT_L3("Bd0> default gateway: %s\r\n", gateway);
sayzyas	11:ff06edc0219c	1431	DEBUG_PRINT_L3("Bd0> ---------------------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	1432
sayzyas	10:a2bd7d07c7f8	1433	Thread::wait(500);
sayzyas	10:a2bd7d07c7f8	1434	fclose(rfp);
sayzyas	11:ff06edc0219c	1435	DEBUG_PRINT_L3("\r\n");
sayzyas	10:a2bd7d07c7f8	1436
sayzyas	10:a2bd7d07c7f8	1437	return _OK_;
sayzyas	10:a2bd7d07c7f8	1438	}
sayzyas	10:a2bd7d07c7f8	1439	}
sayzyas	10:a2bd7d07c7f8	1440	// ======================================================================
sayzyas	10:a2bd7d07c7f8	1441	// Winch control function
sayzyas	10:a2bd7d07c7f8	1442	// ======================================================================
sayzyas	10:a2bd7d07c7f8	1443	void winchMovingControl(
sayzyas	10:a2bd7d07c7f8	1444	int mode, // Operationg mode: Relative / Abslute
sayzyas	10:a2bd7d07c7f8	1445	char* dbufferP, // Date buffer pointer
sayzyas	10:a2bd7d07c7f8	1446	int dbuffer_s, // Date buffer size
sayzyas	10:a2bd7d07c7f8	1447	winchData_t* winchDataP, // Winch data structure pointer
sayzyas	10:a2bd7d07c7f8	1448	int winchData_s, // Winch data structure size
sayzyas	10:a2bd7d07c7f8	1449	char* I2C_cmd
sayzyas	10:a2bd7d07c7f8	1450	){
sayzyas	10:a2bd7d07c7f8	1451	int rcv_data_cnt;
sayzyas	10:a2bd7d07c7f8	1452	// int moving_data;
sayzyas	10:a2bd7d07c7f8	1453	int man_speed;
sayzyas	13:2c70c772fe24	1454
sayzyas	13:2c70c772fe24	1455	int cnt;
sayzyas	13:2c70c772fe24	1456
sayzyas	13:2c70c772fe24	1457
sayzyas	10:a2bd7d07c7f8	1458	bool flg_stop_operation = false;
sayzyas	10:a2bd7d07c7f8	1459
sayzyas	13:2c70c772fe24	1460	int16_t winchTempPosition;
sayzyas	10:a2bd7d07c7f8	1461
sayzyas	10:a2bd7d07c7f8	1462	char I2C_read[NumberOfI2CCommand+1];
sayzyas	11:ff06edc0219c	1463	char I2C_readcmd[NumberOfI2CCommand+1];
sayzyas	10:a2bd7d07c7f8	1464
sayzyas	10:a2bd7d07c7f8	1465	// if (hwbtn_Opeflg == 1){
sayzyas	10:a2bd7d07c7f8	1466	// Thread::wait(1);
sayzyas	10:a2bd7d07c7f8	1467	// }
sayzyas	10:a2bd7d07c7f8	1468	// else{
sayzyas	10:a2bd7d07c7f8	1469
sayzyas	10:a2bd7d07c7f8	1470
sayzyas	10:a2bd7d07c7f8	1471	if( flg_ButtonOn == true ) {Thread::wait(2);}
sayzyas	10:a2bd7d07c7f8	1472
sayzyas	10:a2bd7d07c7f8	1473	I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
sayzyas	10:a2bd7d07c7f8	1474	I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
sayzyas	10:a2bd7d07c7f8	1475	I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
sayzyas	10:a2bd7d07c7f8	1476	I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
sayzyas	10:a2bd7d07c7f8	1477
sayzyas	10:a2bd7d07c7f8	1478	if( mode == WINCH_POSITION_CLEAR ){
sayzyas	10:a2bd7d07c7f8	1479	led3 = ON;
sayzyas	11:ff06edc0219c	1480	DEBUG_PRINT_L3("Bd0> === WINCH_POSITION_CLEAR ===\r\n");
sayzyas	10:a2bd7d07c7f8	1481	rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
sayzyas	11:ff06edc0219c	1482	DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	1483	if( rcv_data_cnt < 0 ){
sayzyas	11:ff06edc0219c	1484	DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
sayzyas	10:a2bd7d07c7f8	1485	// tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	10:a2bd7d07c7f8	1486	// break;
sayzyas	10:a2bd7d07c7f8	1487	}
sayzyas	10:a2bd7d07c7f8	1488	else{
sayzyas	11:ff06edc0219c	1489	DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
sayzyas	10:a2bd7d07c7f8	1490	// if( !strcmp( dbuffer, "WinchStepDnOf" ) ){
sayzyas	10:a2bd7d07c7f8	1491	// break;
sayzyas	10:a2bd7d07c7f8	1492	// }
sayzyas	10:a2bd7d07c7f8	1493	}
sayzyas	10:a2bd7d07c7f8	1494
sayzyas	10:a2bd7d07c7f8	1495	I2C_cmd[1] = 'Z'; // Zero clear
sayzyas	13:2c70c772fe24	1496	i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1497
sayzyas	10:a2bd7d07c7f8	1498	led3 = OFF;
sayzyas	10:a2bd7d07c7f8	1499	}
sayzyas	10:a2bd7d07c7f8	1500	else if (( mode == WINCH_MMODE_RELATIVE ) \|\| ( mode == WINCH_MMODE_ABSOLUTE )){
sayzyas	11:ff06edc0219c	1501	if ( mode == WINCH_MMODE_RELATIVE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_RELATIVE === \r\n");
sayzyas	11:ff06edc0219c	1502	if ( mode == WINCH_MMODE_ABSOLUTE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_ABSOLUTE === \r\n");
sayzyas	10:a2bd7d07c7f8	1503
sayzyas	10:a2bd7d07c7f8	1504	rcv_data_cnt = tcp_client.receive( dbuffer, dbuffer_s);
sayzyas	10:a2bd7d07c7f8	1505
sayzyas	10:a2bd7d07c7f8	1506	*(dbufferP+rcv_data_cnt) = '\0';
sayzyas	11:ff06edc0219c	1507	winchDataP->operation = '\r\n';
sayzyas	10:a2bd7d07c7f8	1508
sayzyas	11:ff06edc0219c	1509	DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	1510	// Copy received data to Winch data structure.
sayzyas	10:a2bd7d07c7f8	1511	memcpy( winchDataP, (winchData_t *)dbuffer, winchData_s);
sayzyas	10:a2bd7d07c7f8	1512	// winchDataP->dt_WinchDstPosition += winchDataP->dt_WinchCntPosition;
sayzyas	11:ff06edc0219c	1513	DEBUG_PRINT_L3("Bd0> Winch Rtv Move [ From %04d >>> To %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
sayzyas	10:a2bd7d07c7f8	1514
sayzyas	10:a2bd7d07c7f8	1515	swbtn_OpeMutex.lock();
sayzyas	10:a2bd7d07c7f8	1516	swbtn_Opeflg = 1;
sayzyas	10:a2bd7d07c7f8	1517	swbtn_OpeMutex.unlock();
sayzyas	10:a2bd7d07c7f8	1518
sayzyas	13:2c70c772fe24	1519	cnt = 0;
sayzyas	13:2c70c772fe24	1520
sayzyas	10:a2bd7d07c7f8	1521	while( true ){
sayzyas	10:a2bd7d07c7f8	1522	while( true ){
sayzyas	10:a2bd7d07c7f8	1523	led3 = ON;
sayzyas	10:a2bd7d07c7f8	1524	////winchDataP->dt_WinchCntPosition = res_position; // Current position.
sayzyas	10:a2bd7d07c7f8	1525
sayzyas	11:ff06edc0219c	1526	DEBUG_PRINT_L3("Bd0> == Winch Position ==============\r\n");
sayzyas	11:ff06edc0219c	1527	DEBUG_PRINT_L3("Bd0> CURRENT : %d\r\n", winchDataP->dt_WinchCntPosition );
sayzyas	11:ff06edc0219c	1528	DEBUG_PRINT_L3("Bd0> DESTINATION: %d\r\n", winchDataP->dt_WinchDstPosition );
sayzyas	11:ff06edc0219c	1529	DEBUG_PRINT_L3("Bd0> ================================\r\n");
sayzyas	10:a2bd7d07c7f8	1530
sayzyas	10:a2bd7d07c7f8	1531	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	11:ff06edc0219c	1532	DEBUG_PRINT_L3("Bd0> Send Winch Rtv data [ %04d >>>> %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
sayzyas	10:a2bd7d07c7f8	1533
sayzyas	10:a2bd7d07c7f8	1534	rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
sayzyas	11:ff06edc0219c	1535	DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	1536	if( rcv_data_cnt < 0 ){
sayzyas	11:ff06edc0219c	1537	DEBUG_PRINT_L3("##ERROR## Data receive\r\n" );
sayzyas	10:a2bd7d07c7f8	1538
sayzyas	10:a2bd7d07c7f8	1539	// tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	10:a2bd7d07c7f8	1540	break;
sayzyas	10:a2bd7d07c7f8	1541	}
sayzyas	10:a2bd7d07c7f8	1542	else{
sayzyas	11:ff06edc0219c	1543	DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
sayzyas	10:a2bd7d07c7f8	1544	if( !strcmp( dbuffer, "WinchRtvStop" ) ){
sayzyas	10:a2bd7d07c7f8	1545	flg_stop_operation = true;
sayzyas	10:a2bd7d07c7f8	1546	break;
sayzyas	10:a2bd7d07c7f8	1547	}
sayzyas	10:a2bd7d07c7f8	1548	}
sayzyas	13:2c70c772fe24	1549	// Forward rotation : winch down
sayzyas	10:a2bd7d07c7f8	1550	if( winchDataP->dt_WinchCntPosition < winchDataP->dt_WinchDstPosition ){
sayzyas	10:a2bd7d07c7f8	1551	I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
sayzyas	13:2c70c772fe24	1552	if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
sayzyas	13:2c70c772fe24	1553	I2C_cmd[3] = (setValue.winchCtrl.sv_WDM_mst >> 1); // very slow speed
sayzyas	13:2c70c772fe24	1554	}
sayzyas	13:2c70c772fe24	1555	else if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
sayzyas	13:2c70c772fe24	1556	I2C_cmd[3] = setValue.winchCtrl.sv_WDM_mst; // slow speed
sayzyas	10:a2bd7d07c7f8	1557	}
sayzyas	10:a2bd7d07c7f8	1558	else{
sayzyas	13:2c70c772fe24	1559	I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_F; // normal speed
sayzyas	10:a2bd7d07c7f8	1560	}
sayzyas	10:a2bd7d07c7f8	1561	}
sayzyas	13:2c70c772fe24	1562	// Reverse rotation : winch up
sayzyas	13:2c70c772fe24	1563	else if ( winchDataP->dt_WinchCntPosition > winchDataP->dt_WinchDstPosition ){
sayzyas	10:a2bd7d07c7f8	1564	I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
sayzyas	13:2c70c772fe24	1565	if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
sayzyas	13:2c70c772fe24	1566	I2C_cmd[3] = (setValue.winchCtrl.sv_WDM_mrt >> 1); // very slow speed
sayzyas	13:2c70c772fe24	1567	}
sayzyas	13:2c70c772fe24	1568	else if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
sayzyas	13:2c70c772fe24	1569	I2C_cmd[3] = setValue.winchCtrl.sv_WDM_mrt; // slow speed
sayzyas	10:a2bd7d07c7f8	1570	}
sayzyas	10:a2bd7d07c7f8	1571	else{
sayzyas	13:2c70c772fe24	1572	I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_R; // normal speed
sayzyas	10:a2bd7d07c7f8	1573	}
sayzyas	10:a2bd7d07c7f8	1574	}
sayzyas	10:a2bd7d07c7f8	1575
sayzyas	13:2c70c772fe24	1576	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1577	// Read winch current position from Resolver.
sayzyas	13:2c70c772fe24	1578
sayzyas	13:2c70c772fe24	1579	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1580	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1581	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1582	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1583	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1584	}
sayzyas	13:2c70c772fe24	1585	mtx_wcp.lock();
sayzyas	10:a2bd7d07c7f8	1586	winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas	13:2c70c772fe24	1587	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	1588	winchDataP->operation = 0x00;
sayzyas	13:2c70c772fe24	1589	//i2c.read(I2C_ADDRESS_RESOLVER, I2C_resdata, 2); // Read
sayzyas	10:a2bd7d07c7f8	1590	//res_position = (I2C_resdata[1] << 8) \| I2C_resdata[0];
sayzyas	10:a2bd7d07c7f8	1591	// --------------------------------------
sayzyas	10:a2bd7d07c7f8	1592	// Read motor current
sayzyas	10:a2bd7d07c7f8	1593	// --------------------------------------
sayzyas	13:2c70c772fe24	1594	read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
sayzyas	10:a2bd7d07c7f8	1595	winchDataP->dt_WinchMotor1Current = I2C_read[0]; // Motor current set
sayzyas	10:a2bd7d07c7f8	1596	winchDataP->dt_WinchMotor2Current = I2C_read[1]; // Motor current set
sayzyas	10:a2bd7d07c7f8	1597	winchDataP->operation = I2C_read[2];
sayzyas	11:ff06edc0219c	1598	DEBUG_PRINT_L3( "Bd0> 15: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
sayzyas	13:2c70c772fe24	1599	DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
sayzyas	10:a2bd7d07c7f8	1600	led3 = OFF;
sayzyas	13:2c70c772fe24	1601	if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) == 0 ){
sayzyas	13:2c70c772fe24	1602	// if( winchDataP->dt_WinchCntPosition == winchDataP->dt_WinchDstPosition ){
sayzyas	13:2c70c772fe24	1603	DEBUG_PRINT_L3( "Bd0> Current:%d -> Destination:%d\r\n" , winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition);
sayzyas	10:a2bd7d07c7f8	1604	break;
sayzyas	10:a2bd7d07c7f8	1605	}
sayzyas	11:ff06edc0219c	1606	Thread::wait(2); // Time interval for program debugging
sayzyas	13:2c70c772fe24	1607	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1608	}
sayzyas	13:2c70c772fe24	1609
sayzyas	13:2c70c772fe24	1610
sayzyas	13:2c70c772fe24	1611
sayzyas	13:2c70c772fe24	1612	DEBUG_PRINT_L3( "Bd0> ! Winch Stop\r\n" );
sayzyas	10:a2bd7d07c7f8	1613	I2C_cmd[2] = MOTOR_STP; // Motor1 STOP
sayzyas	10:a2bd7d07c7f8	1614	I2C_cmd[3] = 0; // Speed
sayzyas	10:a2bd7d07c7f8	1615	I2C_cmd[6] = MOTOR_STP; // Motor2 STOP
sayzyas	10:a2bd7d07c7f8	1616	I2C_cmd[7] = 0; // Speed
sayzyas	10:a2bd7d07c7f8	1617
sayzyas	13:2c70c772fe24	1618	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	13:2c70c772fe24	1619	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1620
sayzyas	13:2c70c772fe24	1621	// Thread::wait(1000); // Time interval for program debugging
sayzyas	10:a2bd7d07c7f8	1622
sayzyas	13:2c70c772fe24	1623	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1624	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1625	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1626	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1627	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1628	}
sayzyas	13:2c70c772fe24	1629
sayzyas	13:2c70c772fe24	1630
sayzyas	13:2c70c772fe24	1631	mtx_wcp.lock();
sayzyas	10:a2bd7d07c7f8	1632	winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas	13:2c70c772fe24	1633	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	1634	winchDataP->operation = 0x00;
sayzyas	11:ff06edc0219c	1635	DEBUG_PRINT_L3("Bd0> Check destination is same to setting point or not: %d\r\n", winchCurrentPosition);
sayzyas	13:2c70c772fe24	1636	if( winchDataP->dt_WinchDstPosition == winchCurrentPosition ){
sayzyas	13:2c70c772fe24	1637	cnt++;
sayzyas	11:ff06edc0219c	1638	DEBUG_PRINT_L3("Bd0> Destination is same to setting point, then stop operation\r\n" );
sayzyas	13:2c70c772fe24	1639	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1640	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1641	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1642	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1643	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1644	}
sayzyas	13:2c70c772fe24	1645	if ( cnt >= 5 ){
sayzyas	13:2c70c772fe24	1646	break; // When final destination == set point , then break. else adjust position again.
sayzyas	13:2c70c772fe24	1647	}
sayzyas	10:a2bd7d07c7f8	1648	}
sayzyas	13:2c70c772fe24	1649	// Force Stop by Stop button
sayzyas	10:a2bd7d07c7f8	1650	if( flg_stop_operation == true ){
sayzyas	11:ff06edc0219c	1651	DEBUG_PRINT_L3("Bd0> Winch auto operation force stop\r\n" );
sayzyas	10:a2bd7d07c7f8	1652	flg_stop_operation = false;
sayzyas	10:a2bd7d07c7f8	1653	break;
sayzyas	10:a2bd7d07c7f8	1654	}
sayzyas	10:a2bd7d07c7f8	1655	}
sayzyas	13:2c70c772fe24	1656	/*
sayzyas	13:2c70c772fe24	1657	Thread::wait(300); // Time interval for program debugging
sayzyas	13:2c70c772fe24	1658	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1659	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1660	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1661	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1662	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1663	}
sayzyas	13:2c70c772fe24	1664	mtx_wcp.lock();
sayzyas	10:a2bd7d07c7f8	1665	winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas	13:2c70c772fe24	1666	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1667	winchDataP->operation = 0x00;
sayzyas	13:2c70c772fe24	1668	DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
sayzyas	13:2c70c772fe24	1669	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	13:2c70c772fe24	1670
sayzyas	13:2c70c772fe24	1671	Thread::wait(300); // Time interval for program debugging
sayzyas	13:2c70c772fe24	1672	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1673	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1674	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1675	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1676	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1677	}
sayzyas	13:2c70c772fe24	1678	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1679	winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas	13:2c70c772fe24	1680	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1681	winchDataP->operation = 0x00;
sayzyas	13:2c70c772fe24	1682	DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
sayzyas	13:2c70c772fe24	1683	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	13:2c70c772fe24	1684	*/
sayzyas	13:2c70c772fe24	1685	Thread::wait(300); // Time interval for program debugging
sayzyas	13:2c70c772fe24	1686	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1687	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1688	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1689	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1690	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1691	}
sayzyas	13:2c70c772fe24	1692	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1693	winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas	13:2c70c772fe24	1694	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	1695	winchDataP->operation = 0x77;
sayzyas	13:2c70c772fe24	1696	DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
sayzyas	10:a2bd7d07c7f8	1697	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	10:a2bd7d07c7f8	1698
sayzyas	10:a2bd7d07c7f8	1699	swbtn_OpeMutex.lock();
sayzyas	10:a2bd7d07c7f8	1700	swbtn_Opeflg = 0;
sayzyas	10:a2bd7d07c7f8	1701	swbtn_OpeMutex.unlock();
sayzyas	10:a2bd7d07c7f8	1702
sayzyas	10:a2bd7d07c7f8	1703	led3 = OFF;
sayzyas	10:a2bd7d07c7f8	1704	}
sayzyas	10:a2bd7d07c7f8	1705
sayzyas	10:a2bd7d07c7f8	1706	// ----------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	1707	// In case of commad received from PC by TCP connection.
sayzyas	10:a2bd7d07c7f8	1708	// In case of hard ware button pushed is by gamepad task
sayzyas	10:a2bd7d07c7f8	1709	// ----------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	1710	else if (( mode == WINCH_STEPDOWN_BTN_ON )\|\|( mode == WINCH_U_STEPDOWN_BTN_ON )) {
sayzyas	10:a2bd7d07c7f8	1711
sayzyas	11:ff06edc0219c	1712	if ( mode == WINCH_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPDOWN_BTN_ON ===\r\n" );
sayzyas	11:ff06edc0219c	1713	if ( mode == WINCH_U_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPDOWN_BTN_ON ===\r\n" );
sayzyas	10:a2bd7d07c7f8	1714
sayzyas	10:a2bd7d07c7f8	1715	swbtn_OpeMutex.lock();
sayzyas	10:a2bd7d07c7f8	1716	swbtn_Opeflg = 1;
sayzyas	10:a2bd7d07c7f8	1717	swbtn_OpeMutex.unlock();
sayzyas	10:a2bd7d07c7f8	1718	while( 1 ){
sayzyas	10:a2bd7d07c7f8	1719	led3 = ON;
sayzyas	11:ff06edc0219c	1720	DEBUG_PRINT_L3("Bd0> WINCH_STEPDOWN_BTN_ON\r\n");
sayzyas	10:a2bd7d07c7f8	1721	rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
sayzyas	11:ff06edc0219c	1722	DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	1723	if( rcv_data_cnt < 0 ){
sayzyas	11:ff06edc0219c	1724	DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
sayzyas	10:a2bd7d07c7f8	1725	// tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	10:a2bd7d07c7f8	1726	break;
sayzyas	10:a2bd7d07c7f8	1727	}
sayzyas	10:a2bd7d07c7f8	1728	else{
sayzyas	11:ff06edc0219c	1729	DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
sayzyas	10:a2bd7d07c7f8	1730	if(( !strcmp( dbuffer, "WinchStepDnOf" ))\|\|( !strcmp( dbuffer, "WinchuStepDnOf" )) ){
sayzyas	10:a2bd7d07c7f8	1731	break;
sayzyas	10:a2bd7d07c7f8	1732	}
sayzyas	10:a2bd7d07c7f8	1733	}
sayzyas	10:a2bd7d07c7f8	1734
sayzyas	10:a2bd7d07c7f8	1735	if ( mode == WINCH_U_STEPDOWN_BTN_ON ) man_speed = 50;
sayzyas	10:a2bd7d07c7f8	1736	else man_speed = 100;
sayzyas	10:a2bd7d07c7f8	1737
sayzyas	10:a2bd7d07c7f8	1738	I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
sayzyas	10:a2bd7d07c7f8	1739	I2C_cmd[3] = man_speed; // Speed
sayzyas	10:a2bd7d07c7f8	1740
sayzyas	13:2c70c772fe24	1741	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1742
sayzyas	10:a2bd7d07c7f8	1743	led3 = OFF;
sayzyas	10:a2bd7d07c7f8	1744
sayzyas	13:2c70c772fe24	1745	read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
sayzyas	10:a2bd7d07c7f8	1746	winchDataP->dt_WinchMotor1Current = I2C_read[0];
sayzyas	10:a2bd7d07c7f8	1747	winchDataP->dt_WinchMotor2Current = I2C_read[1];
sayzyas	10:a2bd7d07c7f8	1748	winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
sayzyas	11:ff06edc0219c	1749	DEBUG_PRINT_L3( "Bd0> 16: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
sayzyas	10:a2bd7d07c7f8	1750	if( winchDataP->operation == 0x88 ){
sayzyas	10:a2bd7d07c7f8	1751	winchDataP->dt_WinchMotor1Current = 0xFF;
sayzyas	10:a2bd7d07c7f8	1752	}
sayzyas	13:2c70c772fe24	1753	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1754
sayzyas	13:2c70c772fe24	1755	DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
sayzyas	13:2c70c772fe24	1756
sayzyas	13:2c70c772fe24	1757	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1758	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1759	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1760	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1761	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1762	}
sayzyas	13:2c70c772fe24	1763	mtx_wcp.lock();
sayzyas	10:a2bd7d07c7f8	1764	winchDataP->dt_WinchCntPosition = winchCurrentPosition;
sayzyas	13:2c70c772fe24	1765	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	1766	winchDataP->operation = 0x00;
sayzyas	13:2c70c772fe24	1767	// ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x00 );
sayzyas	10:a2bd7d07c7f8	1768	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	11:ff06edc0219c	1769	DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
sayzyas	11:ff06edc0219c	1770	// Thread::wait(2); // Time interval for program debugging
sayzyas	13:2c70c772fe24	1771	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1772	}
sayzyas	11:ff06edc0219c	1773	DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
sayzyas	10:a2bd7d07c7f8	1774	I2C_cmd[2] = MOTOR_STP; // Motor1 FWD
sayzyas	10:a2bd7d07c7f8	1775	I2C_cmd[3] = 0; // Speed
sayzyas	10:a2bd7d07c7f8	1776	I2C_cmd[6] = MOTOR_STP; // Motor2 FWD
sayzyas	10:a2bd7d07c7f8	1777	I2C_cmd[7] = 0; // Speed
sayzyas	10:a2bd7d07c7f8	1778
sayzyas	13:2c70c772fe24	1779	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1780
sayzyas	11:ff06edc0219c	1781	DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
sayzyas	13:2c70c772fe24	1782
sayzyas	13:2c70c772fe24	1783	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1784	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1785	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1786	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1787	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1788	}
sayzyas	13:2c70c772fe24	1789	mtx_wcp.lock();
sayzyas	10:a2bd7d07c7f8	1790	winchDataP->dt_WinchCntPosition = winchCurrentPosition;
sayzyas	13:2c70c772fe24	1791	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	1792	winchDataP->operation = 0x77;
sayzyas	13:2c70c772fe24	1793	//ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x77 );
sayzyas	10:a2bd7d07c7f8	1794	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	13:2c70c772fe24	1795	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1796
sayzyas	10:a2bd7d07c7f8	1797	swbtn_OpeMutex.lock();
sayzyas	10:a2bd7d07c7f8	1798	swbtn_Opeflg = 0;
sayzyas	10:a2bd7d07c7f8	1799	swbtn_OpeMutex.unlock();
sayzyas	10:a2bd7d07c7f8	1800	led3 = OFF;
sayzyas	10:a2bd7d07c7f8	1801	}
sayzyas	10:a2bd7d07c7f8	1802	// ----------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	1803	// In case of commad received from PC by TCP connection.
sayzyas	10:a2bd7d07c7f8	1804	// In case of hard ware button pushed is by gamepad task
sayzyas	10:a2bd7d07c7f8	1805	// ----------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	1806	else if (( mode == WINCH_STEPUP_BTN_ON )\|\|( mode == WINCH_U_STEPUP_BTN_ON )) {
sayzyas	10:a2bd7d07c7f8	1807
sayzyas	11:ff06edc0219c	1808	if ( mode == WINCH_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPUP_BTN_ON ===\r\n" );
sayzyas	11:ff06edc0219c	1809	if ( mode == WINCH_U_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPUP_BTN_ON ===\r\n" );
sayzyas	10:a2bd7d07c7f8	1810
sayzyas	10:a2bd7d07c7f8	1811	swbtn_OpeMutex.lock();
sayzyas	10:a2bd7d07c7f8	1812	swbtn_Opeflg = 1;
sayzyas	10:a2bd7d07c7f8	1813	swbtn_OpeMutex.unlock();
sayzyas	10:a2bd7d07c7f8	1814	while( 1 ){
sayzyas	10:a2bd7d07c7f8	1815	led3 = ON;
sayzyas	11:ff06edc0219c	1816	DEBUG_PRINT_L3("Bd0> WINCH_STEPUP_BTN_ON\r\n");
sayzyas	10:a2bd7d07c7f8	1817	rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
sayzyas	11:ff06edc0219c	1818	DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	1819	if( rcv_data_cnt < 0 ){
sayzyas	11:ff06edc0219c	1820	DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
sayzyas	10:a2bd7d07c7f8	1821	// tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	10:a2bd7d07c7f8	1822	break;
sayzyas	10:a2bd7d07c7f8	1823	}
sayzyas	10:a2bd7d07c7f8	1824	else{
sayzyas	11:ff06edc0219c	1825	DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
sayzyas	10:a2bd7d07c7f8	1826	if(
sayzyas	11:ff06edc0219c	1827	( !strcmp( dbuffer, "WinchStepUpOf" ))\|\|(!strcmp( dbuffer, "WinchuStepUpOf" )) ){
sayzyas	10:a2bd7d07c7f8	1828	break;
sayzyas	10:a2bd7d07c7f8	1829	}
sayzyas	10:a2bd7d07c7f8	1830	}
sayzyas	10:a2bd7d07c7f8	1831
sayzyas	10:a2bd7d07c7f8	1832	if ( mode == WINCH_U_STEPUP_BTN_ON ) man_speed = 50;
sayzyas	10:a2bd7d07c7f8	1833	else man_speed = 100;
sayzyas	10:a2bd7d07c7f8	1834
sayzyas	10:a2bd7d07c7f8	1835	I2C_cmd[2] = MOTOR_RVS; // Motor1 FWD
sayzyas	10:a2bd7d07c7f8	1836	I2C_cmd[3] = man_speed; // Speed
sayzyas	10:a2bd7d07c7f8	1837
sayzyas	13:2c70c772fe24	1838	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1839
sayzyas	10:a2bd7d07c7f8	1840	led3 = OFF;
sayzyas	10:a2bd7d07c7f8	1841
sayzyas	13:2c70c772fe24	1842	read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
sayzyas	10:a2bd7d07c7f8	1843	winchDataP->dt_WinchMotor1Current = I2C_read[0];
sayzyas	10:a2bd7d07c7f8	1844	winchDataP->dt_WinchMotor2Current = I2C_read[1];
sayzyas	10:a2bd7d07c7f8	1845	winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
sayzyas	11:ff06edc0219c	1846	DEBUG_PRINT_L3( "Bd0> 17: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
sayzyas	10:a2bd7d07c7f8	1847	if( winchDataP->operation == 0x88 ){
sayzyas	10:a2bd7d07c7f8	1848	winchDataP->dt_WinchMotor1Current = 0xFF;
sayzyas	10:a2bd7d07c7f8	1849	}
sayzyas	13:2c70c772fe24	1850	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1851
sayzyas	13:2c70c772fe24	1852	DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
sayzyas	13:2c70c772fe24	1853
sayzyas	13:2c70c772fe24	1854	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1855	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1856	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1857	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1858	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1859	}
sayzyas	13:2c70c772fe24	1860	mtx_wcp.lock();
sayzyas	10:a2bd7d07c7f8	1861	winchDataP->dt_WinchCntPosition = winchCurrentPosition;
sayzyas	13:2c70c772fe24	1862	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	1863	winchDataP->operation = 0x00;
sayzyas	13:2c70c772fe24	1864	// ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x00 );
sayzyas	10:a2bd7d07c7f8	1865	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	11:ff06edc0219c	1866	DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
sayzyas	11:ff06edc0219c	1867	// Thread::wait(2); // Time interval for program debugging
sayzyas	13:2c70c772fe24	1868	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1869	}
sayzyas	11:ff06edc0219c	1870	DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
sayzyas	10:a2bd7d07c7f8	1871	I2C_cmd[2] = MOTOR_STP; // Motor1 FWD
sayzyas	10:a2bd7d07c7f8	1872	I2C_cmd[3] = 0; // Speed
sayzyas	10:a2bd7d07c7f8	1873	I2C_cmd[6] = MOTOR_STP; // Motor2 FWD
sayzyas	10:a2bd7d07c7f8	1874	I2C_cmd[7] = 0; // Speed
sayzyas	10:a2bd7d07c7f8	1875
sayzyas	13:2c70c772fe24	1876	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	11:ff06edc0219c	1877
sayzyas	11:ff06edc0219c	1878	DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
sayzyas	13:2c70c772fe24	1879	if( flg_ButtonOn == false ){
sayzyas	13:2c70c772fe24	1880	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1881	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1882	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1883	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1884	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1885	}
sayzyas	13:2c70c772fe24	1886	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1887	winchDataP->dt_WinchCntPosition = winchCurrentPosition;
sayzyas	13:2c70c772fe24	1888	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1889	winchDataP->operation = 0x77;
sayzyas	13:2c70c772fe24	1890	// ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x77 );
sayzyas	13:2c70c772fe24	1891	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	13:2c70c772fe24	1892	}
sayzyas	13:2c70c772fe24	1893	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1894
sayzyas	10:a2bd7d07c7f8	1895	swbtn_OpeMutex.lock();
sayzyas	10:a2bd7d07c7f8	1896	swbtn_Opeflg = 0;
sayzyas	10:a2bd7d07c7f8	1897	swbtn_OpeMutex.unlock();
sayzyas	10:a2bd7d07c7f8	1898
sayzyas	10:a2bd7d07c7f8	1899	led3 = OFF;
sayzyas	10:a2bd7d07c7f8	1900	}
sayzyas	10:a2bd7d07c7f8	1901
sayzyas	10:a2bd7d07c7f8	1902	else {
sayzyas	11:ff06edc0219c	1903	// DEBUG_PRINT_L3("STEPSTEPSTEPSTEPSTEP\r\n");
sayzyas	10:a2bd7d07c7f8	1904	}
sayzyas	10:a2bd7d07c7f8	1905	// }
sayzyas	10:a2bd7d07c7f8	1906	}
sayzyas	10:a2bd7d07c7f8	1907
sayzyas	10:a2bd7d07c7f8	1908	// **********************************************************************
sayzyas	10:a2bd7d07c7f8	1909	//
sayzyas	10:a2bd7d07c7f8	1910	// Main Function of this program
sayzyas	10:a2bd7d07c7f8	1911	//
sayzyas	10:a2bd7d07c7f8	1912	// **********************************************************************
sayzyas	10:a2bd7d07c7f8	1913	int main()
sayzyas	10:a2bd7d07c7f8	1914	{
sayzyas	10:a2bd7d07c7f8	1915	winchData_t winchData;
sayzyas	10:a2bd7d07c7f8	1916
sayzyas	10:a2bd7d07c7f8	1917	char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
sayzyas	13:2c70c772fe24	1918	// char I2C_res[NumberOfI2CCommand+1] = "\0";
sayzyas	10:a2bd7d07c7f8	1919
sayzyas	13:2c70c772fe24	1920	// char ip_address[20];
sayzyas	13:2c70c772fe24	1921	// char subnet_mask[20];
sayzyas	13:2c70c772fe24	1922	// char gateway[20];
sayzyas	13:2c70c772fe24	1923
sayzyas	13:2c70c772fe24	1924	char* ip_address;
sayzyas	13:2c70c772fe24	1925	char* subnet_mask;
sayzyas	13:2c70c772fe24	1926	char* gateway;
sayzyas	10:a2bd7d07c7f8	1927
sayzyas	10:a2bd7d07c7f8	1928	int ret;
sayzyas	10:a2bd7d07c7f8	1929	int try_cnt;
sayzyas	10:a2bd7d07c7f8	1930	int rcv_data_cnt;
sayzyas	10:a2bd7d07c7f8	1931
sayzyas	10:a2bd7d07c7f8	1932	bool flg_ethernet = false;
sayzyas	10:a2bd7d07c7f8	1933
sayzyas	10:a2bd7d07c7f8	1934	char ttt[20];
sayzyas	13:2c70c772fe24	1935
sayzyas	10:a2bd7d07c7f8	1936	// Set UART(USB) baudrate
sayzyas	10:a2bd7d07c7f8	1937	pc.baud(115200);
sayzyas	10:a2bd7d07c7f8	1938
sayzyas	10:a2bd7d07c7f8	1939	cf_led_demo( &led1, &led2, &led3, &led4, 15, 35 );
sayzyas	10:a2bd7d07c7f8	1940
sayzyas	10:a2bd7d07c7f8	1941	DEBUG_PRINT_L0("\r\n");
sayzyas	11:ff06edc0219c	1942	DEBUG_PRINT_L0("Bd0> +--------------------------------------------------------------\r\n");
sayzyas	11:ff06edc0219c	1943	DEBUG_PRINT_L0("Bd0> \| Project: B2 Crawler Explorer for 1F-1 PCV internal inspection\r\n");
sayzyas	11:ff06edc0219c	1944	DEBUG_PRINT_L0("Bd0> \|---------\r\n");
sayzyas	11:ff06edc0219c	1945	DEBUG_PRINT_L0("Bd0> \| This is: Main Control Program of Main Controller\r\n");
sayzyas	10:a2bd7d07c7f8	1946	DEBUG_PRINT_L0("Bd0> \| Target MCU: mbed LPC1768\r\n");
sayzyas	10:a2bd7d07c7f8	1947	DEBUG_PRINT_L0("Bd0> \| Letest update: %s\r\n", LatestUpDate);
sayzyas	10:a2bd7d07c7f8	1948	DEBUG_PRINT_L0("Bd0> \| Program Revision: %s\r\n", ProgramRevision);
sayzyas	10:a2bd7d07c7f8	1949	DEBUG_PRINT_L0("Bd0> \| Author: %s\r\n", Author);
sayzyas	10:a2bd7d07c7f8	1950	DEBUG_PRINT_L0("Bd0> \| Copyright(C) 2015 %s Allright Reserved\r\n", Company);
sayzyas	11:ff06edc0219c	1951	DEBUG_PRINT_L0("Bd0> ---------------------------------------------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	1952	sprintf( ttt, "%s", ProgramRevision );
sayzyas	10:a2bd7d07c7f8	1953
sayzyas	11:ff06edc0219c	1954	DEBUG_PRINT_L0("Bd0> Start ststem initializing ...\r\n");
sayzyas	11:ff06edc0219c	1955	DEBUG_PRINT_L0("Bd0> ------------------------\r\n");
sayzyas	11:ff06edc0219c	1956	DEBUG_PRINT_L0("Bd0> Initalizing Ethernet ...\r\n");
sayzyas	11:ff06edc0219c	1957	DEBUG_PRINT_L0("Bd0> ------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	1958
sayzyas	13:2c70c772fe24	1959	//read_NetSetting_lfs( ip_address, subnet_mask, gateway );
sayzyas	13:2c70c772fe24	1960
sayzyas	13:2c70c772fe24	1961	ip_address = "192.168.11.24";
sayzyas	13:2c70c772fe24	1962	subnet_mask = "255.255.255.0";
sayzyas	13:2c70c772fe24	1963	gateway = "192.168.11.1";
sayzyas	10:a2bd7d07c7f8	1964
sayzyas	11:ff06edc0219c	1965	DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
sayzyas	11:ff06edc0219c	1966	DEBUG_PRINT_L0("Bd0> ip address : %s\r\n", ip_address);
sayzyas	11:ff06edc0219c	1967	DEBUG_PRINT_L0("Bd0> subnet mask : %s\r\n", subnet_mask);
sayzyas	11:ff06edc0219c	1968	DEBUG_PRINT_L0("Bd0> default gateway: %s\r\n", gateway);
sayzyas	11:ff06edc0219c	1969	DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	1970
sayzyas	10:a2bd7d07c7f8	1971	#ifdef __ETERNET_DHCP__
sayzyas	10:a2bd7d07c7f8	1972	ret = eth.init(); // Use DHCP
sayzyas	10:a2bd7d07c7f8	1973	#else // __ETERNET_DHCP__
sayzyas	10:a2bd7d07c7f8	1974	ret = eth.init(
sayzyas	10:a2bd7d07c7f8	1975	ip_address, // const char* ip,
sayzyas	10:a2bd7d07c7f8	1976	subnet_mask, // const char* mask,
sayzyas	10:a2bd7d07c7f8	1977	gateway // const char* gateway
sayzyas	10:a2bd7d07c7f8	1978	);
sayzyas	10:a2bd7d07c7f8	1979	#endif // __ETERNET_DHCP__
sayzyas	10:a2bd7d07c7f8	1980	if( ret == 0 ){
sayzyas	11:ff06edc0219c	1981	DEBUG_PRINT_L0("Bd0> Eternet init ... OK\r\n");
sayzyas	10:a2bd7d07c7f8	1982	ret = eth.connect();
sayzyas	10:a2bd7d07c7f8	1983	if( ret == 0 ){
sayzyas	10:a2bd7d07c7f8	1984	cf_led_onoff( &led1,&led2,&led3,&led4, false, false, false, true );
sayzyas	11:ff06edc0219c	1985	DEBUG_PRINT_L0("Bd0> Eternat connect ... OK\r\n");
sayzyas	11:ff06edc0219c	1986	DEBUG_PRINT_L0("Bd0> [ IP Address : %s ]\r\n", eth.getIPAddress());
sayzyas	10:a2bd7d07c7f8	1987	udp_server.bind(UDP_SERVER_PORT);
sayzyas	10:a2bd7d07c7f8	1988	tcp_server.bind(TCP_SERVER_PORT);
sayzyas	10:a2bd7d07c7f8	1989	tcp_server.listen();
sayzyas	10:a2bd7d07c7f8	1990	flg_ethernet = true;
sayzyas	13:2c70c772fe24	1991	led4 = ON; // Ethernet OK
sayzyas	10:a2bd7d07c7f8	1992	}
sayzyas	10:a2bd7d07c7f8	1993	else{
sayzyas	10:a2bd7d07c7f8	1994	cf_led_error( &led1,&led2,&led3,&led4 );
sayzyas	11:ff06edc0219c	1995	DEBUG_PRINT_L0("Bd0> *ERROR* Eternat connect Fali\r\n");
sayzyas	10:a2bd7d07c7f8	1996	}
sayzyas	10:a2bd7d07c7f8	1997	}
sayzyas	10:a2bd7d07c7f8	1998	else{
sayzyas	11:ff06edc0219c	1999	DEBUG_PRINT_L0("Bd0> * ERROR* Eternat init Fail\r\n");
sayzyas	10:a2bd7d07c7f8	2000	}
sayzyas	10:a2bd7d07c7f8	2001
sayzyas	13:2c70c772fe24	2002	Thread::wait(50);
sayzyas	10:a2bd7d07c7f8	2003
sayzyas	10:a2bd7d07c7f8	2004	//---------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2005	// Read CrExp setting value from Local File System
sayzyas	10:a2bd7d07c7f8	2006	// setting file "SET.DAT".
sayzyas	10:a2bd7d07c7f8	2007	// When error occured, LED1 will be blinking shortly.
sayzyas	10:a2bd7d07c7f8	2008	//---------------------------------------------------
sayzyas	11:ff06edc0219c	2009	DEBUG_PRINT_L0("Bd0> ---------------------------\r\n");
sayzyas	11:ff06edc0219c	2010	DEBUG_PRINT_L0("Bd0> Read setting value from LFS\r\n");
sayzyas	11:ff06edc0219c	2011	DEBUG_PRINT_L0("Bd0> ---------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	2012	try_cnt = LFS_READ_COUNT;
sayzyas	10:a2bd7d07c7f8	2013	while( 1 ){
sayzyas	10:a2bd7d07c7f8	2014	if( read_Setting_lfs() == _OK_ ) break;
sayzyas	10:a2bd7d07c7f8	2015	else try_cnt -= 1;
sayzyas	10:a2bd7d07c7f8	2016	if( try_cnt == 0 ){
sayzyas	11:ff06edc0219c	2017	DEBUG_PRINT_L0("Bd0> *ERROR* LFS read error\r\n");
sayzyas	10:a2bd7d07c7f8	2018	while(1){
sayzyas	10:a2bd7d07c7f8	2019	led1 = !led1;
sayzyas	10:a2bd7d07c7f8	2020	Thread::wait(30);
sayzyas	10:a2bd7d07c7f8	2021	}
sayzyas	10:a2bd7d07c7f8	2022	}
sayzyas	10:a2bd7d07c7f8	2023	}
sayzyas	11:ff06edc0219c	2024	DEBUG_PRINT_L0("Bd0> LFS read OK\r\n");
sayzyas	13:2c70c772fe24	2025	led3 = ON; // Setting Data Read OK
sayzyas	10:a2bd7d07c7f8	2026
sayzyas	10:a2bd7d07c7f8	2027	//---------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2028	// Checking Targer LCXpresso824-MAX board here.
sayzyas	10:a2bd7d07c7f8	2029	// Send Hello Packet and waiting reply message from
sayzyas	10:a2bd7d07c7f8	2030	// target.
sayzyas	10:a2bd7d07c7f8	2031	// When error occured, LED1 will blinking slowly.
sayzyas	10:a2bd7d07c7f8	2032	//---------------------------------------------------
sayzyas	11:ff06edc0219c	2033	DEBUG_PRINT_L0("--------------------------\r\n");
sayzyas	11:ff06edc0219c	2034	DEBUG_PRINT_L0("Check the target controler\r\n");
sayzyas	11:ff06edc0219c	2035	DEBUG_PRINT_L0("--------------------------\r\n");
sayzyas	11:ff06edc0219c	2036
sayzyas	12:3e6b6fcf540b	2037	/*
sayzyas	10:a2bd7d07c7f8	2038	try_cnt = TARGET_CHECK_COUNT;
sayzyas	10:a2bd7d07c7f8	2039	while(1){
sayzyas	10:a2bd7d07c7f8	2040	// **********************************************************************
sayzyas	10:a2bd7d07c7f8	2041	// **********************************************************************
sayzyas	10:a2bd7d07c7f8	2042	// Check each target motor control 824 board here
sayzyas	10:a2bd7d07c7f8	2043	// currently only one target checked for debugging ...
sayzyas	10:a2bd7d07c7f8	2044	// **********************************************************************
sayzyas	10:a2bd7d07c7f8	2045	// **********************************************************************
sayzyas	10:a2bd7d07c7f8	2046	I2C_cmd[1] = MOTOR_FWD;
sayzyas	13:2c70c772fe24	2047	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	2048	Thread::wait(100);
sayzyas	13:2c70c772fe24	2049	i2c.read(I2C_ADDRESS_WINCH, I2C_res, NumberOfI2CCommand);
sayzyas	11:ff06edc0219c	2050	DEBUG_PRINT_L0("Bd0> Try count down: %d/%d\r\n", try_cnt, TARGET_CHECK_COUNT );
sayzyas	13:2c70c772fe24	2051	DEBUG_PRINT_L0("Bd0> Return from [ i2c address = 0x%2x : %s ]\r\n", I2C_ADDRESS_WINCH, I2C_res);
sayzyas	11:ff06edc0219c	2052	DEBUG_PRINT_L0("Bd0> MCTR1[Winch motor controller] found\r\n");
sayzyas	10:a2bd7d07c7f8	2053	if( I2C_res[0] == 'S' ){
sayzyas	10:a2bd7d07c7f8	2054	break;
sayzyas	10:a2bd7d07c7f8	2055	}
sayzyas	10:a2bd7d07c7f8	2056	else try_cnt -= 1;
sayzyas	10:a2bd7d07c7f8	2057	if( try_cnt == 0 ){
sayzyas	11:ff06edc0219c	2058	DEBUG_PRINT_L0("Bd0> ##ERROR##\r\n");
sayzyas	10:a2bd7d07c7f8	2059	led1 = OFF;
sayzyas	10:a2bd7d07c7f8	2060	while(1){
sayzyas	10:a2bd7d07c7f8	2061	led1 = !led1; // ON
sayzyas	10:a2bd7d07c7f8	2062	Thread::wait(80);
sayzyas	10:a2bd7d07c7f8	2063	led1 = !led1; // OFF
sayzyas	10:a2bd7d07c7f8	2064	Thread::wait(80);
sayzyas	10:a2bd7d07c7f8	2065	led1 = !led1; // ON
sayzyas	10:a2bd7d07c7f8	2066	Thread::wait(80);
sayzyas	10:a2bd7d07c7f8	2067	led1 = !led1; // OFF
sayzyas	10:a2bd7d07c7f8	2068	Thread::wait(80);
sayzyas	10:a2bd7d07c7f8	2069	led1 = !led1; // ON
sayzyas	10:a2bd7d07c7f8	2070	Thread::wait(80);
sayzyas	10:a2bd7d07c7f8	2071	led1 = !led1; // OFF
sayzyas	10:a2bd7d07c7f8	2072	Thread::wait(80);
sayzyas	10:a2bd7d07c7f8	2073	led1 = !led1; // ON
sayzyas	10:a2bd7d07c7f8	2074	Thread::wait(80);
sayzyas	10:a2bd7d07c7f8	2075	led1 = !led1; // OFF
sayzyas	10:a2bd7d07c7f8	2076	Thread::wait(500);
sayzyas	10:a2bd7d07c7f8	2077	}
sayzyas	10:a2bd7d07c7f8	2078	}
sayzyas	10:a2bd7d07c7f8	2079	}
sayzyas	11:ff06edc0219c	2080	DEBUG_PRINT_L0("Bd0> Target system found\r\n");
sayzyas	12:3e6b6fcf540b	2081	*/
sayzyas	13:2c70c772fe24	2082	led2 = ON; // Check target OK
sayzyas	13:2c70c772fe24	2083
sayzyas	13:2c70c772fe24	2084	/* Set basic function default setting */
sayzyas	13:2c70c772fe24	2085	baseOperation.sv_JS_OpeMode = 0;
sayzyas	13:2c70c772fe24	2086	baseOperation.sv_JS_OpeMode = 0;
sayzyas	13:2c70c772fe24	2087	baseOperation.sv_WinchValid = 0;
sayzyas	11:ff06edc0219c	2088
sayzyas	10:a2bd7d07c7f8	2089	/*
sayzyas	10:a2bd7d07c7f8	2090	**************************************************
sayzyas	10:a2bd7d07c7f8	2091	Send Calculation Data to Resolver Controller
sayzyas	10:a2bd7d07c7f8	2092	**************************************************
sayzyas	10:a2bd7d07c7f8	2093	*/
sayzyas	10:a2bd7d07c7f8	2094	DEBUG_PRINT_L0("Bd0> Send Calculation Data to Resolver Controller");
sayzyas	10:a2bd7d07c7f8	2095	I2C_cmd[2] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100>>8)&0xFF); // Dram diameter upper
sayzyas	10:a2bd7d07c7f8	2096	I2C_cmd[3] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100)&0xFF); // Dram diameter lower
sayzyas	10:a2bd7d07c7f8	2097	I2C_cmd[4] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100>>8)&0xFF); // Cable diameter upper
sayzyas	10:a2bd7d07c7f8	2098	I2C_cmd[5] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100)&0xFF); // Cable diameter lower
sayzyas	10:a2bd7d07c7f8	2099	I2C_cmd[6] = setValue.winchCtrl.sv_WRS_RResolution; // Resolver resolution
sayzyas	13:2c70c772fe24	2100	i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	11:ff06edc0219c	2101	DEBUG_PRINT_L0(" ... done\r\n");
sayzyas	10:a2bd7d07c7f8	2102
sayzyas	11:ff06edc0219c	2103	DEBUG_PRINT_L0("Bd0> ----------------------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	2104	/* Max thread count is (may be ..) 2, How can I increase this , I don't know ?? */
sayzyas	10:a2bd7d07c7f8	2105	DEBUG_PRINT_L0("Bd0> Start host interface task ... ");
sayzyas	10:a2bd7d07c7f8	2106	Thread thread_hif(clientPC_interface_task, NULL, osPriorityHigh, 256 * 2);
sayzyas	11:ff06edc0219c	2107	DEBUG_PRINT_L0("\r\nBd0> Start host gamepad task ... ");
sayzyas	10:a2bd7d07c7f8	2108	Thread thread_gpd(gamepad_task, NULL, osPriorityHigh, 256 * 2);
sayzyas	11:ff06edc0219c	2109	DEBUG_PRINT_L0("\r\nBd0> ----------------------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	2110
sayzyas	11:ff06edc0219c	2111	DEBUG_PRINT_L0( "Bd0> ------------------------\r\n");
sayzyas	11:ff06edc0219c	2112	DEBUG_PRINT_L0( "Bd0> Initializing completed !\r\n");
sayzyas	11:ff06edc0219c	2113	DEBUG_PRINT_L0( "Bd0> ------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	2114
sayzyas	13:2c70c772fe24	2115	led4 = OFF;
sayzyas	13:2c70c772fe24	2116	led3 = OFF;
sayzyas	13:2c70c772fe24	2117	led2 = OFF;
sayzyas	10:a2bd7d07c7f8	2118	led1 = ON; // Initializing is OK then Power Indicator LED ON
sayzyas	10:a2bd7d07c7f8	2119
sayzyas	10:a2bd7d07c7f8	2120	I2C_cmd[4] = 0x00;
sayzyas	10:a2bd7d07c7f8	2121	I2C_cmd[5] = 0x01;
sayzyas	13:2c70c772fe24	2122	i2c.write(I2C_ADDRESS_HANDY, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	2123
sayzyas	10:a2bd7d07c7f8	2124	while( true ) {
sayzyas	10:a2bd7d07c7f8	2125	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	2126	// -----------------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2127	// Communicate with client PC program.
sayzyas	10:a2bd7d07c7f8	2128	// TCP connection:
sayzyas	10:a2bd7d07c7f8	2129	// -----------------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2130	if( flg_ethernet == true ) // in case of Ethernet OK
sayzyas	10:a2bd7d07c7f8	2131	{
sayzyas	10:a2bd7d07c7f8	2132	tcp_server.accept(tcp_client);
sayzyas	10:a2bd7d07c7f8	2133	tcp_client.set_blocking(false, 3500); // Timeout after (300) msec
sayzyas	11:ff06edc0219c	2134	DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
sayzyas	13:2c70c772fe24	2135	DEBUG_PRINT_L3("Bd0> TCP Connection from: %s\r\n", tcp_client.get_address());
sayzyas	11:ff06edc0219c	2136	DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	2137
sayzyas	10:a2bd7d07c7f8	2138	while(true){
sayzyas	10:a2bd7d07c7f8	2139
sayzyas	13:2c70c772fe24	2140	// --------------------------------------------------------------
sayzyas	13:2c70c772fe24	2141	// Following instructions are blocking when no ethernat access
sayzyas	13:2c70c772fe24	2142	// --------------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2143	rcv_data_cnt = tcp_client.receive(dbuffer, sizeof(dbuffer));
sayzyas	11:ff06edc0219c	2144	DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	2145	if( rcv_data_cnt < 0 ){
sayzyas	11:ff06edc0219c	2146	// DEBUG_PRINT("## TCP Receive packet fail ##\r\n");
sayzyas	10:a2bd7d07c7f8	2147	break;
sayzyas	10:a2bd7d07c7f8	2148	}
sayzyas	10:a2bd7d07c7f8	2149	else{
sayzyas	10:a2bd7d07c7f8	2150	if( !strcmp( dbuffer, "WinchPositionClear" ) ){
sayzyas	10:a2bd7d07c7f8	2151	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2152	WINCH_POSITION_CLEAR,
sayzyas	10:a2bd7d07c7f8	2153	dbuffer,
sayzyas	10:a2bd7d07c7f8	2154	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2155	&winchData,
sayzyas	10:a2bd7d07c7f8	2156	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2157	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2158	);
sayzyas	10:a2bd7d07c7f8	2159	}
sayzyas	10:a2bd7d07c7f8	2160
sayzyas	10:a2bd7d07c7f8	2161	else if( !strcmp( dbuffer, "WinchRtvStart" ) ){
sayzyas	10:a2bd7d07c7f8	2162	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2163	WINCH_MMODE_RELATIVE,
sayzyas	10:a2bd7d07c7f8	2164	dbuffer,
sayzyas	10:a2bd7d07c7f8	2165	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2166	&winchData,
sayzyas	10:a2bd7d07c7f8	2167	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2168	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2169	);
sayzyas	10:a2bd7d07c7f8	2170	}
sayzyas	10:a2bd7d07c7f8	2171	else if( !strcmp( dbuffer, "WinchAbsStart" ) ){
sayzyas	10:a2bd7d07c7f8	2172	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2173	WINCH_MMODE_ABSOLUTE,
sayzyas	10:a2bd7d07c7f8	2174	dbuffer,
sayzyas	10:a2bd7d07c7f8	2175	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2176	&winchData,
sayzyas	10:a2bd7d07c7f8	2177	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2178	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2179	);
sayzyas	10:a2bd7d07c7f8	2180	}
sayzyas	10:a2bd7d07c7f8	2181	else if( !strcmp( dbuffer, "WinchStepUpOn" ) ){
sayzyas	10:a2bd7d07c7f8	2182	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2183	WINCH_STEPUP_BTN_ON,
sayzyas	10:a2bd7d07c7f8	2184	dbuffer,
sayzyas	10:a2bd7d07c7f8	2185	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2186	&winchData,
sayzyas	10:a2bd7d07c7f8	2187	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2188	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2189	);
sayzyas	10:a2bd7d07c7f8	2190	}
sayzyas	10:a2bd7d07c7f8	2191	else if( !strcmp( dbuffer, "WinchStepUpOf" ) ){
sayzyas	10:a2bd7d07c7f8	2192	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2193	WINCH_STEPUP_BTN_OFF,
sayzyas	10:a2bd7d07c7f8	2194	dbuffer,
sayzyas	10:a2bd7d07c7f8	2195	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2196	&winchData,
sayzyas	10:a2bd7d07c7f8	2197	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2198	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2199	);
sayzyas	10:a2bd7d07c7f8	2200	}
sayzyas	10:a2bd7d07c7f8	2201	else if( !strcmp( dbuffer, "WinchStepDnOn" ) ){
sayzyas	10:a2bd7d07c7f8	2202	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2203	WINCH_STEPDOWN_BTN_ON,
sayzyas	10:a2bd7d07c7f8	2204	dbuffer,
sayzyas	10:a2bd7d07c7f8	2205	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2206	&winchData,
sayzyas	10:a2bd7d07c7f8	2207	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2208	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2209	);
sayzyas	10:a2bd7d07c7f8	2210	}
sayzyas	10:a2bd7d07c7f8	2211	else if( !strcmp( dbuffer, "WinchStepDnOf" ) ){
sayzyas	10:a2bd7d07c7f8	2212	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2213	WINCH_STEPDOWN_BTN_OFF,
sayzyas	10:a2bd7d07c7f8	2214	dbuffer,
sayzyas	10:a2bd7d07c7f8	2215	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2216	&winchData,
sayzyas	10:a2bd7d07c7f8	2217	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2218	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2219	);
sayzyas	10:a2bd7d07c7f8	2220	}
sayzyas	10:a2bd7d07c7f8	2221
sayzyas	10:a2bd7d07c7f8	2222	else if( !strcmp( dbuffer, "WinchuStepUpOn" ) ){
sayzyas	10:a2bd7d07c7f8	2223	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2224	WINCH_U_STEPUP_BTN_ON,
sayzyas	10:a2bd7d07c7f8	2225	dbuffer,
sayzyas	10:a2bd7d07c7f8	2226	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2227	&winchData,
sayzyas	10:a2bd7d07c7f8	2228	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2229	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2230	);
sayzyas	10:a2bd7d07c7f8	2231	}
sayzyas	10:a2bd7d07c7f8	2232	else if( !strcmp( dbuffer, "WinchuStepUpOf" ) ){
sayzyas	10:a2bd7d07c7f8	2233	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2234	WINCH_U_STEPUP_BTN_OFF,
sayzyas	10:a2bd7d07c7f8	2235	dbuffer,
sayzyas	10:a2bd7d07c7f8	2236	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2237	&winchData,
sayzyas	10:a2bd7d07c7f8	2238	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2239	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2240	);
sayzyas	10:a2bd7d07c7f8	2241	}
sayzyas	10:a2bd7d07c7f8	2242	else if( !strcmp( dbuffer, "WinchuStepDnOn" ) ){
sayzyas	10:a2bd7d07c7f8	2243	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2244	WINCH_U_STEPDOWN_BTN_ON,
sayzyas	10:a2bd7d07c7f8	2245	dbuffer,
sayzyas	10:a2bd7d07c7f8	2246	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2247	&winchData,
sayzyas	10:a2bd7d07c7f8	2248	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2249	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2250	);
sayzyas	10:a2bd7d07c7f8	2251	}
sayzyas	10:a2bd7d07c7f8	2252	else if( !strcmp( dbuffer, "WinchuStepDnOf" ) ){
sayzyas	10:a2bd7d07c7f8	2253	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2254	WINCH_U_STEPDOWN_BTN_OFF,
sayzyas	10:a2bd7d07c7f8	2255	dbuffer,
sayzyas	10:a2bd7d07c7f8	2256	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2257	&winchData,
sayzyas	10:a2bd7d07c7f8	2258	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2259	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2260	);
sayzyas	10:a2bd7d07c7f8	2261	}
sayzyas	10:a2bd7d07c7f8	2262
sayzyas	10:a2bd7d07c7f8	2263	else if( !strcmp( dbuffer, "SetValue" ) ){
sayzyas	11:ff06edc0219c	2264	DEBUG_PRINT_L3("Bd0> SetValue Request from client\r\n");
sayzyas	10:a2bd7d07c7f8	2265	rcv_data_cnt = tcp_client.receive( dbuffer, sizeof(dbuffer));
sayzyas	11:ff06edc0219c	2266	DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	2267	memcpy( &setValue, (setValue_t *)dbuffer, sizeof( setValue)) ;
sayzyas	10:a2bd7d07c7f8	2268	// Display setting value list to consol
sayzyas	10:a2bd7d07c7f8	2269	dspSetValue2Console( &pc, &setValue );
sayzyas	10:a2bd7d07c7f8	2270	// Write setting data to local file system
sayzyas	10:a2bd7d07c7f8	2271	// Thread::wait(100);
sayzyas	10:a2bd7d07c7f8	2272	write_Setting_lfs();
sayzyas	10:a2bd7d07c7f8	2273	// Thread::wait(500);
sayzyas	10:a2bd7d07c7f8	2274	// Read setting data from local file system
sayzyas	11:ff06edc0219c	2275	if ( read_Setting2_lfs() == _FAIL_ ){
sayzyas	11:ff06edc0219c	2276	DEBUG_PRINT_L0("Bd0> ### ERROR### Can't read out setting data from lfs\r\n");
sayzyas	10:a2bd7d07c7f8	2277	}
sayzyas	10:a2bd7d07c7f8	2278	}
sayzyas	10:a2bd7d07c7f8	2279	else if(!strcmp( dbuffer, "GetValue" )){
sayzyas	10:a2bd7d07c7f8	2280	DEBUG_PRINT_L3("Bd0> GetValue request from TCP client <-- send");
sayzyas	11:ff06edc0219c	2281	read_Setting2_lfs();
sayzyas	10:a2bd7d07c7f8	2282	// Display setting value list to consol
sayzyas	10:a2bd7d07c7f8	2283	dspSetValue2Console( &pc, &setValue );
sayzyas	10:a2bd7d07c7f8	2284	tcp_client.send_all( (char*)&setValue, sizeof(setValue));
sayzyas	11:ff06edc0219c	2285	DEBUG_PRINT_L2("(%d)\r\n", sizeof(setValue));
sayzyas	10:a2bd7d07c7f8	2286	}
sayzyas	10:a2bd7d07c7f8	2287	}
sayzyas	10:a2bd7d07c7f8	2288	if( rcv_data_cnt <= 0 ) break;
sayzyas	10:a2bd7d07c7f8	2289	}
sayzyas	10:a2bd7d07c7f8	2290	tcp_client.close();
sayzyas	10:a2bd7d07c7f8	2291	}
sayzyas	10:a2bd7d07c7f8	2292	}
sayzyas	10:a2bd7d07c7f8	2293	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	26 Jul 2018
Imports:	0
Forks:	0
Commits:	17
Dependents:	0
Dependencies:	5
Followers:	1

0_main.cpp@13:2c70c772fe24, 2016-03-28 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning