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@14:3a5ae61ab1f4, 2016-04-14 (annotated)

Committer:: sayzyas
Date:: Thu Apr 14 10:25:08 2016 +0000
Revision:: 14:3a5ae61ab1f4
Parent:: 13:2c70c772fe24
Child:: 15:01680ed6b799

2016.04.14

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	14:3a5ae61ab1f4	65	LocalFileSystem local("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	char dbuffer[128];
sayzyas	13:2c70c772fe24	72	uint8_t motor_speed = 0;
sayzyas	14:3a5ae61ab1f4	73
sayzyas	14:3a5ae61ab1f4	74	// Global Parameter of setting
sayzyas	14:3a5ae61ab1f4	75	setValue_t setValue; // Setting Data
sayzyas	14:3a5ae61ab1f4	76
sayzyas	10:a2bd7d07c7f8	77	/* Status flag */
sayzyas	10:a2bd7d07c7f8	78	/*
sayzyas	10:a2bd7d07c7f8	79	0000 0000 : button LI LK RI RK PCW PCCW TU TD
sayzyas	10:a2bd7d07c7f8	80	0000 0000 : limit switch
sayzyas	10:a2bd7d07c7f8	81	0000 0000 : res
sayzyas	10:a2bd7d07c7f8	82	0000 0000 : res
sayzyas	10:a2bd7d07c7f8	83	*/
sayzyas	10:a2bd7d07c7f8	84	uint32_t flg_exp_status = 0;
sayzyas	10:a2bd7d07c7f8	85	Mutex flg_mutex;
sayzyas	10:a2bd7d07c7f8	86	Mutex swbtn_OpeMutex;
sayzyas	10:a2bd7d07c7f8	87	int swbtn_Opeflg = 0;
sayzyas	10:a2bd7d07c7f8	88
sayzyas	13:2c70c772fe24	89	int16_t winchCurrentPosition;
sayzyas	13:2c70c772fe24	90
sayzyas	13:2c70c772fe24	91	Mutex mtx_wcp;
sayzyas	13:2c70c772fe24	92
sayzyas	10:a2bd7d07c7f8	93	bool flg_ButtonOn = false;
sayzyas	14:3a5ae61ab1f4	94	bool flg_lsw_valid = false;
sayzyas	10:a2bd7d07c7f8	95
sayzyas	10:a2bd7d07c7f8	96	int flg_JS_shape_mode = 0;
sayzyas	10:a2bd7d07c7f8	97	int flg_JS_ope_mode = 0;
sayzyas	14:3a5ae61ab1f4	98	int motor1_current_pct;
sayzyas	14:3a5ae61ab1f4	99	int motor2_current_pct;
sayzyas	13:2c70c772fe24	100	uint8_t limitSw_Sts = 0;
sayzyas	14:3a5ae61ab1f4	101	char motorLock_sts = '\0';
sayzyas	10:a2bd7d07c7f8	102
sayzyas	10:a2bd7d07c7f8	103
sayzyas	10:a2bd7d07c7f8	104	/* Prototype */
sayzyas	10:a2bd7d07c7f8	105	void write_Setting_lfs(void);
sayzyas	10:a2bd7d07c7f8	106	int read_Setting_lfs(void);
sayzyas	10:a2bd7d07c7f8	107	void dsp_console_setting_value(void);
sayzyas	10:a2bd7d07c7f8	108	void winchMovingControl( int, char, int, winchData_t, int, char* );
sayzyas	10:a2bd7d07c7f8	109	extern void dspSetValue2Console( Serial, setValue_t );
sayzyas	10:a2bd7d07c7f8	110	extern void lcd_out( TextLCD* ,int, int, char* );
sayzyas	10:a2bd7d07c7f8	111
sayzyas	10:a2bd7d07c7f8	112	// ============================================================
sayzyas	10:a2bd7d07c7f8	113	// Read motor current
sayzyas	10:a2bd7d07c7f8	114	// ============================================================
sayzyas	10:a2bd7d07c7f8	115	void read_motorCurrent(
sayzyas	10:a2bd7d07c7f8	116	int i2c_addr,
sayzyas	11:ff06edc0219c	117	char* I2C_readcmd,
sayzyas	10:a2bd7d07c7f8	118	char* I2C_res,
sayzyas	10:a2bd7d07c7f8	119	int NumberOfI2Cdata
sayzyas	10:a2bd7d07c7f8	120	){
sayzyas	14:3a5ae61ab1f4	121	i2c.read(i2c_addr, I2C_res, NumberOfI2Cdata);
sayzyas	10:a2bd7d07c7f8	122	/*
sayzyas	11:ff06edc0219c	123	DEBUG_PRINT_L0(" ++++++++++++++++++++++++++++++\r\n" );
sayzyas	11:ff06edc0219c	124	DEBUG_PRINT_L0(" Read Motor1 Current [%d]\r\n", I2C_res[0] );
sayzyas	11:ff06edc0219c	125	DEBUG_PRINT_L0(" Read Motor2 Current [%d]\r\n", I2C_res[1] );
sayzyas	11:ff06edc0219c	126	DEBUG_PRINT_L0(" Read [%d]\r\n", I2C_res[2] );
sayzyas	11:ff06edc0219c	127	DEBUG_PRINT_L0(" ++++++++++++++++++++++++++++++\r\n" );
sayzyas	10:a2bd7d07c7f8	128	*/
sayzyas	10:a2bd7d07c7f8	129	}
sayzyas	10:a2bd7d07c7f8	130
sayzyas	10:a2bd7d07c7f8	131	// ============================================================
sayzyas	10:a2bd7d07c7f8	132	// Send Status to PC
sayzyas	10:a2bd7d07c7f8	133	// ============================================================
sayzyas	10:a2bd7d07c7f8	134	void sendStatus2PC( char *cmd, int32_t numberOfCmd ){
sayzyas	10:a2bd7d07c7f8	135	int i;
sayzyas	10:a2bd7d07c7f8	136	led4 = 1;
sayzyas	10:a2bd7d07c7f8	137	for ( i = 0; i < numberOfCmd; i++ ) {
sayzyas	10:a2bd7d07c7f8	138	pc.putc(*cmd++);
sayzyas	10:a2bd7d07c7f8	139	}
sayzyas	10:a2bd7d07c7f8	140	led4 = 0;
sayzyas	10:a2bd7d07c7f8	141	}
sayzyas	10:a2bd7d07c7f8	142
sayzyas	10:a2bd7d07c7f8	143	// ============================================================
sayzyas	10:a2bd7d07c7f8	144	// Read winch current position from Resolver.
sayzyas	10:a2bd7d07c7f8	145	// ============================================================
sayzyas	10:a2bd7d07c7f8	146	int16_t ReadWinchCurrentPosition( int32_t i2c_addr )
sayzyas	10:a2bd7d07c7f8	147	{
sayzyas	10:a2bd7d07c7f8	148	char I2C_data[2];
sayzyas	11:ff06edc0219c	149	int16_t res_position = 0;
sayzyas	11:ff06edc0219c	150	int rts;
sayzyas	10:a2bd7d07c7f8	151
sayzyas	11:ff06edc0219c	152	rts = i2c.read(i2c_addr, I2C_data, 2); // Read
sayzyas	13:2c70c772fe24	153	mtx_wcp.lock();
sayzyas	11:ff06edc0219c	154	if( rts == 0 ){
sayzyas	11:ff06edc0219c	155	res_position = (I2C_data[1] << 8) \| I2C_data[0];
sayzyas	11:ff06edc0219c	156	}
sayzyas	11:ff06edc0219c	157	else{
sayzyas	13:2c70c772fe24	158	res_position = -1;
sayzyas	11:ff06edc0219c	159	}
sayzyas	11:ff06edc0219c	160	// winchCurrentPosition = res_position;
sayzyas	13:2c70c772fe24	161	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	162	return res_position;
sayzyas	10:a2bd7d07c7f8	163	}
sayzyas	10:a2bd7d07c7f8	164
sayzyas	11:ff06edc0219c	165
sayzyas	11:ff06edc0219c	166	char I2C_res[NumberOfI2CCommand+1] = "\0";
sayzyas	10:a2bd7d07c7f8	167	// ============================================================
sayzyas	10:a2bd7d07c7f8	168	// Button control
sayzyas	10:a2bd7d07c7f8	169	// ============================================================
sayzyas	10:a2bd7d07c7f8	170	void onControl(
sayzyas	10:a2bd7d07c7f8	171	uint8_t btn00, uint8_t btn01, uint8_t btn02, uint8_t btn03,
sayzyas	10:a2bd7d07c7f8	172	uint8_t btn04, uint8_t btn05, uint8_t btn06, uint8_t btn07,
sayzyas	10:a2bd7d07c7f8	173	uint8_t btn08, uint8_t btn09, uint8_t btn10, uint8_t btn11,
sayzyas	10:a2bd7d07c7f8	174	uint8_t btn12, uint8_t btn13, uint8_t btn14, uint8_t btn15,
sayzyas	10:a2bd7d07c7f8	175	uint16_t gamePadVID, uint16_t gamePadPID
sayzyas	10:a2bd7d07c7f8	176	){
sayzyas	10:a2bd7d07c7f8	177	/* OLD type *
sayzyas	10:a2bd7d07c7f8	178	* I2C Command (7byte)
sayzyas	10:a2bd7d07c7f8	179	* 0: '#' // Preamble
sayzyas	10:a2bd7d07c7f8	180	* 1: '0' // Target upper
sayzyas	10:a2bd7d07c7f8	181	* 2: '1' // Target lower
sayzyas	10:a2bd7d07c7f8	182	* 3: '0' // Command 1
sayzyas	10:a2bd7d07c7f8	183	* 4: '1/3' // Command 2
sayzyas	10:a2bd7d07c7f8	184	* 5: '0/1'
sayzyas	10:a2bd7d07c7f8	185	*/
sayzyas	10:a2bd7d07c7f8	186
sayzyas	10:a2bd7d07c7f8	187	/* New Type 15.11.06 ~
sayzyas	10:a2bd7d07c7f8	188	[0] :
sayzyas	10:a2bd7d07c7f8	189	[1] :
sayzyas	10:a2bd7d07c7f8	190	[2] :
sayzyas	10:a2bd7d07c7f8	191	[3] :
sayzyas	10:a2bd7d07c7f8	192	[4] : motor 1 direction (A=Forward, B=Reverse, F=Stop)
sayzyas	10:a2bd7d07c7f8	193	[5] : motor 1 speed
sayzyas	10:a2bd7d07c7f8	194	[6] : motor 2 direction (A=Forward, B=Reverse, F=Stop)
sayzyas	10:a2bd7d07c7f8	195	[7] : motor 2 speed <-- New added
sayzyas	10:a2bd7d07c7f8	196	[8] : N/F
sayzyas	10:a2bd7d07c7f8	197	[9] : N/F
sayzyas	10:a2bd7d07c7f8	198	*/
sayzyas	14:3a5ae61ab1f4	199	char I2C_cmd[NumberOfI2CCommand+1] = "#0100000000000";
sayzyas	11:ff06edc0219c	200	char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
sayzyas	10:a2bd7d07c7f8	201
sayzyas	10:a2bd7d07c7f8	202	uint8_t btnStatus_RFK = 0;
sayzyas	10:a2bd7d07c7f8	203	uint8_t btnStatus_RFI = 0;
sayzyas	10:a2bd7d07c7f8	204	uint8_t btnStatus_LBK = 0;
sayzyas	10:a2bd7d07c7f8	205	uint8_t btnStatus_LBI = 0;
sayzyas	10:a2bd7d07c7f8	206	uint8_t btnStatus_WUP = 0;
sayzyas	10:a2bd7d07c7f8	207	uint8_t btnStatus_WDN = 0;
sayzyas	10:a2bd7d07c7f8	208	uint8_t btnStatus_RJSFwdRvs = 0; // R-JS Fwd/Rvs
sayzyas	10:a2bd7d07c7f8	209	uint8_t btnStatus_RJSLftRgt = 0; // R-JS Left/Light
sayzyas	10:a2bd7d07c7f8	210	uint8_t btnStatus_LJSFwdRvs = 0; // L-JS Fwd/Rvs
sayzyas	10:a2bd7d07c7f8	211	uint8_t btnStatus_LJSLftRgt = 0; // L-Js Left/Right
sayzyas	10:a2bd7d07c7f8	212
sayzyas	10:a2bd7d07c7f8	213	uint8_t btnStatus_Start = 0; // Guide button status for ELECOM GamePad
sayzyas	10:a2bd7d07c7f8	214	uint8_t btnStatus_CrossUp = 0;
sayzyas	10:a2bd7d07c7f8	215	uint8_t btnStatus_CrossDn = 0;
sayzyas	10:a2bd7d07c7f8	216	uint8_t btnStatus_CrossRt = 0;
sayzyas	10:a2bd7d07c7f8	217	uint8_t btnStatus_CrossLt = 0;
sayzyas	10:a2bd7d07c7f8	218
sayzyas	10:a2bd7d07c7f8	219	// For JS Ope mode B
sayzyas	10:a2bd7d07c7f8	220	uint8_t btnID_RFK = 0;
sayzyas	10:a2bd7d07c7f8	221	uint8_t btnID_RFI = 0;
sayzyas	10:a2bd7d07c7f8	222	uint8_t btnID_LBK = 0;
sayzyas	10:a2bd7d07c7f8	223	uint8_t btnID_LBI = 0;
sayzyas	13:2c70c772fe24	224	uint8_t btnID_RFLBI = 0; // RF-I and LB-I both button on same time
sayzyas	13:2c70c772fe24	225	uint8_t btnID_RFLBK = 0; // RF-K and LB-K both button on same time
sayzyas	10:a2bd7d07c7f8	226	uint8_t btnID_WUP = 0;
sayzyas	10:a2bd7d07c7f8	227	uint8_t btnID_WDN = 0;
sayzyas	10:a2bd7d07c7f8	228
sayzyas	10:a2bd7d07c7f8	229	uint8_t btnID_Start = 0; // Guide button ID for ELECOM GamePad
sayzyas	10:a2bd7d07c7f8	230	uint8_t btnID_JS_SD = 0; // JS mode Single / Dual
sayzyas	10:a2bd7d07c7f8	231	uint8_t btnID_JD_IK = 0; // JS mode I-Shape / KO-Shape
sayzyas	10:a2bd7d07c7f8	232
sayzyas	10:a2bd7d07c7f8	233	uint8_t btnID_CrossUp = 0;
sayzyas	10:a2bd7d07c7f8	234	uint8_t btnID_CrossDn = 0;
sayzyas	10:a2bd7d07c7f8	235	uint8_t btnID_CrossRt = 0;
sayzyas	10:a2bd7d07c7f8	236	uint8_t btnID_CrossLt = 0;
sayzyas	10:a2bd7d07c7f8	237
sayzyas	10:a2bd7d07c7f8	238	if( gamePadVID == GAMEPAD_VID_RSTHANDY ){ // NRst own original controller
sayzyas	10:a2bd7d07c7f8	239	DEBUG_PRINT_L4("Bd0> [Rst HDY] ");
sayzyas	10:a2bd7d07c7f8	240	btnID_WDN = 0x10;
sayzyas	10:a2bd7d07c7f8	241	btnID_WUP = 0x20;
sayzyas	10:a2bd7d07c7f8	242	btnID_RFK = 0x01;
sayzyas	10:a2bd7d07c7f8	243	btnID_RFI = 0x02;
sayzyas	10:a2bd7d07c7f8	244	btnID_LBK = 0x04;
sayzyas	10:a2bd7d07c7f8	245	btnID_LBI = 0x08;
sayzyas	10:a2bd7d07c7f8	246	btnID_CrossUp = 0;
sayzyas	10:a2bd7d07c7f8	247	btnID_CrossDn = 4;
sayzyas	10:a2bd7d07c7f8	248	btnID_CrossRt = 2;
sayzyas	10:a2bd7d07c7f8	249	btnID_CrossLt = 6;
sayzyas	13:2c70c772fe24	250	// ---- This is temporary setting ------------------------------
sayzyas	13:2c70c772fe24	251	btnID_RFLBI = 192; // RF-I and LB-I both button on same time
sayzyas	13:2c70c772fe24	252	btnID_RFLBK = 48; // RF-K and LB-K both button on same time
sayzyas	13:2c70c772fe24	253	// --------------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	254	btnID_JS_SD = 1; // JS mode Single or Dual
sayzyas	10:a2bd7d07c7f8	255	btnID_JD_IK = 2; // JS mode I-Shape KO-Shape
sayzyas	10:a2bd7d07c7f8	256	if ( gamePadPID == GAMEPAD_PID_RSTHANDY ){
sayzyas	14:3a5ae61ab1f4	257	btnStatus_WDN = btn04;
sayzyas	10:a2bd7d07c7f8	258	btnStatus_WUP = btn04;
sayzyas	10:a2bd7d07c7f8	259	btnStatus_RFK = btn04;
sayzyas	10:a2bd7d07c7f8	260	btnStatus_RFI = btn04;
sayzyas	10:a2bd7d07c7f8	261	btnStatus_LBK = btn04;
sayzyas	10:a2bd7d07c7f8	262	btnStatus_LBI = btn04;
sayzyas	10:a2bd7d07c7f8	263	btnStatus_RJSFwdRvs = btn03; // Assign analog js to this sw input by LPC1768mbed
sayzyas	10:a2bd7d07c7f8	264	btnStatus_RJSLftRgt = btn02; // Assign analog js to this sw input by LPC1768mbed
sayzyas	10:a2bd7d07c7f8	265	btnStatus_LJSFwdRvs = btn01; // Assign analog js to this sw input by LPC1768mbed
sayzyas	10:a2bd7d07c7f8	266	btnStatus_LJSLftRgt = btn00; // Assign analog js to this sw input by LPC1768mbed
sayzyas	10:a2bd7d07c7f8	267	btnStatus_CrossUp = btn06;
sayzyas	10:a2bd7d07c7f8	268	btnStatus_CrossDn = btn06;
sayzyas	10:a2bd7d07c7f8	269	btnStatus_CrossRt = btn06;
sayzyas	10:a2bd7d07c7f8	270	btnStatus_CrossLt = btn06;
sayzyas	14:3a5ae61ab1f4	271	btnStatus_Start = btn05; //
sayzyas	10:a2bd7d07c7f8	272	}
sayzyas	10:a2bd7d07c7f8	273	}
sayzyas	10:a2bd7d07c7f8	274	else if (gamePadVID == GAMEPAD_VID_ELECOM ){
sayzyas	10:a2bd7d07c7f8	275	DEBUG_PRINT_L4("Bd0> [ELECOM] ");
sayzyas	14:3a5ae61ab1f4	276	btnID_WDN = 4;
sayzyas	10:a2bd7d07c7f8	277	btnID_WUP = 2;
sayzyas	10:a2bd7d07c7f8	278	btnID_RFK = 32;
sayzyas	10:a2bd7d07c7f8	279	btnID_RFI = 128;
sayzyas	10:a2bd7d07c7f8	280	btnID_LBK = 16;
sayzyas	10:a2bd7d07c7f8	281	btnID_LBI = 64;
sayzyas	13:2c70c772fe24	282	// ---------------------
sayzyas	13:2c70c772fe24	283	btnID_RFLBI = 192; // RF-I and LB-I both button on same time
sayzyas	13:2c70c772fe24	284	btnID_RFLBK = 48; // RF-K and LB-K both button on same time
sayzyas	13:2c70c772fe24	285	// ---------------------
sayzyas	10:a2bd7d07c7f8	286	btnID_Start = 8; // Guide button ID for ELECOM GamePad
sayzyas	10:a2bd7d07c7f8	287	btnID_CrossUp = 0;
sayzyas	10:a2bd7d07c7f8	288	btnID_CrossDn = 4;
sayzyas	10:a2bd7d07c7f8	289	btnID_CrossRt = 2;
sayzyas	10:a2bd7d07c7f8	290	btnID_CrossLt = 6;
sayzyas	10:a2bd7d07c7f8	291	if ( gamePadPID == GAMEPAD_PID_ELECOM_JCU3613M ){
sayzyas	14:3a5ae61ab1f4	292	btnStatus_WDN = btn04;
sayzyas	10:a2bd7d07c7f8	293	btnStatus_WUP = btn04;
sayzyas	10:a2bd7d07c7f8	294	btnStatus_RFK = btn04;
sayzyas	10:a2bd7d07c7f8	295	btnStatus_RFI = btn04;
sayzyas	10:a2bd7d07c7f8	296	btnStatus_LBK = btn04;
sayzyas	10:a2bd7d07c7f8	297	btnStatus_LBI = btn04;
sayzyas	10:a2bd7d07c7f8	298	btnStatus_RJSFwdRvs = btn03;
sayzyas	10:a2bd7d07c7f8	299	btnStatus_RJSLftRgt = btn02;
sayzyas	10:a2bd7d07c7f8	300	btnStatus_LJSFwdRvs = btn01;
sayzyas	10:a2bd7d07c7f8	301	btnStatus_LJSLftRgt = btn00;
sayzyas	10:a2bd7d07c7f8	302	btnStatus_Start = btn05; // Guide button status for ELECOM GamePad
sayzyas	10:a2bd7d07c7f8	303	btnStatus_CrossUp = btn06;
sayzyas	10:a2bd7d07c7f8	304	btnStatus_CrossDn = btn06;
sayzyas	10:a2bd7d07c7f8	305	btnStatus_CrossRt = btn06;
sayzyas	10:a2bd7d07c7f8	306	btnStatus_CrossLt = btn06;
sayzyas	10:a2bd7d07c7f8	307	}
sayzyas	10:a2bd7d07c7f8	308	}
sayzyas	10:a2bd7d07c7f8	309	else if( gamePadVID == GAMEPAD_VID_LOGICOOL ){
sayzyas	14:3a5ae61ab1f4	310	btnID_WDN = 40;
sayzyas	10:a2bd7d07c7f8	311	btnID_WUP = 136;
sayzyas	10:a2bd7d07c7f8	312	btnID_RFK = 2;
sayzyas	10:a2bd7d07c7f8	313	btnID_RFI = 8;
sayzyas	10:a2bd7d07c7f8	314	btnID_LBK = 1;
sayzyas	10:a2bd7d07c7f8	315	btnID_LBI = 4;
sayzyas	10:a2bd7d07c7f8	316	btnID_Start = 32; // Guide button ID for ELECOM GamePad
sayzyas	10:a2bd7d07c7f8	317	btnID_CrossUp = 0;
sayzyas	10:a2bd7d07c7f8	318	btnID_CrossDn = 4;
sayzyas	10:a2bd7d07c7f8	319	btnID_CrossRt = 2;
sayzyas	10:a2bd7d07c7f8	320	btnID_CrossLt = 6;
sayzyas	10:a2bd7d07c7f8	321	if ( gamePadPID == GAMEPAD_PID_LOGICOOL_F710 ){
sayzyas	10:a2bd7d07c7f8	322	DEBUG_PRINT_L4("Bd0> [LOGI F710] ");
sayzyas	14:3a5ae61ab1f4	323	btnStatus_WDN = btn05;
sayzyas	10:a2bd7d07c7f8	324	btnStatus_WUP = btn05;
sayzyas	10:a2bd7d07c7f8	325	btnStatus_RFK = btn06;
sayzyas	10:a2bd7d07c7f8	326	btnStatus_RFI = btn06;
sayzyas	10:a2bd7d07c7f8	327	btnStatus_LBK = btn06;
sayzyas	10:a2bd7d07c7f8	328	btnStatus_LBI = btn06;
sayzyas	10:a2bd7d07c7f8	329	}
sayzyas	10:a2bd7d07c7f8	330	else if ( gamePadPID == GAMEPAD_PID_LOGICOOL_F310 ){
sayzyas	10:a2bd7d07c7f8	331	DEBUG_PRINT_L4("Bd0> [LOGI F310] ");
sayzyas	14:3a5ae61ab1f4	332	btnStatus_WDN = btn04;
sayzyas	10:a2bd7d07c7f8	333	btnStatus_WUP = btn04;
sayzyas	10:a2bd7d07c7f8	334	btnStatus_RFK = btn05;
sayzyas	10:a2bd7d07c7f8	335	btnStatus_RFI = btn05;
sayzyas	10:a2bd7d07c7f8	336	btnStatus_LBK = btn05;
sayzyas	10:a2bd7d07c7f8	337	btnStatus_LBI = btn05;
sayzyas	10:a2bd7d07c7f8	338	btnStatus_RJSFwdRvs = btn03;
sayzyas	10:a2bd7d07c7f8	339	btnStatus_RJSLftRgt = btn02;
sayzyas	10:a2bd7d07c7f8	340	btnStatus_LJSFwdRvs = btn01;
sayzyas	10:a2bd7d07c7f8	341	btnStatus_LJSLftRgt = btn00;
sayzyas	10:a2bd7d07c7f8	342	btnStatus_Start = btn05; // Guide button status for ELECOM GamePad
sayzyas	10:a2bd7d07c7f8	343	btnStatus_CrossUp = btn04;
sayzyas	10:a2bd7d07c7f8	344	btnStatus_CrossDn = btn04;
sayzyas	10:a2bd7d07c7f8	345	btnStatus_CrossRt = btn04;
sayzyas	10:a2bd7d07c7f8	346	btnStatus_CrossLt = btn04;
sayzyas	10:a2bd7d07c7f8	347	}
sayzyas	10:a2bd7d07c7f8	348	}
sayzyas	10:a2bd7d07c7f8	349	else if ( gamePadVID == GAMEPAD_VID_SANWA){
sayzyas	10:a2bd7d07c7f8	350	DEBUG_PRINT_L4("Bd0> [SANWA] ");
sayzyas	14:3a5ae61ab1f4	351	btnID_WDN = 2;
sayzyas	10:a2bd7d07c7f8	352	btnID_WUP = 4;
sayzyas	10:a2bd7d07c7f8	353	btnID_RFK = 2;
sayzyas	10:a2bd7d07c7f8	354	btnID_RFI = 1;
sayzyas	10:a2bd7d07c7f8	355	btnID_LBK = 128;
sayzyas	10:a2bd7d07c7f8	356	btnID_LBI = 64;
sayzyas	14:3a5ae61ab1f4	357	// ---------------------
sayzyas	14:3a5ae61ab1f4	358	btnID_RFLBI = 80; // RF-I and LB-I both button on same time
sayzyas	14:3a5ae61ab1f4	359	btnID_RFLBK = 40; // RF-K and LB-K both button on same time
sayzyas	14:3a5ae61ab1f4	360	// ---------------------
sayzyas	14:3a5ae61ab1f4	361	btnID_CrossUp = 0;
sayzyas	14:3a5ae61ab1f4	362	btnID_CrossDn = 255;
sayzyas	14:3a5ae61ab1f4	363	btnID_CrossRt = 0;
sayzyas	14:3a5ae61ab1f4	364	btnID_CrossLt = 255;
sayzyas	10:a2bd7d07c7f8	365	if ( gamePadPID == GAMEPAD_PID_SANWA_JYP70US ){
sayzyas	14:3a5ae61ab1f4	366	btnStatus_WDN = btn05;
sayzyas	10:a2bd7d07c7f8	367	btnStatus_WUP = btn05;
sayzyas	10:a2bd7d07c7f8	368	btnStatus_RFK = btn06;
sayzyas	10:a2bd7d07c7f8	369	btnStatus_RFI = btn06;
sayzyas	10:a2bd7d07c7f8	370	btnStatus_LBK = btn05;
sayzyas	10:a2bd7d07c7f8	371	btnStatus_LBI = btn05;
sayzyas	14:3a5ae61ab1f4	372	btnStatus_RJSFwdRvs = btn03; // Assign analog js to this sw input by LPC1768mbed
sayzyas	14:3a5ae61ab1f4	373	btnStatus_RJSLftRgt = btn02; // Assign analog js to this sw input by LPC1768mbed
sayzyas	14:3a5ae61ab1f4	374	btnStatus_LJSFwdRvs = btn01; // Assign analog js to this sw input by LPC1768mbed
sayzyas	14:3a5ae61ab1f4	375	btnStatus_LJSLftRgt = btn00; // Assign analog js to this sw input by LPC1768mbed
sayzyas	14:3a5ae61ab1f4	376	btnStatus_CrossUp = btn01;
sayzyas	14:3a5ae61ab1f4	377	btnStatus_CrossDn = btn01;
sayzyas	14:3a5ae61ab1f4	378	btnStatus_CrossRt = btn00;
sayzyas	14:3a5ae61ab1f4	379	btnStatus_CrossLt = btn00;
sayzyas	10:a2bd7d07c7f8	380	}
sayzyas	10:a2bd7d07c7f8	381	}
sayzyas	10:a2bd7d07c7f8	382
sayzyas	10:a2bd7d07c7f8	383	#ifdef __DISP_GAMAPAD_STATUS_ALL__ // For Debug
sayzyas	10:a2bd7d07c7f8	384	// 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	385	// btn00,btn01,btn02,btn03,btn04,btn05,btn06,btn07,btn08,
sayzyas	10:a2bd7d07c7f8	386	// gamePadVID, gamePadPID);
sayzyas	14:3a5ae61ab1f4	387	DEBUG_PRINT_SW("Bd0> -- Button Status -------------------------------\r\n");
sayzyas	14:3a5ae61ab1f4	388	DEBUG_PRINT_SW("Bd0> 00(%02x) 01(%02x) 02(%02x) 03(%02x)\r\n", btn00,btn01,btn02,btn03);
sayzyas	14:3a5ae61ab1f4	389	DEBUG_PRINT_SW("Bd0> 04(%02x) 05(%02x) 06(%02x) 07(%02x) 08(%02x)\r\n", btn04,btn05,btn06,btn07,btn08);
sayzyas	14:3a5ae61ab1f4	390	DEBUG_PRINT_SW("Bd0> 09(%02x) 10(%02x) 11(%02x) 12(%02x) 13(%02x) 14(%02x) 15(%02x)\r\n", btn09,btn10,btn11,btn12,btn13,btn14,btn15);
sayzyas	14:3a5ae61ab1f4	391	DEBUG_PRINT_SW("Bd0> ------------------------------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	392	#endif
sayzyas	10:a2bd7d07c7f8	393
sayzyas	14:3a5ae61ab1f4	394	I2C_cmd[I2C_CP_M1_DIR] = '\0';
sayzyas	14:3a5ae61ab1f4	395	I2C_cmd[I2C_CP_M1_SPEED] = '\0';
sayzyas	14:3a5ae61ab1f4	396	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = '\0';
sayzyas	14:3a5ae61ab1f4	397	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = '\0';
sayzyas	14:3a5ae61ab1f4	398	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = '\0';
sayzyas	14:3a5ae61ab1f4	399	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = '\0';
sayzyas	14:3a5ae61ab1f4	400	I2C_cmd[I2C_CP_M2_DIR] = '\0';
sayzyas	14:3a5ae61ab1f4	401	I2C_cmd[I2C_CP_M2_SPEED] = '\0';
sayzyas	14:3a5ae61ab1f4	402	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = '\0';
sayzyas	14:3a5ae61ab1f4	403	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = '\0';
sayzyas	14:3a5ae61ab1f4	404	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = '\0';
sayzyas	14:3a5ae61ab1f4	405	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = '\0';
sayzyas	10:a2bd7d07c7f8	406
sayzyas	10:a2bd7d07c7f8	407	int tmpSpeed = 0;
sayzyas	10:a2bd7d07c7f8	408
sayzyas	10:a2bd7d07c7f8	409	if (swbtn_Opeflg == 1){
sayzyas	10:a2bd7d07c7f8	410	Thread::wait(1);
sayzyas	10:a2bd7d07c7f8	411	}
sayzyas	10:a2bd7d07c7f8	412	else{
sayzyas	14:3a5ae61ab1f4	413	if( flg_lsw_valid == true ){
sayzyas	14:3a5ae61ab1f4	414	I2C_cmd[1] = 'V';
sayzyas	14:3a5ae61ab1f4	415	}
sayzyas	14:3a5ae61ab1f4	416	else{
sayzyas	14:3a5ae61ab1f4	417	I2C_cmd[1] = '0';
sayzyas	14:3a5ae61ab1f4	418	}
sayzyas	10:a2bd7d07c7f8	419	if( gamePadVID == GAMEPAD_VID_RSTHANDY ){ // NRst own original controller
sayzyas	10:a2bd7d07c7f8	420	flg_exp_status &= 0xFFFFFFF0;
sayzyas	10:a2bd7d07c7f8	421	if(!( btnStatus_Start & 0x01 )){ // I-Shape
sayzyas	11:ff06edc0219c	422	flg_mutex.lock();
sayzyas	13:2c70c772fe24	423	baseOperation.sv_JS_ShapeMode = 0;
sayzyas	13:2c70c772fe24	424	baseOperation.sv_WinchValid = 0;
sayzyas	11:ff06edc0219c	425	flg_mutex.unlock();
sayzyas	10:a2bd7d07c7f8	426	flg_exp_status \|= 0x00000001;
sayzyas	10:a2bd7d07c7f8	427	}
sayzyas	10:a2bd7d07c7f8	428	else{ // KO-Shape
sayzyas	11:ff06edc0219c	429	flg_mutex.lock();
sayzyas	13:2c70c772fe24	430	baseOperation.sv_JS_ShapeMode = 1;
sayzyas	11:ff06edc0219c	431	flg_mutex.unlock();
sayzyas	10:a2bd7d07c7f8	432	flg_exp_status \|= 0x00000002;
sayzyas	10:a2bd7d07c7f8	433	}
sayzyas	10:a2bd7d07c7f8	434
sayzyas	13:2c70c772fe24	435	if(!(btnStatus_Start & 0x02 )){ // Tfm,crawler part valid
sayzyas	11:ff06edc0219c	436	flg_mutex.lock();
sayzyas	13:2c70c772fe24	437	baseOperation.sv_WinchValid = 0;
sayzyas	11:ff06edc0219c	438	flg_mutex.unlock();
sayzyas	10:a2bd7d07c7f8	439	flg_exp_status \|= 0x00000004;
sayzyas	10:a2bd7d07c7f8	440	}
sayzyas	13:2c70c772fe24	441	else{ // Winch part valid
sayzyas	11:ff06edc0219c	442	flg_mutex.lock();
sayzyas	13:2c70c772fe24	443	baseOperation.sv_WinchValid = 1;
sayzyas	11:ff06edc0219c	444	flg_mutex.unlock();
sayzyas	10:a2bd7d07c7f8	445	flg_exp_status \|= 0x00000008;
sayzyas	10:a2bd7d07c7f8	446	}
sayzyas	11:ff06edc0219c	447	DEBUG_PRINT_L4( "-----------------------------\r\n" );
sayzyas	11:ff06edc0219c	448	DEBUG_PRINT_L4( "%d : %d\r\n",btnStatus_Start, flg_exp_status );
sayzyas	13:2c70c772fe24	449	DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
sayzyas	13:2c70c772fe24	450	DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
sayzyas	11:ff06edc0219c	451	DEBUG_PRINT_L4( "-----------------------------\r\n" );
sayzyas	10:a2bd7d07c7f8	452	}
sayzyas	10:a2bd7d07c7f8	453	else{
sayzyas	10:a2bd7d07c7f8	454	/*
sayzyas	10:a2bd7d07c7f8	455	*
sayzyas	10:a2bd7d07c7f8	456	* GamePad software switch
sayzyas	14:3a5ae61ab1f4	457	* Cross button Up on : JS shape mode I
sayzyas	14:3a5ae61ab1f4	458	* Cross button Down on : JS shape mode KO
sayzyas	14:3a5ae61ab1f4	459	* Cross button Right on: Winch part valid
sayzyas	14:3a5ae61ab1f4	460	* Cross button Left on : Crawlerm, Transform part valid
sayzyas	10:a2bd7d07c7f8	461	* 7 6 5 4 3 2 1 0
sayzyas	10:a2bd7d07c7f8	462	* +-+-+-+-+-+-+-+-+
sayzyas	14:3a5ae61ab1f4	463	* \|x\|x\|x\|x\|x\|x\|x\|o\| 1: I-Shape JSmode, 2: K-Shape JSmode, 4: Left part(Crawler, Tfm) part valid, 8: Winch part valid
sayzyas	10:a2bd7d07c7f8	464	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	465	*/
sayzyas	13:2c70c772fe24	466	if ( btnStatus_CrossUp == btnID_CrossUp ){ // I Shape
sayzyas	13:2c70c772fe24	467	flg_mutex.lock();
sayzyas	13:2c70c772fe24	468	baseOperation.sv_JS_ShapeMode = 0;
sayzyas	13:2c70c772fe24	469	flg_mutex.unlock();
sayzyas	13:2c70c772fe24	470	DEBUG_PRINT_L4( "--------------------------------\r\n" );
sayzyas	13:2c70c772fe24	471	DEBUG_PRINT_L4( " I\r\n" );
sayzyas	13:2c70c772fe24	472	DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
sayzyas	13:2c70c772fe24	473	DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
sayzyas	13:2c70c772fe24	474	DEBUG_PRINT_L4( "--------------------------------\r\n" );
sayzyas	13:2c70c772fe24	475	flg_exp_status \|= 0x00000001;
sayzyas	10:a2bd7d07c7f8	476	}
sayzyas	13:2c70c772fe24	477	else if( btnStatus_CrossDn == btnID_CrossDn ){ // KO Shape
sayzyas	13:2c70c772fe24	478	flg_mutex.lock();
sayzyas	13:2c70c772fe24	479	baseOperation.sv_JS_ShapeMode = 1;
sayzyas	13:2c70c772fe24	480	flg_mutex.unlock();
sayzyas	13:2c70c772fe24	481	DEBUG_PRINT_L4( "-------------------------\r\n" );
sayzyas	13:2c70c772fe24	482	DEBUG_PRINT_L4( " KO\r\n" );
sayzyas	13:2c70c772fe24	483	DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
sayzyas	13:2c70c772fe24	484	DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
sayzyas	13:2c70c772fe24	485	DEBUG_PRINT_L4( "-------------------------\r\n" );
sayzyas	13:2c70c772fe24	486	flg_exp_status \|= 0x00000002;
sayzyas	13:2c70c772fe24	487	}
sayzyas	13:2c70c772fe24	488	else if( btnStatus_CrossRt == btnID_CrossLt ){ // Valid Part : Crawler (Left)
sayzyas	13:2c70c772fe24	489	flg_mutex.lock();
sayzyas	13:2c70c772fe24	490	baseOperation.sv_WinchValid = 0;
sayzyas	13:2c70c772fe24	491	flg_mutex.unlock();
sayzyas	13:2c70c772fe24	492	DEBUG_PRINT_L4( "-----------------------------\r\n" );
sayzyas	13:2c70c772fe24	493	DEBUG_PRINT_L4( " Tfm, Crawler\r\n" );
sayzyas	13:2c70c772fe24	494	DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
sayzyas	13:2c70c772fe24	495	DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
sayzyas	13:2c70c772fe24	496	DEBUG_PRINT_L4( "-----------------------------\r\n" );
sayzyas	13:2c70c772fe24	497	flg_exp_status \|= 0x00000004;
sayzyas	13:2c70c772fe24	498	}
sayzyas	13:2c70c772fe24	499	else if( btnStatus_CrossLt == btnID_CrossRt ){ // Valid Part : Winch (Right)
sayzyas	13:2c70c772fe24	500	flg_mutex.lock();
sayzyas	13:2c70c772fe24	501	baseOperation.sv_WinchValid = 1;
sayzyas	13:2c70c772fe24	502	flg_mutex.unlock();
sayzyas	13:2c70c772fe24	503	DEBUG_PRINT_L4( "-----------------------------\r\n" );
sayzyas	13:2c70c772fe24	504	DEBUG_PRINT_L4( " Winch\r\n" );
sayzyas	13:2c70c772fe24	505	DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", baseOperation.sv_JS_ShapeMode);
sayzyas	13:2c70c772fe24	506	DEBUG_PRINT_L4( " Valid part : %d\r\n", baseOperation.sv_WinchValid );
sayzyas	13:2c70c772fe24	507	DEBUG_PRINT_L4( "-----------------------------\r\n" );
sayzyas	13:2c70c772fe24	508	flg_exp_status \|= 0x00000008;
sayzyas	13:2c70c772fe24	509	}
sayzyas	10:a2bd7d07c7f8	510	else{
sayzyas	10:a2bd7d07c7f8	511	flg_exp_status &= 0xFFFFFFF0;
sayzyas	13:2c70c772fe24	512	}
sayzyas	13:2c70c772fe24	513	}
sayzyas	13:2c70c772fe24	514	/*
sayzyas	13:2c70c772fe24	515	// ====================================
sayzyas	13:2c70c772fe24	516	// TRANSFORM Motor Control
sayzyas	13:2c70c772fe24	517	// ====================================
sayzyas	13:2c70c772fe24	518	* 7 6 5 4 3 2 1 0
sayzyas	13:2c70c772fe24	519	* +-+-+-+-+-+-+-+-+
sayzyas	13:2c70c772fe24	520	* \|o\|x\|x\|x\|x\|x\|x\|x\| 1: RF-I, 2: RF-K, 4: LB-I, 8: LB-K
sayzyas	13:2c70c772fe24	521	* +-+-+-+-+-+-+-+-+
sayzyas	13:2c70c772fe24	522	*/
sayzyas	13:2c70c772fe24	523
sayzyas	13:2c70c772fe24	524	if ((btnStatus_RFK==btnID_RFLBK)&&(baseOperation.sv_WinchValid==0)){ // Both sw on
sayzyas	13:2c70c772fe24	525	flg_ButtonOn = true;
sayzyas	13:2c70c772fe24	526	DEBUG_PRINT_L3( "Bd0> BTN RF-K & LB-K\r\n" );
sayzyas	13:2c70c772fe24	527	led3 = 1;
sayzyas	13:2c70c772fe24	528	#ifdef __IIC_COMAMND_SEND__
sayzyas	14:3a5ae61ab1f4	529	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
sayzyas	14:3a5ae61ab1f4	530	I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_F; // Speed
sayzyas	14:3a5ae61ab1f4	531	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor1 FWD
sayzyas	14:3a5ae61ab1f4	532	I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_F; // Speed
sayzyas	13:2c70c772fe24	533	#endif
sayzyas	13:2c70c772fe24	534	flg_exp_status \|= 0x30000000;
sayzyas	14:3a5ae61ab1f4	535	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	536	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	537	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	538	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
sayzyas	14:3a5ae61ab1f4	539	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	540	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	541	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	542	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
sayzyas	13:2c70c772fe24	543	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	544	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	14:3a5ae61ab1f4	545	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	546	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	547	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	548	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	549	}
sayzyas	14:3a5ae61ab1f4	550	else{
sayzyas	14:3a5ae61ab1f4	551	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	552	}
sayzyas	14:3a5ae61ab1f4	553	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	554	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	555	}
sayzyas	14:3a5ae61ab1f4	556	else{
sayzyas	14:3a5ae61ab1f4	557	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	558	}
sayzyas	13:2c70c772fe24	559	limitSw_Sts = I2C_res[3];
sayzyas	14:3a5ae61ab1f4	560	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	14:3a5ae61ab1f4	561	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	14:3a5ae61ab1f4	562	DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	14:3a5ae61ab1f4	563	DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
sayzyas	13:2c70c772fe24	564	#endif // __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	565	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	566	flg_ButtonOn = false;
sayzyas	13:2c70c772fe24	567	}
sayzyas	13:2c70c772fe24	568
sayzyas	13:2c70c772fe24	569	else if ((btnStatus_RFI== btnID_RFLBI)&&(baseOperation.sv_WinchValid==0)){ // Both sw on
sayzyas	13:2c70c772fe24	570	flg_ButtonOn = true;
sayzyas	13:2c70c772fe24	571	DEBUG_PRINT_L3( "Bd0> BTN RF-I & LB-I\r\n" );
sayzyas	13:2c70c772fe24	572	led3 = 1;
sayzyas	13:2c70c772fe24	573	#ifdef __IIC_COMAMND_SEND__
sayzyas	14:3a5ae61ab1f4	574	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 FWD
sayzyas	14:3a5ae61ab1f4	575	I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_F; // Speed
sayzyas	14:3a5ae61ab1f4	576	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor1 FWD
sayzyas	14:3a5ae61ab1f4	577	I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_F; // Speed
sayzyas	13:2c70c772fe24	578	#endif
sayzyas	13:2c70c772fe24	579	flg_exp_status \|= 0x10000000;
sayzyas	14:3a5ae61ab1f4	580	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	581	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	582	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	583	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
sayzyas	14:3a5ae61ab1f4	584	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	585	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	586	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	587	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
sayzyas	13:2c70c772fe24	588	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	589	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	14:3a5ae61ab1f4	590	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	591	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	592	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	593	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	594	}
sayzyas	14:3a5ae61ab1f4	595	else{
sayzyas	14:3a5ae61ab1f4	596	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	597	}
sayzyas	14:3a5ae61ab1f4	598	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	599	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	600	}
sayzyas	14:3a5ae61ab1f4	601	else{
sayzyas	14:3a5ae61ab1f4	602	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	603	}
sayzyas	13:2c70c772fe24	604	limitSw_Sts = I2C_res[3];
sayzyas	14:3a5ae61ab1f4	605	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	14:3a5ae61ab1f4	606	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	14:3a5ae61ab1f4	607	DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	14:3a5ae61ab1f4	608	DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
sayzyas	13:2c70c772fe24	609	#endif // __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	610	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	611	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	612	}
sayzyas	13:2c70c772fe24	613
sayzyas	13:2c70c772fe24	614	else if ((btnStatus_RFK==btnID_RFK)&&(baseOperation.sv_WinchValid==0)){ // RF KO
sayzyas	13:2c70c772fe24	615	flg_ButtonOn = true;
sayzyas	13:2c70c772fe24	616	DEBUG_PRINT_L3( "Bd0> BTN RF-K\r\n" );
sayzyas	13:2c70c772fe24	617	led3 = 1;
sayzyas	13:2c70c772fe24	618	#ifdef __IIC_COMAMND_SEND__
sayzyas	14:3a5ae61ab1f4	619	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
sayzyas	14:3a5ae61ab1f4	620	I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_F; // Speed
sayzyas	13:2c70c772fe24	621	#endif
sayzyas	13:2c70c772fe24	622	flg_exp_status \|= 0x10000000;
sayzyas	14:3a5ae61ab1f4	623	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	624	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	625	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	626	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
sayzyas	14:3a5ae61ab1f4	627	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	628	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	629	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	630	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
sayzyas	13:2c70c772fe24	631	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	632	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	14:3a5ae61ab1f4	633	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	634	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	635	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	636	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	637	}
sayzyas	14:3a5ae61ab1f4	638	else{
sayzyas	14:3a5ae61ab1f4	639	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	640	}
sayzyas	14:3a5ae61ab1f4	641	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	642	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	643	}
sayzyas	14:3a5ae61ab1f4	644	else{
sayzyas	14:3a5ae61ab1f4	645	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	646	}
sayzyas	13:2c70c772fe24	647	limitSw_Sts = I2C_res[3];
sayzyas	14:3a5ae61ab1f4	648	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	14:3a5ae61ab1f4	649	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	14:3a5ae61ab1f4	650	DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	14:3a5ae61ab1f4	651	DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
sayzyas	13:2c70c772fe24	652	#endif // __IIC_COMAMND_SEND__
sayzyas	13:2c70c772fe24	653	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	654	flg_ButtonOn = false;
sayzyas	13:2c70c772fe24	655	}
sayzyas	13:2c70c772fe24	656	else if ((btnStatus_RFI==btnID_RFI)&&(baseOperation.sv_WinchValid==0)){ // RF I
sayzyas	13:2c70c772fe24	657	DEBUG_PRINT_L3( "Bd0> BTN RF-I\r\n" );
sayzyas	13:2c70c772fe24	658	led3 = 1;
sayzyas	13:2c70c772fe24	659	#ifdef __IIC_COMAMND_SEND__
sayzyas	14:3a5ae61ab1f4	660	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
sayzyas	14:3a5ae61ab1f4	661	I2C_cmd[I2C_CP_M1_SPEED] = setValue.tfmCtrl.sv_RFTM_srto_R; // Speed
sayzyas	13:2c70c772fe24	662	#endif
sayzyas	13:2c70c772fe24	663	flg_exp_status \|= 0x20000000;
sayzyas	14:3a5ae61ab1f4	664	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	665	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	666	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	667	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
sayzyas	14:3a5ae61ab1f4	668	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	669	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	670	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	671	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
sayzyas	13:2c70c772fe24	672	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	673	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	14:3a5ae61ab1f4	674	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	675	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	676	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	677	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	678	}
sayzyas	14:3a5ae61ab1f4	679	else{
sayzyas	14:3a5ae61ab1f4	680	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	681	}
sayzyas	14:3a5ae61ab1f4	682	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	683	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	684	}
sayzyas	14:3a5ae61ab1f4	685	else{
sayzyas	14:3a5ae61ab1f4	686	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	687	}
sayzyas	13:2c70c772fe24	688	limitSw_Sts = I2C_res[3];
sayzyas	14:3a5ae61ab1f4	689	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	14:3a5ae61ab1f4	690	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	14:3a5ae61ab1f4	691	DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	14:3a5ae61ab1f4	692	DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
sayzyas	13:2c70c772fe24	693	#endif // __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	694	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	695	flg_ButtonOn = false;
sayzyas	13:2c70c772fe24	696	}
sayzyas	13:2c70c772fe24	697
sayzyas	13:2c70c772fe24	698	else if ((btnStatus_LBK==btnID_LBK)&&(baseOperation.sv_WinchValid==0)){ // LB KO
sayzyas	13:2c70c772fe24	699	flg_ButtonOn = true;
sayzyas	13:2c70c772fe24	700	DEBUG_PRINT_L3( "Bd0> BTN LB-K\r\n" );
sayzyas	13:2c70c772fe24	701	led3 = 1;
sayzyas	13:2c70c772fe24	702	#ifdef __IIC_COMAMND_SEND__
sayzyas	14:3a5ae61ab1f4	703	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 FWD
sayzyas	14:3a5ae61ab1f4	704	I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_F; // Speed
sayzyas	13:2c70c772fe24	705	#endif
sayzyas	13:2c70c772fe24	706	flg_exp_status \|= 0x40000000;
sayzyas	14:3a5ae61ab1f4	707	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	708	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	709	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	710	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
sayzyas	14:3a5ae61ab1f4	711	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	712	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	713	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	714	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
sayzyas	13:2c70c772fe24	715	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	716	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	14:3a5ae61ab1f4	717	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	718	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	719	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	720	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	721	}
sayzyas	14:3a5ae61ab1f4	722	else{
sayzyas	14:3a5ae61ab1f4	723	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	724	}
sayzyas	14:3a5ae61ab1f4	725	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	726	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	727	}
sayzyas	14:3a5ae61ab1f4	728	else{
sayzyas	14:3a5ae61ab1f4	729	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	730	}
sayzyas	13:2c70c772fe24	731	limitSw_Sts = I2C_res[3];
sayzyas	14:3a5ae61ab1f4	732	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	14:3a5ae61ab1f4	733	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	14:3a5ae61ab1f4	734	DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	14:3a5ae61ab1f4	735	DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
sayzyas	13:2c70c772fe24	736	#endif // __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	737	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	738	flg_ButtonOn = false;
sayzyas	13:2c70c772fe24	739	}
sayzyas	13:2c70c772fe24	740	else if ((btnStatus_LBI==btnID_LBI)&&(baseOperation.sv_WinchValid==0)) { // LB I
sayzyas	13:2c70c772fe24	741	flg_ButtonOn = true;
sayzyas	13:2c70c772fe24	742	DEBUG_PRINT_L3( "Bd0> BTN LB-I\r\n" );
sayzyas	13:2c70c772fe24	743	led3 = 1;
sayzyas	13:2c70c772fe24	744	#ifdef __IIC_COMAMND_SEND__
sayzyas	14:3a5ae61ab1f4	745	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor1 RVS
sayzyas	14:3a5ae61ab1f4	746	I2C_cmd[I2C_CP_M2_SPEED] = setValue.tfmCtrl.sv_LBTM_srto_R; // Speed
sayzyas	13:2c70c772fe24	747	#endif
sayzyas	13:2c70c772fe24	748	flg_exp_status \|= 0x80000000;
sayzyas	14:3a5ae61ab1f4	749	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	750	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	751	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	752	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
sayzyas	14:3a5ae61ab1f4	753	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	754	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	755	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	756	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
sayzyas	13:2c70c772fe24	757	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	758	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	14:3a5ae61ab1f4	759	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	760	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	761	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	762	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	763	}
sayzyas	14:3a5ae61ab1f4	764	else{
sayzyas	14:3a5ae61ab1f4	765	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	766	}
sayzyas	14:3a5ae61ab1f4	767	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	768	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	769	}
sayzyas	14:3a5ae61ab1f4	770	else{
sayzyas	14:3a5ae61ab1f4	771	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	772	}
sayzyas	13:2c70c772fe24	773	limitSw_Sts = I2C_res[3];
sayzyas	14:3a5ae61ab1f4	774	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	14:3a5ae61ab1f4	775	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	14:3a5ae61ab1f4	776	DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	14:3a5ae61ab1f4	777	DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
sayzyas	13:2c70c772fe24	778	#endif // __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	779	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	780	flg_ButtonOn = false;
sayzyas	13:2c70c772fe24	781	}
sayzyas	13:2c70c772fe24	782	// ====================================
sayzyas	13:2c70c772fe24	783	// ALL motor off commmand packet send
sayzyas	13:2c70c772fe24	784	// -- This part isn't operated in case of general game controller, because no event. --
sayzyas	13:2c70c772fe24	785	// ====================================
sayzyas	13:2c70c772fe24	786	else if (baseOperation.sv_WinchValid==0){
sayzyas	13:2c70c772fe24	787	led3 = 0;
sayzyas	13:2c70c772fe24	788	#ifdef __IIC_COMAMND_SEND__
sayzyas	14:3a5ae61ab1f4	789	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP;
sayzyas	14:3a5ae61ab1f4	790	I2C_cmd[I2C_CP_M1_SPEED] = 0;
sayzyas	14:3a5ae61ab1f4	791	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP;
sayzyas	14:3a5ae61ab1f4	792	I2C_cmd[I2C_CP_M2_SPEED] = 0;
sayzyas	13:2c70c772fe24	793	Thread::wait(5);
sayzyas	13:2c70c772fe24	794	#endif
sayzyas	13:2c70c772fe24	795	flg_exp_status &= 0x0FFFFFFF;
sayzyas	14:3a5ae61ab1f4	796	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	797	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	798	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_RFTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	799	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_RFTM_ith_R & 0xFF );
sayzyas	14:3a5ae61ab1f4	800	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	801	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	802	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.tfmCtrl.sv_LBTM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	803	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.tfmCtrl.sv_LBTM_ith_R & 0xFF );
sayzyas	13:2c70c772fe24	804	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	805	i2c.write(I2C_ADDRESS_TRANSFORM, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	806
sayzyas	13:2c70c772fe24	807	#ifdef __READ_TFM_MOTOR_CURRENT__
sayzyas	14:3a5ae61ab1f4	808	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	809	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	810	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	811	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	812	}
sayzyas	14:3a5ae61ab1f4	813	else{
sayzyas	14:3a5ae61ab1f4	814	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	815	}
sayzyas	14:3a5ae61ab1f4	816	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	817	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	818	}
sayzyas	14:3a5ae61ab1f4	819	else{
sayzyas	14:3a5ae61ab1f4	820	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	821	}
sayzyas	13:2c70c772fe24	822	limitSw_Sts = I2C_res[3];
sayzyas	14:3a5ae61ab1f4	823	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	14:3a5ae61ab1f4	824	DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	14:3a5ae61ab1f4	825	DEBUG_PRINT_L2( "Bd0> Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	14:3a5ae61ab1f4	826	DEBUG_PRINT_L2( "Bd0> Motor Lock Status (%c)\r\n", motorLock_sts );
sayzyas	13:2c70c772fe24	827	#endif // __READ_TFM_MOTOR_CURRENT__
sayzyas	13:2c70c772fe24	828
sayzyas	13:2c70c772fe24	829	}
sayzyas	13:2c70c772fe24	830
sayzyas	10:a2bd7d07c7f8	831	/*/ ====================================
sayzyas	10:a2bd7d07c7f8	832	// Crawler Moving Control
sayzyas	10:a2bd7d07c7f8	833	// ====================================
sayzyas	14:3a5ae61ab1f4	834	// JoyStick mode 1: Independence mode ( Dual JoyStick mode )
sayzyas	10:a2bd7d07c7f8	835	//
sayzyas	10:a2bd7d07c7f8	836	// * *
sayzyas	10:a2bd7d07c7f8	837	// * L * * R *
sayzyas	10:a2bd7d07c7f8	838	// * *
sayzyas	10:a2bd7d07c7f8	839	// F 4 1
sayzyas	10:a2bd7d07c7f8	840	//
sayzyas	10:a2bd7d07c7f8	841	// R 8 2
sayzyas	10:a2bd7d07c7f8	842	//
sayzyas	10:a2bd7d07c7f8	843	// Forward move 5
sayzyas	10:a2bd7d07c7f8	844	// Reverce move a
sayzyas	10:a2bd7d07c7f8	845	// Right rotation 6
sayzyas	10:a2bd7d07c7f8	846	// Left rotation 9
sayzyas	10:a2bd7d07c7f8	847	* 7 6 5 4 3 2 1 0
sayzyas	10:a2bd7d07c7f8	848	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	849	* \|x\|x\|o\|x\|x\|x\|x\|x\| 1: R Fwd, 2: R Rvs, 4: L Fwd, 8: L Rvs
sayzyas	10:a2bd7d07c7f8	850	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	851	*/
sayzyas	13:2c70c772fe24	852	if( baseOperation.sv_JS_OpeMode == 1 ){
sayzyas	14:3a5ae61ab1f4	853	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	854	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	855	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	856	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_R & 0xFF );
sayzyas	14:3a5ae61ab1f4	857	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	858	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	859	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	860	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_R & 0xFF );
sayzyas	13:2c70c772fe24	861	if ((btnStatus_LJSFwdRvs<setValue.crawlerCtrl.sv_LBCM_dzc-setValue.crawlerCtrl.sv_LBCM_dzl)&&(baseOperation.sv_WinchValid==0)){
sayzyas	10:a2bd7d07c7f8	862	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	863	led3 = 1;
sayzyas	14:3a5ae61ab1f4	864	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 FWD
sayzyas	14:3a5ae61ab1f4	865	tmpSpeed = ( setValue.crawlerCtrl.sv_LBCM_dzc+1 - btnStatus_LJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc * setValue.crawlerCtrl.sv_LBCM_srto_F / 100; // Speed
sayzyas	14:3a5ae61ab1f4	866	I2C_cmd[I2C_CP_M2_SPEED] = (char)tmpSpeed;
sayzyas	13:2c70c772fe24	867	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	11:ff06edc0219c	868	DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Fwd (Speed=%d)\r\n", tmpSpeed);
sayzyas	13:2c70c772fe24	869	// Read motor current from target
sayzyas	14:3a5ae61ab1f4	870	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	871	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	872	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	873	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	874	}
sayzyas	14:3a5ae61ab1f4	875	else{
sayzyas	14:3a5ae61ab1f4	876	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	877	}
sayzyas	13:2c70c772fe24	878	DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	879
sayzyas	11:ff06edc0219c	880	flg_exp_status \|= 0x00400000;
sayzyas	13:2c70c772fe24	881	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	882	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	883	}
sayzyas	13:2c70c772fe24	884	else if ((btnStatus_LJSFwdRvs>setValue.crawlerCtrl.sv_LBCM_dzc+setValue.crawlerCtrl.sv_LBCM_dzu)&&(baseOperation.sv_WinchValid==0)){
sayzyas	10:a2bd7d07c7f8	885	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	886	led3 = 1;
sayzyas	14:3a5ae61ab1f4	887	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 RVS
sayzyas	14:3a5ae61ab1f4	888	tmpSpeed = ( btnStatus_LJSFwdRvs - setValue.crawlerCtrl.sv_LBCM_dzc ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc+1 * setValue.crawlerCtrl.sv_LBCM_srto_F / 100; // Speed I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = (char)tmpSpeed;
sayzyas	14:3a5ae61ab1f4	889	I2C_cmd[I2C_CP_M2_SPEED] = (char)tmpSpeed;
sayzyas	13:2c70c772fe24	890	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	11:ff06edc0219c	891	DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Rvs (Speed=%d)\r\n", tmpSpeed);
sayzyas	11:ff06edc0219c	892
sayzyas	13:2c70c772fe24	893	// Read motor current from target
sayzyas	14:3a5ae61ab1f4	894	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	895	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	896	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	897	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	898	}
sayzyas	14:3a5ae61ab1f4	899	else{
sayzyas	14:3a5ae61ab1f4	900	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	901	}
sayzyas	13:2c70c772fe24	902	DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	11:ff06edc0219c	903
sayzyas	10:a2bd7d07c7f8	904	flg_exp_status \|= 0x00800000;
sayzyas	13:2c70c772fe24	905	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	906	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	907	}
sayzyas	13:2c70c772fe24	908	else if (baseOperation.sv_WinchValid==0) {
sayzyas	14:3a5ae61ab1f4	909	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Stop
sayzyas	14:3a5ae61ab1f4	910	I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed=0
sayzyas	14:3a5ae61ab1f4	911	flg_exp_status &= 0xFF3FFFFF;
sayzyas	13:2c70c772fe24	912	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	913	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	914	}
sayzyas	10:a2bd7d07c7f8	915
sayzyas	13:2c70c772fe24	916	if ((btnStatus_RJSFwdRvs<setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)&&(baseOperation.sv_WinchValid==0)){
sayzyas	10:a2bd7d07c7f8	917	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	918	led3 = 1;
sayzyas	14:3a5ae61ab1f4	919	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
sayzyas	14:3a5ae61ab1f4	920	tmpSpeed = (( 128 - btnStatus_RJSFwdRvs ) * 100 / 127 ) * setValue.crawlerCtrl.sv_RFCM_srto_R / 100;
sayzyas	14:3a5ae61ab1f4	921	I2C_cmd[I2C_CP_M1_SPEED] = (char)tmpSpeed;
sayzyas	11:ff06edc0219c	922	DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Fwd (Speed=%d)\r\n", tmpSpeed);
sayzyas	13:2c70c772fe24	923	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	11:ff06edc0219c	924
sayzyas	13:2c70c772fe24	925	// Read motor current from target
sayzyas	14:3a5ae61ab1f4	926	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	927	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	928	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	929	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	930	}
sayzyas	14:3a5ae61ab1f4	931	else{
sayzyas	14:3a5ae61ab1f4	932	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	933	}
sayzyas	13:2c70c772fe24	934	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	11:ff06edc0219c	935
sayzyas	10:a2bd7d07c7f8	936	flg_exp_status \|= 0x00100000;
sayzyas	13:2c70c772fe24	937	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	938	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	939	}
sayzyas	13:2c70c772fe24	940	else if ((btnStatus_RJSFwdRvs>setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu)&&(baseOperation.sv_WinchValid==0)){
sayzyas	10:a2bd7d07c7f8	941	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	942	led3 = 1;
sayzyas	14:3a5ae61ab1f4	943	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
sayzyas	14:3a5ae61ab1f4	944	tmpSpeed = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc+1 * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
sayzyas	14:3a5ae61ab1f4	945	I2C_cmd[I2C_CP_M1_SPEED] = (char)tmpSpeed;
sayzyas	11:ff06edc0219c	946	DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Rvs (Speed=%d)\r\n", tmpSpeed);
sayzyas	13:2c70c772fe24	947	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	11:ff06edc0219c	948
sayzyas	13:2c70c772fe24	949	// Read motor current from target
sayzyas	14:3a5ae61ab1f4	950	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	951	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	952	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	953	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	954	}
sayzyas	14:3a5ae61ab1f4	955	else{
sayzyas	14:3a5ae61ab1f4	956	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	957	}
sayzyas	13:2c70c772fe24	958	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	11:ff06edc0219c	959
sayzyas	10:a2bd7d07c7f8	960	flg_exp_status \|= 0x00200000;
sayzyas	13:2c70c772fe24	961	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	962	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	963	}
sayzyas	13:2c70c772fe24	964	else if (baseOperation.sv_WinchValid==0) {
sayzyas	14:3a5ae61ab1f4	965	DEBUG_PRINT_L2("*** MOTOR STOP **\r\n");
sayzyas	14:3a5ae61ab1f4	966	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Stop
sayzyas	14:3a5ae61ab1f4	967	I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed=0
sayzyas	10:a2bd7d07c7f8	968	flg_exp_status &= 0xFFCFFFFF;
sayzyas	13:2c70c772fe24	969	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	970	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	971	}
sayzyas	10:a2bd7d07c7f8	972	led3 = 0;
sayzyas	10:a2bd7d07c7f8	973	}
sayzyas	14:3a5ae61ab1f4	974	/* JoyStick mode 0: Syncronous mode ( Single JoyStick mode )
sayzyas	10:a2bd7d07c7f8	975	//
sayzyas	10:a2bd7d07c7f8	976	// * **
sayzyas	10:a2bd7d07c7f8	977	// * X * * LR *
sayzyas	10:a2bd7d07c7f8	978	// * **
sayzyas	10:a2bd7d07c7f8	979	// F 4 1
sayzyas	10:a2bd7d07c7f8	980	//
sayzyas	10:a2bd7d07c7f8	981	// R 8 2
sayzyas	10:a2bd7d07c7f8	982	//
sayzyas	10:a2bd7d07c7f8	983	// Forward move 5
sayzyas	10:a2bd7d07c7f8	984	// Reverce move a
sayzyas	10:a2bd7d07c7f8	985	// Right rotation 6
sayzyas	10:a2bd7d07c7f8	986	// Left rotation 9
sayzyas	10:a2bd7d07c7f8	987	* 7 6 5 4 3 2 1 0
sayzyas	10:a2bd7d07c7f8	988	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	989	* \|x\|x\|o\|x\|x\|x\|x\|x\| 1: R Fwd, 2: R Rvs, 4: L Fwd, 8: L Rvs
sayzyas	10:a2bd7d07c7f8	990	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	991	*/
sayzyas	10:a2bd7d07c7f8	992	else{ // Single JoyStick mode
sayzyas	14:3a5ae61ab1f4	993	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	994	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	995	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_RFCM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	996	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_RFCM_ith_R & 0xFF );
sayzyas	14:3a5ae61ab1f4	997	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	998	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	999	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.crawlerCtrl.sv_LBCM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1000	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.crawlerCtrl.sv_LBCM_ith_R & 0xFF );
sayzyas	13:2c70c772fe24	1001	if(
sayzyas	13:2c70c772fe24	1002	( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl ) &&
sayzyas	13:2c70c772fe24	1003	( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50) &&
sayzyas	13:2c70c772fe24	1004	( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50 ) &&
sayzyas	13:2c70c772fe24	1005	( baseOperation.sv_WinchValid == 0)
sayzyas	13:2c70c772fe24	1006	){
sayzyas	10:a2bd7d07c7f8	1007	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	1008	led3 = 1;
sayzyas	13:2c70c772fe24	1009	if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
sayzyas	14:3a5ae61ab1f4	1010	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Reverse
sayzyas	14:3a5ae61ab1f4	1011	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
sayzyas	11:ff06edc0219c	1012	}
sayzyas	11:ff06edc0219c	1013	else{
sayzyas	14:3a5ae61ab1f4	1014	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Fwd
sayzyas	14:3a5ae61ab1f4	1015	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
sayzyas	11:ff06edc0219c	1016	}
sayzyas	14:3a5ae61ab1f4	1017	I2C_cmd[I2C_CP_M1_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
sayzyas	14:3a5ae61ab1f4	1018	I2C_cmd[I2C_CP_M2_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
sayzyas	13:2c70c772fe24	1019	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	14:3a5ae61ab1f4	1020	DEBUG_PRINT_L3( "Bd0> Single Mode: Dir1 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_RVS_CNTTH_U], I2C_cmd[I2C_CP_M2_SPEED]);
sayzyas	13:2c70c772fe24	1021	flg_exp_status \|= 0x00500000;
sayzyas	13:2c70c772fe24	1022	//flg_exp_status \|= 0x00400000; // 0x00500000
sayzyas	13:2c70c772fe24	1023
sayzyas	13:2c70c772fe24	1024	// Read motor current from target
sayzyas	14:3a5ae61ab1f4	1025	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	1026	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	1027	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	1028	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	1029	}
sayzyas	14:3a5ae61ab1f4	1030	else{
sayzyas	14:3a5ae61ab1f4	1031	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	1032	}
sayzyas	14:3a5ae61ab1f4	1033	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	1034	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	1035	}
sayzyas	14:3a5ae61ab1f4	1036	else{
sayzyas	14:3a5ae61ab1f4	1037	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	1038	}
sayzyas	13:2c70c772fe24	1039	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	1040	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	1041
sayzyas	13:2c70c772fe24	1042	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	12:3e6b6fcf540b	1043	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	1044	}
sayzyas	13:2c70c772fe24	1045	else if(
sayzyas	13:2c70c772fe24	1046	( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu ) &&
sayzyas	13:2c70c772fe24	1047	( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50 ) &&
sayzyas	13:2c70c772fe24	1048	( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
sayzyas	13:2c70c772fe24	1049	( baseOperation.sv_WinchValid == 0)
sayzyas	13:2c70c772fe24	1050	){
sayzyas	10:a2bd7d07c7f8	1051	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	1052	led3 = 1;
sayzyas	13:2c70c772fe24	1053	if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
sayzyas	14:3a5ae61ab1f4	1054	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Reverse
sayzyas	14:3a5ae61ab1f4	1055	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor1 Reverse
sayzyas	11:ff06edc0219c	1056	}
sayzyas	11:ff06edc0219c	1057	else{
sayzyas	14:3a5ae61ab1f4	1058	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Rvs
sayzyas	14:3a5ae61ab1f4	1059	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 Fwd
sayzyas	11:ff06edc0219c	1060	}
sayzyas	14:3a5ae61ab1f4	1061	I2C_cmd[I2C_CP_M1_SPEED] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
sayzyas	14:3a5ae61ab1f4	1062	I2C_cmd[I2C_CP_M2_SPEED] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
sayzyas	13:2c70c772fe24	1063	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	14:3a5ae61ab1f4	1064	DEBUG_PRINT_L3( "Bd0> Single Mode: Dir2 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_RVS_CNTTH_U], I2C_cmd[I2C_CP_M2_SPEED]);
sayzyas	13:2c70c772fe24	1065	flg_exp_status \|= 0x00600000;
sayzyas	13:2c70c772fe24	1066	//flg_exp_status \|= 0x00400000; // 0x00600000
sayzyas	13:2c70c772fe24	1067
sayzyas	13:2c70c772fe24	1068	// Read motor current from target
sayzyas	14:3a5ae61ab1f4	1069	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	1070	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	1071	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	1072	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	1073	}
sayzyas	14:3a5ae61ab1f4	1074	else{
sayzyas	14:3a5ae61ab1f4	1075	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	1076	}
sayzyas	14:3a5ae61ab1f4	1077	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	1078	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	1079	}
sayzyas	14:3a5ae61ab1f4	1080	else{
sayzyas	14:3a5ae61ab1f4	1081	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	1082	}
sayzyas	13:2c70c772fe24	1083	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	1084	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	1085
sayzyas	13:2c70c772fe24	1086	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	1087	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	1088	}
sayzyas	13:2c70c772fe24	1089	else if(
sayzyas	13:2c70c772fe24	1090	( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu ) &&
sayzyas	13:2c70c772fe24	1091	( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50) &&
sayzyas	13:2c70c772fe24	1092	( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
sayzyas	13:2c70c772fe24	1093	( baseOperation.sv_WinchValid == 0)
sayzyas	13:2c70c772fe24	1094	){
sayzyas	10:a2bd7d07c7f8	1095	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	1096	led3 = 1;
sayzyas	13:2c70c772fe24	1097	if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
sayzyas	14:3a5ae61ab1f4	1098	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Rvs
sayzyas	14:3a5ae61ab1f4	1099	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
sayzyas	11:ff06edc0219c	1100	}
sayzyas	11:ff06edc0219c	1101	else{
sayzyas	14:3a5ae61ab1f4	1102	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 Rvs
sayzyas	14:3a5ae61ab1f4	1103	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
sayzyas	11:ff06edc0219c	1104	}
sayzyas	14:3a5ae61ab1f4	1105	I2C_cmd[I2C_CP_M1_SPEED] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
sayzyas	14:3a5ae61ab1f4	1106	I2C_cmd[I2C_CP_M2_SPEED] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
sayzyas	13:2c70c772fe24	1107	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	14:3a5ae61ab1f4	1108	DEBUG_PRINT_L3( "Bd0> Single Mode: Dir3 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_RVS_CNTTH_U], I2C_cmd[I2C_CP_M2_SPEED]);
sayzyas	13:2c70c772fe24	1109	flg_exp_status \|= 0x00A00000;
sayzyas	13:2c70c772fe24	1110	//flg_exp_status \|= 0x00800000; // 0x00A00000
sayzyas	13:2c70c772fe24	1111	// Read motor current from target
sayzyas	13:2c70c772fe24	1112
sayzyas	14:3a5ae61ab1f4	1113	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	1114	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	1115	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	1116	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	1117	}
sayzyas	14:3a5ae61ab1f4	1118	else{
sayzyas	14:3a5ae61ab1f4	1119	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	1120	}
sayzyas	14:3a5ae61ab1f4	1121	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	1122	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	1123	}
sayzyas	14:3a5ae61ab1f4	1124	else{
sayzyas	14:3a5ae61ab1f4	1125	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	1126	}
sayzyas	13:2c70c772fe24	1127	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	1128	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	1129
sayzyas	13:2c70c772fe24	1130	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	1131	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	1132	}
sayzyas	13:2c70c772fe24	1133	else if(
sayzyas	13:2c70c772fe24	1134	( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzu ) &&
sayzyas	13:2c70c772fe24	1135	( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu + 50) &&
sayzyas	13:2c70c772fe24	1136	( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc - setValue.crawlerCtrl.sv_RFCM_dzl - 50) &&
sayzyas	13:2c70c772fe24	1137	( baseOperation.sv_WinchValid == 0)
sayzyas	13:2c70c772fe24	1138	){
sayzyas	10:a2bd7d07c7f8	1139	flg_ButtonOn = true;
sayzyas	10:a2bd7d07c7f8	1140	led3 = 1;
sayzyas	13:2c70c772fe24	1141	if( baseOperation.sv_JS_ShapeMode == 0 ){ // I-Shape
sayzyas	14:3a5ae61ab1f4	1142	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Reverse
sayzyas	14:3a5ae61ab1f4	1143	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor1 Reverse
sayzyas	11:ff06edc0219c	1144	}
sayzyas	11:ff06edc0219c	1145	else{
sayzyas	14:3a5ae61ab1f4	1146	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 Fwd
sayzyas	14:3a5ae61ab1f4	1147	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 Rvs
sayzyas	11:ff06edc0219c	1148	}
sayzyas	14:3a5ae61ab1f4	1149	I2C_cmd[I2C_CP_M1_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_F / 100; // Speed
sayzyas	14:3a5ae61ab1f4	1150	I2C_cmd[I2C_CP_M2_SPEED] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_srto_R / 100; // Speed
sayzyas	13:2c70c772fe24	1151	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	14:3a5ae61ab1f4	1152	DEBUG_PRINT_L3( "Bd0> Single Mode: Dir4 [%d, %d] (Speed: %d, %d)\r\n", I2C_cmd[I2C_CP_M1_DIR], I2C_cmd[I2C_CP_M1_SPEED], I2C_cmd[I2C_CP_M1_RVS_CNTTH_U], I2C_cmd[I2C_CP_M2_SPEED]);
sayzyas	13:2c70c772fe24	1153	flg_exp_status \|= 0x00900000;
sayzyas	13:2c70c772fe24	1154	//flg_exp_status \|= 0x00800000; // 0x00900000
sayzyas	13:2c70c772fe24	1155
sayzyas	13:2c70c772fe24	1156	// Read motor current from target
sayzyas	14:3a5ae61ab1f4	1157	read_motorCurrent( I2C_ADDRESS_CRAWLER, I2C_readcmd, I2C_res, 5 );
sayzyas	14:3a5ae61ab1f4	1158	motorLock_sts = I2C_res[4];
sayzyas	14:3a5ae61ab1f4	1159	if( motorLock_sts == '1' ){
sayzyas	14:3a5ae61ab1f4	1160	motor1_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	1161	}
sayzyas	14:3a5ae61ab1f4	1162	else{
sayzyas	14:3a5ae61ab1f4	1163	motor1_current_pct = I2C_res[0];
sayzyas	14:3a5ae61ab1f4	1164	}
sayzyas	14:3a5ae61ab1f4	1165	if( motorLock_sts == '2' ){
sayzyas	14:3a5ae61ab1f4	1166	motor2_current_pct = 999;
sayzyas	14:3a5ae61ab1f4	1167	}
sayzyas	14:3a5ae61ab1f4	1168	else{
sayzyas	14:3a5ae61ab1f4	1169	motor2_current_pct = I2C_res[1];
sayzyas	14:3a5ae61ab1f4	1170	}
sayzyas	13:2c70c772fe24	1171	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	1172	DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	1173
sayzyas	13:2c70c772fe24	1174	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	1175	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	1176	}
sayzyas	10:a2bd7d07c7f8	1177	// ====================================
sayzyas	10:a2bd7d07c7f8	1178	// ALL motor off commmand packet send
sayzyas	10:a2bd7d07c7f8	1179	// ====================================
sayzyas	13:2c70c772fe24	1180	else if (baseOperation.sv_WinchValid==0) {
sayzyas	10:a2bd7d07c7f8	1181	led3 = 0;
sayzyas	10:a2bd7d07c7f8	1182	#ifdef __IIC_COMAMND_SEND__
sayzyas	14:3a5ae61ab1f4	1183	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 Fwd
sayzyas	14:3a5ae61ab1f4	1184	I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed=0
sayzyas	14:3a5ae61ab1f4	1185	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 Rvs
sayzyas	14:3a5ae61ab1f4	1186	I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed=0
sayzyas	13:2c70c772fe24	1187	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	1188	i2c.write(I2C_ADDRESS_CRAWLER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	1189	// Thread::wait(5);
sayzyas	10:a2bd7d07c7f8	1190	#endif
sayzyas	10:a2bd7d07c7f8	1191	flg_exp_status &= 0xFF0FFFFF;
sayzyas	14:3a5ae61ab1f4	1192	// I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_F;
sayzyas	14:3a5ae61ab1f4	1193	// I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_R;
sayzyas	14:3a5ae61ab1f4	1194	// I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_F;
sayzyas	14:3a5ae61ab1f4	1195	// I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_R;
sayzyas	10:a2bd7d07c7f8	1196	}
sayzyas	10:a2bd7d07c7f8	1197	}
sayzyas	13:2c70c772fe24	1198
sayzyas	10:a2bd7d07c7f8	1199	/*
sayzyas	10:a2bd7d07c7f8	1200	* ====================================
sayzyas	10:a2bd7d07c7f8	1201	* Winch Motor Control
sayzyas	10:a2bd7d07c7f8	1202	*
sayzyas	10:a2bd7d07c7f8	1203	* 7 6 5 4 3 2 1 0
sayzyas	10:a2bd7d07c7f8	1204	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	1205	* \|x\|o\|x\|x\|x\|x\|x\|x\| 1: W Down, 2: W Up, 4: -, 8: -
sayzyas	10:a2bd7d07c7f8	1206	* +-+-+-+-+-+-+-+-+
sayzyas	10:a2bd7d07c7f8	1207	*/
sayzyas	13:2c70c772fe24	1208	if ((btnStatus_WDN == btnID_WDN) && (baseOperation.sv_WinchValid == 1)){ // Winch Down (FWD)
sayzyas	10:a2bd7d07c7f8	1209	flg_ButtonOn = true;
sayzyas	11:ff06edc0219c	1210	DEBUG_PRINT_L3( "Bd0> BTN W-DN(Fwd)\r\n" );
sayzyas	10:a2bd7d07c7f8	1211	led3 = 1;
sayzyas	10:a2bd7d07c7f8	1212	#ifdef __IIC_COMAMND_SEND__
sayzyas	14:3a5ae61ab1f4	1213	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
sayzyas	14:3a5ae61ab1f4	1214	I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_F; // Speed
sayzyas	14:3a5ae61ab1f4	1215	// I2C_cmd[I2C_CP_M2_DIR] = MOTOR_FWD; // Motor2 FWD
sayzyas	14:3a5ae61ab1f4	1216	// I2C_cmd[I2C_CP_M2_SPEED] = setValue.winchCtrl.sv_WRM_hsrto_F; // Speed
sayzyas	10:a2bd7d07c7f8	1217	#endif
sayzyas	10:a2bd7d07c7f8	1218	flg_exp_status \|= 0x01000000;
sayzyas	14:3a5ae61ab1f4	1219
sayzyas	14:3a5ae61ab1f4	1220	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1221	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	1222	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1223	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_R & 0xFF );
sayzyas	14:3a5ae61ab1f4	1224	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1225	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	1226	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1227	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_R & 0xFF );
sayzyas	13:2c70c772fe24	1228	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	1229	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	1230	}
sayzyas	13:2c70c772fe24	1231	else if (( btnStatus_WUP == btnID_WUP ) && (baseOperation.sv_WinchValid == 1)) { // Winch Up (Rvs)
sayzyas	10:a2bd7d07c7f8	1232	flg_ButtonOn = true;
sayzyas	11:ff06edc0219c	1233	DEBUG_PRINT_L3( "Bd0> BTN W-UP(Rvs)\r\n" );
sayzyas	10:a2bd7d07c7f8	1234	led3 = 1;
sayzyas	10:a2bd7d07c7f8	1235	#ifdef __IIC_COMAMND_SEND__
sayzyas	14:3a5ae61ab1f4	1236	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
sayzyas	14:3a5ae61ab1f4	1237	I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_R; // Speed
sayzyas	14:3a5ae61ab1f4	1238	// I2C_cmd[I2C_CP_M2_DIR] = MOTOR_RVS; // Motor2 RVS
sayzyas	14:3a5ae61ab1f4	1239	// I2C_cmd[I2C_CP_M2_SPEED] = setValue.winchCtrl.sv_WRM_hsrto_R; // Speed
sayzyas	10:a2bd7d07c7f8	1240	#endif
sayzyas	10:a2bd7d07c7f8	1241	flg_exp_status \|= 0x02000000;
sayzyas	14:3a5ae61ab1f4	1242
sayzyas	14:3a5ae61ab1f4	1243	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1244	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	1245	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1246	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_R & 0xFF );
sayzyas	14:3a5ae61ab1f4	1247	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1248	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	1249	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1250	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_R & 0xFF );
sayzyas	13:2c70c772fe24	1251	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	1252	flg_ButtonOn = false;
sayzyas	10:a2bd7d07c7f8	1253	}
sayzyas	10:a2bd7d07c7f8	1254	// ====================================
sayzyas	10:a2bd7d07c7f8	1255	// ALL motor off commmand packet send
sayzyas	10:a2bd7d07c7f8	1256	// ====================================
sayzyas	13:2c70c772fe24	1257	else if (baseOperation.sv_WinchValid == 1){
sayzyas	10:a2bd7d07c7f8	1258	led3 = 0;
sayzyas	10:a2bd7d07c7f8	1259	#ifdef __IIC_COMAMND_SEND__
sayzyas	14:3a5ae61ab1f4	1260	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP;
sayzyas	14:3a5ae61ab1f4	1261	I2C_cmd[I2C_CP_M1_SPEED] = 0;
sayzyas	14:3a5ae61ab1f4	1262	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP;
sayzyas	14:3a5ae61ab1f4	1263	I2C_cmd[I2C_CP_M2_SPEED] = 0;
sayzyas	10:a2bd7d07c7f8	1264	Thread::wait(5);
sayzyas	10:a2bd7d07c7f8	1265	#endif
sayzyas	10:a2bd7d07c7f8	1266	flg_exp_status &= 0xF0FFFFFF;
sayzyas	14:3a5ae61ab1f4	1267
sayzyas	14:3a5ae61ab1f4	1268	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1269	I2C_cmd[I2C_CP_M1_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	1270	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WDM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1271	I2C_cmd[I2C_CP_M1_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WDM_ith_R & 0xFF );
sayzyas	14:3a5ae61ab1f4	1272	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_F >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1273	I2C_cmd[I2C_CP_M2_FWD_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_F & 0xFF );
sayzyas	14:3a5ae61ab1f4	1274	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = ( setValue.winchCtrl.sv_WRM_ith_R >> 8 ) & 0xFF;
sayzyas	14:3a5ae61ab1f4	1275	I2C_cmd[I2C_CP_M2_RVS_CNTTH_L] = ( setValue.winchCtrl.sv_WRM_ith_R & 0xFF );
sayzyas	13:2c70c772fe24	1276	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	13:2c70c772fe24	1277	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas	10:a2bd7d07c7f8	1278	}
sayzyas	10:a2bd7d07c7f8	1279	}
sayzyas	10:a2bd7d07c7f8	1280	}
sayzyas	10:a2bd7d07c7f8	1281
sayzyas	10:a2bd7d07c7f8	1282	uint32_t getc_fromHost( uint8_t *c ){
sayzyas	10:a2bd7d07c7f8	1283
sayzyas	10:a2bd7d07c7f8	1284	uint32_t rts = 0;
sayzyas	10:a2bd7d07c7f8	1285
sayzyas	10:a2bd7d07c7f8	1286	if(pc.readable()){
sayzyas	10:a2bd7d07c7f8	1287	*c = pc.getc();
sayzyas	10:a2bd7d07c7f8	1288	rts = 0;
sayzyas	10:a2bd7d07c7f8	1289	}
sayzyas	10:a2bd7d07c7f8	1290	else{
sayzyas	10:a2bd7d07c7f8	1291	rts = 1;
sayzyas	10:a2bd7d07c7f8	1292	}
sayzyas	10:a2bd7d07c7f8	1293	return rts;
sayzyas	10:a2bd7d07c7f8	1294	}
sayzyas	10:a2bd7d07c7f8	1295
sayzyas	10:a2bd7d07c7f8	1296	// **************************************************************
sayzyas	10:a2bd7d07c7f8	1297	// TASK: Hoat Interface Task
sayzyas	10:a2bd7d07c7f8	1298	//
sayzyas	10:a2bd7d07c7f8	1299	// **************************************************************
sayzyas	14:3a5ae61ab1f4	1300	int first_counter = 0;
sayzyas	10:a2bd7d07c7f8	1301	void clientPC_interface_task(void const *)
sayzyas	10:a2bd7d07c7f8	1302	{
sayzyas	10:a2bd7d07c7f8	1303	int rcv_data_cnt;
sayzyas	10:a2bd7d07c7f8	1304	//winchData_t winchData;
sayzyas	10:a2bd7d07c7f8	1305
sayzyas	13:2c70c772fe24	1306	char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
sayzyas	10:a2bd7d07c7f8	1307
sayzyas	10:a2bd7d07c7f8	1308	// winchData_t winchData;
sayzyas	13:2c70c772fe24	1309	// int16_t winchCurrentPosition;
sayzyas	13:2c70c772fe24	1310	int16_t winchTempPosition;
sayzyas	11:ff06edc0219c	1311
sayzyas	11:ff06edc0219c	1312	int cnt = 0;
sayzyas	10:a2bd7d07c7f8	1313
sayzyas	10:a2bd7d07c7f8	1314	while(1){
sayzyas	10:a2bd7d07c7f8	1315
sayzyas	11:ff06edc0219c	1316	// DEBUG_PRINT("\r\nWaiting for UDP packet...\r\n");
sayzyas	10:a2bd7d07c7f8	1317	rcv_data_cnt = udp_server.receiveFrom(client, dbuffer, sizeof(dbuffer));
sayzyas	10:a2bd7d07c7f8	1318	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1319	if( rcv_data_cnt < 0 ){
sayzyas	11:ff06edc0219c	1320	DEBUG_PRINT_L0("Bd0> ## Receive packet fail ##\r\n");
sayzyas	10:a2bd7d07c7f8	1321	}
sayzyas	10:a2bd7d07c7f8	1322	else{
sayzyas	10:a2bd7d07c7f8	1323	dbuffer[rcv_data_cnt] = '\0';
sayzyas	10:a2bd7d07c7f8	1324	led4 = 1;
sayzyas	10:a2bd7d07c7f8	1325
sayzyas	11:ff06edc0219c	1326	if(!strcmp( dbuffer, "Hello Z\r\n" )){
sayzyas	10:a2bd7d07c7f8	1327	DEBUG_PRINT_L2("Bd0> Hello Z Packet received from client by UDP\n");
sayzyas	10:a2bd7d07c7f8	1328	char snd_data[] = "Hello I'm CrExoB2";
sayzyas	10:a2bd7d07c7f8	1329	udp_server.sendTo(client, snd_data, sizeof(snd_data));
sayzyas	10:a2bd7d07c7f8	1330	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1331	}
sayzyas	11:ff06edc0219c	1332	else if(!strcmp( dbuffer, "status\r\n")){
sayzyas	11:ff06edc0219c	1333	DEBUG_PRINT_L2("Bd0> Hello Status req received from client by UDP\r\n");
sayzyas	11:ff06edc0219c	1334	strcpy(dbuffer,"XXXX\r\n");
sayzyas	10:a2bd7d07c7f8	1335	udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
sayzyas	10:a2bd7d07c7f8	1336	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1337	}
sayzyas	14:3a5ae61ab1f4	1338	else if(!strcmp( dbuffer, "set_jss")){ // Single JS mode
sayzyas	14:3a5ae61ab1f4	1339	baseOperation.sv_JS_OpeMode = 0;
sayzyas	14:3a5ae61ab1f4	1340	}
sayzyas	14:3a5ae61ab1f4	1341	else if(!strcmp( dbuffer, "set_jsd")){ // Dual JS mode
sayzyas	14:3a5ae61ab1f4	1342	baseOperation.sv_JS_OpeMode = 1;
sayzyas	14:3a5ae61ab1f4	1343	}
sayzyas	14:3a5ae61ab1f4	1344	else if(!strcmp( dbuffer, "lsw_valid")){ // Limit swich detection valid
sayzyas	14:3a5ae61ab1f4	1345	flg_lsw_valid = true;
sayzyas	14:3a5ae61ab1f4	1346	}
sayzyas	14:3a5ae61ab1f4	1347	else if(!strcmp( dbuffer, "lsw_invalid")){ // Limit swich detection invalid
sayzyas	14:3a5ae61ab1f4	1348	flg_lsw_valid = false;
sayzyas	14:3a5ae61ab1f4	1349	}
sayzyas	10:a2bd7d07c7f8	1350	else if(!strcmp( dbuffer, "Hello")){
sayzyas	11:ff06edc0219c	1351	DEBUG_PRINT_L2( "Bd0> Hello Packet received from [ 0x%02x ]\r\n", flg_exp_status );
sayzyas	10:a2bd7d07c7f8	1352
sayzyas	10:a2bd7d07c7f8	1353	/* ***************************************** */
sayzyas	10:a2bd7d07c7f8	1354	/* Read Winch Current Position from Resolver */
sayzyas	10:a2bd7d07c7f8	1355	/* ***************************************** */
sayzyas	13:2c70c772fe24	1356	if (baseOperation.sv_WinchValid == 1){ // read winch current position operation is valid in case of winch part is valid.
sayzyas	13:2c70c772fe24	1357	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1358	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1359	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1360	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1361	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1362	}
sayzyas	13:2c70c772fe24	1363	}
sayzyas	10:a2bd7d07c7f8	1364	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1365
sayzyas	14:3a5ae61ab1f4	1366	first_counter++;
sayzyas	14:3a5ae61ab1f4	1367	if( first_counter > 10 ) {
sayzyas	14:3a5ae61ab1f4	1368	sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts); // Winch down
sayzyas	14:3a5ae61ab1f4	1369	first_counter = 10;
sayzyas	14:3a5ae61ab1f4	1370	}
sayzyas	14:3a5ae61ab1f4	1371	else{
sayzyas	14:3a5ae61ab1f4	1372	sprintf( dbuffer,"JSMD %03d %03d 0000 000\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid );
sayzyas	14:3a5ae61ab1f4	1373	}
sayzyas	10:a2bd7d07c7f8	1374	// -------------------------------------
sayzyas	10:a2bd7d07c7f8	1375	// Crawler Moving
sayzyas	10:a2bd7d07c7f8	1376	// -------------------------------------
sayzyas	10:a2bd7d07c7f8	1377	if( flg_exp_status & 0x00F00000 ){
sayzyas	10:a2bd7d07c7f8	1378	// Forward move 5
sayzyas	10:a2bd7d07c7f8	1379	// Reverce move a
sayzyas	10:a2bd7d07c7f8	1380	// Right rotation 6
sayzyas	10:a2bd7d07c7f8	1381	// Left rotation 9
sayzyas	11:ff06edc0219c	1382
sayzyas	10:a2bd7d07c7f8	1383	if((flg_exp_status & 0x00F00000) == 0x00500000 ){
sayzyas	11:ff06edc0219c	1384	// 01234 5678 9012 34569
sayzyas	14:3a5ae61ab1f4	1385	sprintf( dbuffer, "BCFW %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	14:3a5ae61ab1f4	1386	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1387	}
sayzyas	10:a2bd7d07c7f8	1388	else if((flg_exp_status & 0x00F00000) == 0x00a00000 ){
sayzyas	14:3a5ae61ab1f4	1389	sprintf( dbuffer, "BCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	14:3a5ae61ab1f4	1390	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1391	}
sayzyas	10:a2bd7d07c7f8	1392	else if((flg_exp_status & 0x00F00000) == 0x00600000 ){
sayzyas	14:3a5ae61ab1f4	1393	sprintf( dbuffer, "BCRR %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	14:3a5ae61ab1f4	1394	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1395	}
sayzyas	10:a2bd7d07c7f8	1396	else if((flg_exp_status & 0x00F00000) == 0x00900000 ){
sayzyas	14:3a5ae61ab1f4	1397	sprintf( dbuffer, "BCLR %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	14:3a5ae61ab1f4	1398	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1399	}
sayzyas	10:a2bd7d07c7f8	1400	else if((flg_exp_status & 0x00F00000) == 0x00800000 ){
sayzyas	13:2c70c772fe24	1401	sprintf( dbuffer, "LCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	14:3a5ae61ab1f4	1402	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1403	}
sayzyas	10:a2bd7d07c7f8	1404	else if((flg_exp_status & 0x00F00000) == 0x00400000 ){
sayzyas	13:2c70c772fe24	1405	sprintf( dbuffer, "LCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	14:3a5ae61ab1f4	1406	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1407	}
sayzyas	10:a2bd7d07c7f8	1408	else if((flg_exp_status & 0x00F00000) == 0x00200000 ){
sayzyas	13:2c70c772fe24	1409	sprintf( dbuffer, "RCRV %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	14:3a5ae61ab1f4	1410	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1411	}
sayzyas	10:a2bd7d07c7f8	1412	else if((flg_exp_status & 0x00F00000) == 0x00100000 ){
sayzyas	13:2c70c772fe24	1413	sprintf( dbuffer, "RCFW %03d %03d %04d %03d\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts);
sayzyas	14:3a5ae61ab1f4	1414	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	1415	}
sayzyas	11:ff06edc0219c	1416	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	14:3a5ae61ab1f4	1417	pc.printf("\t\t\t S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1418	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1419	}
sayzyas	10:a2bd7d07c7f8	1420	// -------------------------------------
sayzyas	10:a2bd7d07c7f8	1421	// Transform
sayzyas	10:a2bd7d07c7f8	1422	// -------------------------------------
sayzyas	10:a2bd7d07c7f8	1423	else if( flg_exp_status & 0x20000000 ){
sayzyas	13:2c70c772fe24	1424	sprintf(dbuffer,"RF2I %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // RF Crawler Tfm I
sayzyas	11:ff06edc0219c	1425	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	14:3a5ae61ab1f4	1426	pc.printf("\t\t\t S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1427	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1428	}
sayzyas	10:a2bd7d07c7f8	1429	else if( flg_exp_status & 0x10000000 ){
sayzyas	13:2c70c772fe24	1430	sprintf(dbuffer,"RF2K %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // RF Crawler Tfm K
sayzyas	11:ff06edc0219c	1431	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1432	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1433	}
sayzyas	10:a2bd7d07c7f8	1434	else if( flg_exp_status & 0x80000000 ){
sayzyas	13:2c70c772fe24	1435	sprintf(dbuffer,"LB2I %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // LB Crawler Tfm I
sayzyas	11:ff06edc0219c	1436	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1437	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1438	}
sayzyas	10:a2bd7d07c7f8	1439	else if( flg_exp_status & 0x40000000 ){
sayzyas	13:2c70c772fe24	1440	sprintf(dbuffer,"LB2K %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // LB Crawler Tfm K
sayzyas	11:ff06edc0219c	1441	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1442	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1443	}
sayzyas	10:a2bd7d07c7f8	1444	// -------------------------------------
sayzyas	10:a2bd7d07c7f8	1445	// Winch Moving
sayzyas	10:a2bd7d07c7f8	1446	// -------------------------------------
sayzyas	10:a2bd7d07c7f8	1447	else if( flg_exp_status & 0x01000000 ){ // Wincd down (FWD)
sayzyas	13:2c70c772fe24	1448	sprintf(dbuffer,"WCDN %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
sayzyas	11:ff06edc0219c	1449	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1450	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1451	}
sayzyas	10:a2bd7d07c7f8	1452	else if( flg_exp_status & 0x02000000 ){ // Winch up (RVS)
sayzyas	13:2c70c772fe24	1453	sprintf(dbuffer,"WCUP %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
sayzyas	11:ff06edc0219c	1454	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1455	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1456	}
sayzyas	10:a2bd7d07c7f8	1457	else if( flg_exp_status & 0x0000000f ){
sayzyas	13:2c70c772fe24	1458	if( cnt == 3 ){
sayzyas	14:3a5ae61ab1f4	1459	sprintf( dbuffer,"JSMD %03d %03d 0000 000\0", baseOperation.sv_JS_ShapeMode, baseOperation.sv_WinchValid );
sayzyas	11:ff06edc0219c	1460	cnt = 0;
sayzyas	11:ff06edc0219c	1461	}
sayzyas	11:ff06edc0219c	1462	else{
sayzyas	13:2c70c772fe24	1463	sprintf( dbuffer,"OFF_ %03d %03d %04d %03d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition, limitSw_Sts ); // Winch down
sayzyas	11:ff06edc0219c	1464	}
sayzyas	11:ff06edc0219c	1465	cnt++;
sayzyas	11:ff06edc0219c	1466	DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas	10:a2bd7d07c7f8	1467	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1468	}
sayzyas	10:a2bd7d07c7f8	1469	else{
sayzyas	11:ff06edc0219c	1470	sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
sayzyas	10:a2bd7d07c7f8	1471	Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1472	}
sayzyas	10:a2bd7d07c7f8	1473	/* Send data to client PC */
sayzyas	10:a2bd7d07c7f8	1474	udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
sayzyas	10:a2bd7d07c7f8	1475	Thread::wait(10);
sayzyas	13:2c70c772fe24	1476
sayzyas	13:2c70c772fe24	1477	// ----------------------------------------------------------------------------
sayzyas	13:2c70c772fe24	1478	// Read target(transform controller) status in each time.
sayzyas	13:2c70c772fe24	1479	read_motorCurrent( I2C_ADDRESS_TRANSFORM, I2C_readcmd, I2C_res, 4 );
sayzyas	13:2c70c772fe24	1480	motor1_current_pct = I2C_res[0];
sayzyas	13:2c70c772fe24	1481	motor2_current_pct = I2C_res[1];
sayzyas	13:2c70c772fe24	1482	limitSw_Sts = I2C_res[3];
sayzyas	13:2c70c772fe24	1483	// DEBUG_PRINT_L2( "Bd0> X: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas	13:2c70c772fe24	1484	// DEBUG_PRINT_L2( "Bd0> X: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas	13:2c70c772fe24	1485	// DEBUG_PRINT_L2( "Bd0> X: Limit switch status (0x%02x)%\r\n", limitSw_Sts );
sayzyas	13:2c70c772fe24	1486	// ----------------------------------------------------------------------------
sayzyas	13:2c70c772fe24	1487
sayzyas	10:a2bd7d07c7f8	1488	}
sayzyas	10:a2bd7d07c7f8	1489	}
sayzyas	14:3a5ae61ab1f4	1490	// Thread::wait(100); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1491	led4 = 0;
sayzyas	14:3a5ae61ab1f4	1492	// Thread::wait(100); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas	10:a2bd7d07c7f8	1493	}
sayzyas	10:a2bd7d07c7f8	1494	}
sayzyas	10:a2bd7d07c7f8	1495
sayzyas	10:a2bd7d07c7f8	1496	// **************************************************************
sayzyas	10:a2bd7d07c7f8	1497	// TASK: GamaPad Task
sayzyas	10:a2bd7d07c7f8	1498	//
sayzyas	10:a2bd7d07c7f8	1499	// **************************************************************
sayzyas	10:a2bd7d07c7f8	1500	void gamepad_task(void const *)
sayzyas	10:a2bd7d07c7f8	1501	{
sayzyas	10:a2bd7d07c7f8	1502
sayzyas	10:a2bd7d07c7f8	1503	char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
sayzyas	10:a2bd7d07c7f8	1504
sayzyas	10:a2bd7d07c7f8	1505	// int counter = 0;
sayzyas	10:a2bd7d07c7f8	1506	// USB HOST GAMEPAD
sayzyas	10:a2bd7d07c7f8	1507	USBHostGamepad gamepad;
sayzyas	10:a2bd7d07c7f8	1508
sayzyas	10:a2bd7d07c7f8	1509	led1 = 1;
sayzyas	10:a2bd7d07c7f8	1510
sayzyas	10:a2bd7d07c7f8	1511	while(1) {
sayzyas	10:a2bd7d07c7f8	1512
sayzyas	10:a2bd7d07c7f8	1513	// try to connect a USB Gamepad
sayzyas	10:a2bd7d07c7f8	1514	while(!gamepad.connect()) {
sayzyas	10:a2bd7d07c7f8	1515	flg_gamePad_Connected = 0;
sayzyas	10:a2bd7d07c7f8	1516	led2 = OFF;
sayzyas	10:a2bd7d07c7f8	1517	// led1 = 1;
sayzyas	10:a2bd7d07c7f8	1518	Thread::wait(500);
sayzyas	10:a2bd7d07c7f8	1519	}
sayzyas	10:a2bd7d07c7f8	1520
sayzyas	10:a2bd7d07c7f8	1521	// when connected, attach handler called on gamepad event
sayzyas	10:a2bd7d07c7f8	1522	gamepad.attachEvent(onControl);
sayzyas	10:a2bd7d07c7f8	1523
sayzyas	10:a2bd7d07c7f8	1524	// wait until the Gamepad is disconnected
sayzyas	10:a2bd7d07c7f8	1525	while(gamepad.connected()) {
sayzyas	10:a2bd7d07c7f8	1526	flg_gamePad_Connected = 1;
sayzyas	10:a2bd7d07c7f8	1527	led2 = !led2;
sayzyas	10:a2bd7d07c7f8	1528	led2 = ON;
sayzyas	10:a2bd7d07c7f8	1529	// led1 = 1;
sayzyas	10:a2bd7d07c7f8	1530
sayzyas	11:ff06edc0219c	1531	// Send status to Handy Ctrl controller, but currently this is only for Main Controller.
sayzyas	14:3a5ae61ab1f4	1532	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = 0x00;
sayzyas	14:3a5ae61ab1f4	1533	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = 0x01;
sayzyas	13:2c70c772fe24	1534	i2c.write(I2C_ADDRESS_HANDY, I2C_cmd, NumberOfI2CCommand);
sayzyas	13:2c70c772fe24	1535	// DEBUG_PRINT_L2("Bd0> Send Handy Controller(%02x) >>>>>>>>>>\r\n",I2C_ADDRESS_HANDY );
sayzyas	10:a2bd7d07c7f8	1536
sayzyas	10:a2bd7d07c7f8	1537	Thread::wait(500);
sayzyas	10:a2bd7d07c7f8	1538	}
sayzyas	10:a2bd7d07c7f8	1539	}
sayzyas	10:a2bd7d07c7f8	1540	}
sayzyas	10:a2bd7d07c7f8	1541
sayzyas	10:a2bd7d07c7f8	1542	// ======================================================================
sayzyas	14:3a5ae61ab1f4	1543	// Read setting value from lpcal file system of mbed
sayzyas	10:a2bd7d07c7f8	1544	// ======================================================================
sayzyas	14:3a5ae61ab1f4	1545
sayzyas	14:3a5ae61ab1f4	1546	bool read_LFS( setValue_t* setValue ){
sayzyas	14:3a5ae61ab1f4	1547	FILE *fp;
sayzyas	14:3a5ae61ab1f4	1548	char *fname = "/local/set.txt";
sayzyas	14:3a5ae61ab1f4	1549	char s[150];
sayzyas	14:3a5ae61ab1f4	1550	int c;
sayzyas	14:3a5ae61ab1f4	1551	int data;
sayzyas	14:3a5ae61ab1f4	1552	bool rts;
sayzyas	10:a2bd7d07c7f8	1553
sayzyas	11:ff06edc0219c	1554	flg_mutex.lock();
sayzyas	14:3a5ae61ab1f4	1555	fp = fopen(fname, "r");
sayzyas	14:3a5ae61ab1f4	1556	if( fp != NULL ){ // Open "set.txt" on the local file system for writing
sayzyas	14:3a5ae61ab1f4	1557	c = getc(fp);
sayzyas	14:3a5ae61ab1f4	1558	if( c != '#' ){
sayzyas	14:3a5ae61ab1f4	1559	pc.printf( "#### ERROR This is not a setting file ####\r\n");
sayzyas	14:3a5ae61ab1f4	1560	rts = false;
sayzyas	14:3a5ae61ab1f4	1561	}
sayzyas	14:3a5ae61ab1f4	1562	else{
sayzyas	14:3a5ae61ab1f4	1563	fgets( s, 100, fp );
sayzyas	14:3a5ae61ab1f4	1564	pc.printf( "%s", s );
sayzyas	14:3a5ae61ab1f4	1565
sayzyas	14:3a5ae61ab1f4	1566	fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WDM_ith_F=data; pc.printf("%04d",setValue->winchCtrl.sv_WDM_ith_F); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1567	fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WDM_ith_R=data; pc.printf("%04d",setValue->winchCtrl.sv_WDM_ith_R); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1568	fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRM_ith_F=data; pc.printf("%04d",setValue->winchCtrl.sv_WRM_ith_F); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1569	fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRM_ith_R=data; pc.printf("%04d",setValue->winchCtrl.sv_WRM_ith_R); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1570
sayzyas	14:3a5ae61ab1f4	1571	fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_hsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_hsrto_F); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1572	fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_hsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_hsrto_R); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1573	fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_lsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_lsrto_F); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1574	fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WDM_lsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WDM_lsrto_R); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1575
sayzyas	14:3a5ae61ab1f4	1576	fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_hsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_hsrto_F); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1577	fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_hsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_hsrto_R); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1578	fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_lsrto_F=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_lsrto_F); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1579	fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRM_lsrto_R=data; pc.printf("%03d",setValue->winchCtrl.sv_WRM_lsrto_R); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1580
sayzyas	14:3a5ae61ab1f4	1581	fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRS_DramDmrx100=data; pc.printf("%04d",setValue->winchCtrl.sv_WRS_DramDmrx100); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1582	fscanf(fp,"%04d",&data);setValue->winchCtrl.sv_WRS_CCableDmrx100=data; pc.printf("%04d",setValue->winchCtrl.sv_WRS_CCableDmrx100); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1583	fscanf(fp,"%03d",&data);setValue->winchCtrl.sv_WRS_RResolution=data; pc.printf("%03d",setValue->winchCtrl.sv_WRS_RResolution); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1584	fscanf(fp,"%03d",&data);setValue->winchCtrl.reserved=data; pc.printf("%03d",setValue->winchCtrl.reserved); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1585
sayzyas	14:3a5ae61ab1f4	1586	fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_RFTM_ith_F=data; pc.printf("%04d",setValue->tfmCtrl.sv_RFTM_ith_F); fgets(s, 100, fp ); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1587	fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_RFTM_ith_R=data; pc.printf("%04d",setValue->tfmCtrl.sv_RFTM_ith_R); fgets(s, 100, fp ); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1588	fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_LBTM_ith_F=data; pc.printf("%04d",setValue->tfmCtrl.sv_LBTM_ith_F); fgets(s, 100, fp ); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1589	fscanf(fp,"%04d",&data);setValue->tfmCtrl.sv_LBTM_ith_R=data; pc.printf("%04d",setValue->tfmCtrl.sv_LBTM_ith_R); fgets(s, 100, fp ); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1590	fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_RFTM_srto_F=data; pc.printf("%03d",setValue->tfmCtrl.sv_RFTM_srto_F); fgets(s, 100, fp ); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1591	fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_RFTM_srto_R=data; pc.printf("%03d",setValue->tfmCtrl.sv_RFTM_srto_R); fgets(s, 100, fp ); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1592	fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_LBTM_srto_F=data; pc.printf("%03d",setValue->tfmCtrl.sv_LBTM_srto_F); fgets(s, 100, fp ); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1593	fscanf(fp,"%03d",&data);setValue->tfmCtrl.sv_LBTM_srto_R=data; pc.printf("%03d",setValue->tfmCtrl.sv_LBTM_srto_R); fgets(s, 100, fp ); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1594
sayzyas	14:3a5ae61ab1f4	1595	fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_RFCM_ith_F=data; pc.printf("%04d",setValue->crawlerCtrl.sv_RFCM_ith_F); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1596	fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_RFCM_ith_R=data; pc.printf("%04d",setValue->crawlerCtrl.sv_RFCM_ith_R); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1597	fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_LBCM_ith_F=data; pc.printf("%04d",setValue->crawlerCtrl.sv_LBCM_ith_F); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1598	fscanf(fp,"%04d",&data);setValue->crawlerCtrl.sv_LBCM_ith_R=data; pc.printf("%04d",setValue->crawlerCtrl.sv_LBCM_ith_R); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1599	fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_srto_F=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_srto_F); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1600	fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_srto_R=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_srto_R); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1601	fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_srto_F=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_srto_F); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1602	fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_srto_R=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_srto_R); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1603	fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_dzu=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_dzu); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1604	fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_dzc=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_dzc); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1605	fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_RFCM_dzl=data; pc.printf("%03d",setValue->crawlerCtrl.sv_RFCM_dzl); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1606	fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_dzu=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_dzu); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1607	fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_dzc=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_dzc); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1608	fscanf(fp,"%03d",&data);setValue->crawlerCtrl.sv_LBCM_dzl=data; pc.printf("%03d",setValue->crawlerCtrl.sv_LBCM_dzl); fgets(s,100,fp); pc.printf("%s",s );
sayzyas	14:3a5ae61ab1f4	1609	fscanf(fp,"%03d",&data);setValue->crawlerCtrl.reserved1=data; pc.printf("%03d",setValue->crawlerCtrl.reserved1); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1610	fscanf(fp,"%03d",&data);setValue->crawlerCtrl.reserved2=data; pc.printf("%03d",setValue->crawlerCtrl.reserved2); fgets(s,100,fp); pc.printf("%s",s);
sayzyas	14:3a5ae61ab1f4	1611	}
sayzyas	14:3a5ae61ab1f4	1612	fclose(fp);
sayzyas	14:3a5ae61ab1f4	1613	rts = true;
sayzyas	10:a2bd7d07c7f8	1614	}
sayzyas	10:a2bd7d07c7f8	1615	else{
sayzyas	14:3a5ae61ab1f4	1616	pc.printf( "#### ERROR local file open error ####\r\n");
sayzyas	14:3a5ae61ab1f4	1617	rts = false;
sayzyas	14:3a5ae61ab1f4	1618	}
sayzyas	11:ff06edc0219c	1619	flg_mutex.unlock();
sayzyas	14:3a5ae61ab1f4	1620	return rts;
sayzyas	10:a2bd7d07c7f8	1621	}
sayzyas	10:a2bd7d07c7f8	1622
sayzyas	14:3a5ae61ab1f4	1623	bool write_LFS( setValue_t* setValue ){
sayzyas	14:3a5ae61ab1f4	1624	FILE *fp;
sayzyas	14:3a5ae61ab1f4	1625	char *fname = "/local/set.txt";
sayzyas	14:3a5ae61ab1f4	1626	bool rts = true;
sayzyas	14:3a5ae61ab1f4	1627
sayzyas	14:3a5ae61ab1f4	1628	pc.printf("write setting data to setting file \r\n ");
sayzyas	14:3a5ae61ab1f4	1629
sayzyas	14:3a5ae61ab1f4	1630	fp = fopen(fname, "w"); // Open "out.txt" on the local file system for writing
sayzyas	14:3a5ae61ab1f4	1631	if( fp != NULL ){
sayzyas	14:3a5ae61ab1f4	1632	fprintf(fp, "#### B2 DebrisServayor Setting ####\n");
sayzyas	14:3a5ae61ab1f4	1633	fclose(fp);
sayzyas	14:3a5ae61ab1f4	1634	}
sayzyas	14:3a5ae61ab1f4	1635	else{
sayzyas	14:3a5ae61ab1f4	1636	rts = false;
sayzyas	14:3a5ae61ab1f4	1637	pc.printf("File open error (0)\r\n");
sayzyas	14:3a5ae61ab1f4	1638	}
sayzyas	14:3a5ae61ab1f4	1639	Thread::wait(50);
sayzyas	14:3a5ae61ab1f4	1640	fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
sayzyas	14:3a5ae61ab1f4	1641	if( fp != NULL ){
sayzyas	14:3a5ae61ab1f4	1642	pc.printf("Writing ... %04d #WDM F-C th\n", setValue->winchCtrl.sv_WDM_ith_F);
sayzyas	14:3a5ae61ab1f4	1643	fprintf(fp, "%04d #WDM F-C th\n", setValue->winchCtrl.sv_WDM_ith_F);
sayzyas	14:3a5ae61ab1f4	1644	pc.printf("Writing ... %04d #WDM R-C th\n", setValue->winchCtrl.sv_WDM_ith_R);
sayzyas	14:3a5ae61ab1f4	1645	fprintf(fp, "%04d #WDM R-C th\n", setValue->winchCtrl.sv_WDM_ith_R);
sayzyas	14:3a5ae61ab1f4	1646	pc.printf("Writing ... %04d #WM2 F-C th\n", setValue->winchCtrl.sv_WRM_ith_F);
sayzyas	14:3a5ae61ab1f4	1647	fprintf(fp, "%04d #WM2 F-C th\n", setValue->winchCtrl.sv_WRM_ith_F);
sayzyas	14:3a5ae61ab1f4	1648	pc.printf("Writing ... %04d #WM2 R-C th\n", setValue->winchCtrl.sv_WRM_ith_R);
sayzyas	14:3a5ae61ab1f4	1649	fprintf(fp, "%04d #WM2 R-C th\n", setValue->winchCtrl.sv_WRM_ith_R);
sayzyas	14:3a5ae61ab1f4	1650	fclose(fp);
sayzyas	14:3a5ae61ab1f4	1651	}
sayzyas	14:3a5ae61ab1f4	1652	else{
sayzyas	14:3a5ae61ab1f4	1653	pc.printf("File open error (1)\r\n");
sayzyas	14:3a5ae61ab1f4	1654	}
sayzyas	14:3a5ae61ab1f4	1655	Thread::wait(50);
sayzyas	14:3a5ae61ab1f4	1656
sayzyas	14:3a5ae61ab1f4	1657	fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
sayzyas	14:3a5ae61ab1f4	1658	if( fp != NULL ){
sayzyas	14:3a5ae61ab1f4	1659	pc.printf("Writing ... %03d #WDM F-HS\n", setValue->winchCtrl.sv_WDM_hsrto_F);
sayzyas	14:3a5ae61ab1f4	1660	fprintf(fp, "%03d #WDM F-HS\n", setValue->winchCtrl.sv_WDM_hsrto_F);
sayzyas	14:3a5ae61ab1f4	1661	pc.printf("Writing ... %03d #WDM R-HS\n", setValue->winchCtrl.sv_WDM_hsrto_R);
sayzyas	14:3a5ae61ab1f4	1662	fprintf(fp, "%03d #WDM R-HS\n", setValue->winchCtrl.sv_WDM_hsrto_R);
sayzyas	14:3a5ae61ab1f4	1663	pc.printf("Writing ... %03d #WDM F-LS\n", setValue->winchCtrl.sv_WDM_lsrto_F);
sayzyas	14:3a5ae61ab1f4	1664	fprintf(fp, "%03d #WDM F-LS\n", setValue->winchCtrl.sv_WDM_lsrto_F);
sayzyas	14:3a5ae61ab1f4	1665	pc.printf("Writing ... %03d #WDM R-LS\n", setValue->winchCtrl.sv_WDM_lsrto_R);
sayzyas	14:3a5ae61ab1f4	1666	fprintf(fp, "%03d #WDM R-LS\n", setValue->winchCtrl.sv_WDM_lsrto_R);
sayzyas	14:3a5ae61ab1f4	1667	fclose(fp);
sayzyas	10:a2bd7d07c7f8	1668	}
sayzyas	10:a2bd7d07c7f8	1669	else{
sayzyas	14:3a5ae61ab1f4	1670	rts = false;
sayzyas	14:3a5ae61ab1f4	1671	pc.printf("File open error (2)\r\n");
sayzyas	14:3a5ae61ab1f4	1672	}
sayzyas	14:3a5ae61ab1f4	1673	Thread::wait(50);
sayzyas	14:3a5ae61ab1f4	1674
sayzyas	14:3a5ae61ab1f4	1675	fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
sayzyas	14:3a5ae61ab1f4	1676	if( fp != NULL ){
sayzyas	14:3a5ae61ab1f4	1677	pc.printf("Writing ... %03d #WM2 F-HS\n", setValue->winchCtrl.sv_WRM_hsrto_F);
sayzyas	14:3a5ae61ab1f4	1678	fprintf(fp, "%03d #WM2 F-HS\n", setValue->winchCtrl.sv_WRM_hsrto_F);
sayzyas	14:3a5ae61ab1f4	1679	pc.printf("Writing ... %03d #WM2 R-HS\n", setValue->winchCtrl.sv_WRM_hsrto_R);
sayzyas	14:3a5ae61ab1f4	1680	fprintf(fp, "%03d #WM2 R-HS\n", setValue->winchCtrl.sv_WRM_hsrto_R);
sayzyas	14:3a5ae61ab1f4	1681	pc.printf("Writing ... %03d #WM2 F-LS\n", setValue->winchCtrl.sv_WRM_lsrto_F);
sayzyas	14:3a5ae61ab1f4	1682	fprintf(fp, "%03d #WM2 F-LS\n", setValue->winchCtrl.sv_WRM_lsrto_F);
sayzyas	14:3a5ae61ab1f4	1683	pc.printf("Writing ... %03d #WM2 R-LS\n", setValue->winchCtrl.sv_WRM_lsrto_R);
sayzyas	14:3a5ae61ab1f4	1684	fprintf(fp, "%03d #WM2 R-LS\n", setValue->winchCtrl.sv_WRM_lsrto_R);
sayzyas	14:3a5ae61ab1f4	1685	fclose(fp);
sayzyas	14:3a5ae61ab1f4	1686	}
sayzyas	14:3a5ae61ab1f4	1687	else{
sayzyas	14:3a5ae61ab1f4	1688	rts = false;
sayzyas	14:3a5ae61ab1f4	1689	pc.printf("File open error (3)\r\n");
sayzyas	14:3a5ae61ab1f4	1690	}
sayzyas	14:3a5ae61ab1f4	1691	Thread::wait(50);
sayzyas	14:3a5ae61ab1f4	1692
sayzyas	14:3a5ae61ab1f4	1693	fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
sayzyas	14:3a5ae61ab1f4	1694	if( fp != NULL ){
sayzyas	14:3a5ae61ab1f4	1695	pc.printf("Writing ... %04d #WD D-DMx100\n", setValue->winchCtrl.sv_WRS_DramDmrx100);
sayzyas	14:3a5ae61ab1f4	1696	fprintf(fp, "%04d #WD D-DMx100\n", setValue->winchCtrl.sv_WRS_DramDmrx100);
sayzyas	14:3a5ae61ab1f4	1697	pc.printf("Writing ... %04d #WCC Dx100\n", setValue->winchCtrl.sv_WRS_CCableDmrx100);
sayzyas	14:3a5ae61ab1f4	1698	fprintf(fp, "%04d #WCC Dx100\n", setValue->winchCtrl.sv_WRS_CCableDmrx100);
sayzyas	14:3a5ae61ab1f4	1699	pc.printf("Writing ... %03d #R-Res\n", setValue->winchCtrl.sv_WRS_RResolution);
sayzyas	14:3a5ae61ab1f4	1700	fprintf(fp, "%03d #R-Res\n", setValue->winchCtrl.sv_WRS_RResolution);
sayzyas	14:3a5ae61ab1f4	1701	pc.printf("Writing ... %03d #Res\n", setValue->winchCtrl.reserved);
sayzyas	14:3a5ae61ab1f4	1702	fprintf(fp, "%03d #res\n", setValue->winchCtrl.reserved);
sayzyas	14:3a5ae61ab1f4	1703	fclose(fp);
sayzyas	14:3a5ae61ab1f4	1704	}
sayzyas	14:3a5ae61ab1f4	1705	else{
sayzyas	14:3a5ae61ab1f4	1706	rts = false;
sayzyas	14:3a5ae61ab1f4	1707	pc.printf("File open error (4)\r\n");
sayzyas	14:3a5ae61ab1f4	1708	}
sayzyas	14:3a5ae61ab1f4	1709	Thread::wait(50);
sayzyas	14:3a5ae61ab1f4	1710
sayzyas	14:3a5ae61ab1f4	1711	fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
sayzyas	14:3a5ae61ab1f4	1712	if( fp != NULL ){
sayzyas	14:3a5ae61ab1f4	1713	pc.printf("Writing ... %04d #R-TFM F-C\n", setValue->tfmCtrl.sv_RFTM_ith_F);
sayzyas	14:3a5ae61ab1f4	1714	fprintf(fp, "%04d #R-TFM F-C\n", setValue->tfmCtrl.sv_RFTM_ith_F);
sayzyas	14:3a5ae61ab1f4	1715	pc.printf("Writing ... %04d #R-TFM R-C\n", setValue->tfmCtrl.sv_RFTM_ith_R);
sayzyas	14:3a5ae61ab1f4	1716	fprintf(fp, "%04d #R-TFM R-C\n", setValue->tfmCtrl.sv_RFTM_ith_R);
sayzyas	14:3a5ae61ab1f4	1717	pc.printf("Writing ... %04d #L-TFM F-C\n", setValue->tfmCtrl.sv_LBTM_ith_F);;
sayzyas	14:3a5ae61ab1f4	1718	fprintf(fp, "%04d #L-TFM F-C\n", setValue->tfmCtrl.sv_LBTM_ith_F);;
sayzyas	14:3a5ae61ab1f4	1719	pc.printf("Writing ... %04d #L-TFM R-C\n", setValue->tfmCtrl.sv_LBTM_ith_R);
sayzyas	14:3a5ae61ab1f4	1720	fprintf(fp, "%04d #L-TFM R-C\n", setValue->tfmCtrl.sv_LBTM_ith_R);
sayzyas	14:3a5ae61ab1f4	1721	fclose(fp);
sayzyas	14:3a5ae61ab1f4	1722	}
sayzyas	14:3a5ae61ab1f4	1723	else{
sayzyas	14:3a5ae61ab1f4	1724	rts = false;
sayzyas	14:3a5ae61ab1f4	1725	pc.printf("File open error (5)\r\n");
sayzyas	14:3a5ae61ab1f4	1726	}
sayzyas	14:3a5ae61ab1f4	1727	Thread::wait(50);
sayzyas	11:ff06edc0219c	1728
sayzyas	14:3a5ae61ab1f4	1729	fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
sayzyas	14:3a5ae61ab1f4	1730	if( fp != NULL ){
sayzyas	14:3a5ae61ab1f4	1731	pc.printf("Writing ... %03d #R-TFM F-S\n", setValue->tfmCtrl.sv_RFTM_srto_F);
sayzyas	14:3a5ae61ab1f4	1732	fprintf(fp, "%03d #R-TFM F-S\n", setValue->tfmCtrl.sv_RFTM_srto_F);
sayzyas	14:3a5ae61ab1f4	1733	pc.printf("Writing ... %03d #R-TFM R-S\n", setValue->tfmCtrl.sv_RFTM_srto_R);
sayzyas	14:3a5ae61ab1f4	1734	fprintf(fp, "%03d #R-TFM R-S\n", setValue->tfmCtrl.sv_RFTM_srto_R);
sayzyas	14:3a5ae61ab1f4	1735	pc.printf("Writing ... %03d #L-TFM F-S\n", setValue->tfmCtrl.sv_LBTM_srto_F);
sayzyas	14:3a5ae61ab1f4	1736	fprintf(fp, "%03d #L-TFM F-S\n", setValue->tfmCtrl.sv_LBTM_srto_F);
sayzyas	14:3a5ae61ab1f4	1737	pc.printf("Writing ... %03d #L-TFM R-S\n", setValue->tfmCtrl.sv_LBTM_srto_R);
sayzyas	14:3a5ae61ab1f4	1738	fprintf(fp, "%03d #L-TFM R-S\n", setValue->tfmCtrl.sv_LBTM_srto_R);
sayzyas	14:3a5ae61ab1f4	1739	fclose(fp);
sayzyas	11:ff06edc0219c	1740	}
sayzyas	11:ff06edc0219c	1741	else{
sayzyas	14:3a5ae61ab1f4	1742	rts = false;
sayzyas	14:3a5ae61ab1f4	1743	pc.printf("File open error (6)\r\n");
sayzyas	14:3a5ae61ab1f4	1744	}
sayzyas	14:3a5ae61ab1f4	1745	Thread::wait(50);
sayzyas	14:3a5ae61ab1f4	1746
sayzyas	14:3a5ae61ab1f4	1747	fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
sayzyas	14:3a5ae61ab1f4	1748	if( fp != NULL ){
sayzyas	14:3a5ae61ab1f4	1749	pc.printf("Writing ... %04d #RC F-C th\n", setValue->crawlerCtrl.sv_RFCM_ith_F);
sayzyas	14:3a5ae61ab1f4	1750	fprintf(fp, "%04d #RC F-C th\n", setValue->crawlerCtrl.sv_RFCM_ith_F);
sayzyas	14:3a5ae61ab1f4	1751	pc.printf("Writing ... %04d #RC R-C th\n", setValue->crawlerCtrl.sv_RFCM_ith_R);
sayzyas	14:3a5ae61ab1f4	1752	fprintf(fp, "%04d #RC R-C th\n", setValue->crawlerCtrl.sv_RFCM_ith_R);
sayzyas	14:3a5ae61ab1f4	1753	pc.printf("Writing ... %04d #LC F-C th\n", setValue->crawlerCtrl.sv_LBCM_ith_F);
sayzyas	14:3a5ae61ab1f4	1754	fprintf(fp, "%04d #LC F-C th\n", setValue->crawlerCtrl.sv_LBCM_ith_F);
sayzyas	14:3a5ae61ab1f4	1755	pc.printf("Writing ... %04d #LC R-C th\n", setValue->crawlerCtrl.sv_LBCM_ith_R);
sayzyas	14:3a5ae61ab1f4	1756	fprintf(fp, "%04d #LC R-C th\n", setValue->crawlerCtrl.sv_LBCM_ith_R);
sayzyas	14:3a5ae61ab1f4	1757	fclose(fp);
sayzyas	14:3a5ae61ab1f4	1758	}
sayzyas	14:3a5ae61ab1f4	1759	else{
sayzyas	14:3a5ae61ab1f4	1760	rts = false;
sayzyas	14:3a5ae61ab1f4	1761	pc.printf("File open error (7)\r\n");
sayzyas	14:3a5ae61ab1f4	1762	}
sayzyas	14:3a5ae61ab1f4	1763	Thread::wait(50);
sayzyas	11:ff06edc0219c	1764
sayzyas	14:3a5ae61ab1f4	1765	fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
sayzyas	14:3a5ae61ab1f4	1766	if( fp != NULL ){
sayzyas	14:3a5ae61ab1f4	1767	pc.printf("Writing ... %03d #RC F-S\n", setValue->crawlerCtrl.sv_RFCM_srto_F);
sayzyas	14:3a5ae61ab1f4	1768	fprintf(fp, "%03d #RC F-S\n", setValue->crawlerCtrl.sv_RFCM_srto_F);
sayzyas	14:3a5ae61ab1f4	1769	pc.printf("Writing ... %03d #RC R-S\n", setValue->crawlerCtrl.sv_RFCM_srto_R);
sayzyas	14:3a5ae61ab1f4	1770	fprintf(fp, "%03d #RC R-S\n", setValue->crawlerCtrl.sv_RFCM_srto_R);
sayzyas	14:3a5ae61ab1f4	1771	pc.printf("Writing ... %03d #LC F-S\n", setValue->crawlerCtrl.sv_LBCM_srto_F);
sayzyas	14:3a5ae61ab1f4	1772	fprintf(fp, "%03d #LC F-S\n", setValue->crawlerCtrl.sv_LBCM_srto_F);
sayzyas	14:3a5ae61ab1f4	1773	pc.printf("Writing ... %03d #LC R-S\n", setValue->crawlerCtrl.sv_LBCM_srto_R);
sayzyas	14:3a5ae61ab1f4	1774	fprintf(fp, "%03d #LC R-S\n", setValue->crawlerCtrl.sv_LBCM_srto_R);
sayzyas	14:3a5ae61ab1f4	1775	fclose(fp);
sayzyas	14:3a5ae61ab1f4	1776	}
sayzyas	14:3a5ae61ab1f4	1777	else{
sayzyas	14:3a5ae61ab1f4	1778	rts = false;
sayzyas	14:3a5ae61ab1f4	1779	pc.printf("File open error (8)\r\n");
sayzyas	14:3a5ae61ab1f4	1780	}
sayzyas	14:3a5ae61ab1f4	1781
sayzyas	14:3a5ae61ab1f4	1782	fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
sayzyas	14:3a5ae61ab1f4	1783	if( fp != NULL ){
sayzyas	14:3a5ae61ab1f4	1784	pc.printf("Writing ... %03d #RJS U\n", setValue->crawlerCtrl.sv_RFCM_dzu);
sayzyas	14:3a5ae61ab1f4	1785	fprintf(fp, "%03d #RJS U\n", setValue->crawlerCtrl.sv_RFCM_dzu);
sayzyas	14:3a5ae61ab1f4	1786	pc.printf("Writing ... %03d #RJS C\n", setValue->crawlerCtrl.sv_RFCM_dzc);
sayzyas	14:3a5ae61ab1f4	1787	fprintf(fp, "%03d #RJS C\n", setValue->crawlerCtrl.sv_RFCM_dzc);
sayzyas	14:3a5ae61ab1f4	1788	pc.printf("Writing ... %03d #RJS L\n", setValue->crawlerCtrl.sv_RFCM_dzl);
sayzyas	14:3a5ae61ab1f4	1789	fprintf(fp, "%03d #RJS L\n", setValue->crawlerCtrl.sv_RFCM_dzl);
sayzyas	14:3a5ae61ab1f4	1790	pc.printf("Writing ... %03d #LJS U\n", setValue->crawlerCtrl.sv_LBCM_dzu);
sayzyas	14:3a5ae61ab1f4	1791	fprintf(fp, "%03d #LJS U\n", setValue->crawlerCtrl.sv_LBCM_dzu);
sayzyas	14:3a5ae61ab1f4	1792	fclose(fp);
sayzyas	14:3a5ae61ab1f4	1793	}
sayzyas	14:3a5ae61ab1f4	1794	else{
sayzyas	14:3a5ae61ab1f4	1795	rts = false;
sayzyas	14:3a5ae61ab1f4	1796	pc.printf("File open error (9)\r\n");
sayzyas	14:3a5ae61ab1f4	1797	}
sayzyas	14:3a5ae61ab1f4	1798
sayzyas	14:3a5ae61ab1f4	1799	fp = fopen(fname, "a"); // Open "out.txt" on the local file system for writing
sayzyas	14:3a5ae61ab1f4	1800	if( fp != NULL ){
sayzyas	14:3a5ae61ab1f4	1801	pc.printf("Writing ... %03d #LJS C\n", setValue->crawlerCtrl.sv_LBCM_dzc);
sayzyas	14:3a5ae61ab1f4	1802	fprintf(fp, "%03d #LJS C\n", setValue->crawlerCtrl.sv_LBCM_dzc);
sayzyas	14:3a5ae61ab1f4	1803	pc.printf("Writing ... %03d #LJS L\n", setValue->crawlerCtrl.sv_LBCM_dzl);
sayzyas	14:3a5ae61ab1f4	1804	fprintf(fp, "%03d #LJS L\n", setValue->crawlerCtrl.sv_LBCM_dzl);
sayzyas	14:3a5ae61ab1f4	1805	pc.printf("Writing ... %03d #res\n", setValue->crawlerCtrl.reserved1);
sayzyas	14:3a5ae61ab1f4	1806	fprintf(fp, "%03d #res\n", setValue->crawlerCtrl.reserved1);
sayzyas	14:3a5ae61ab1f4	1807	pc.printf("Writing ... %03d #res\n", setValue->crawlerCtrl.reserved2);
sayzyas	14:3a5ae61ab1f4	1808	fprintf(fp, "%03d #res\n", setValue->crawlerCtrl.reserved2);
sayzyas	14:3a5ae61ab1f4	1809	fclose(fp);
sayzyas	14:3a5ae61ab1f4	1810	}
sayzyas	14:3a5ae61ab1f4	1811	else{
sayzyas	14:3a5ae61ab1f4	1812	rts = false;
sayzyas	14:3a5ae61ab1f4	1813	pc.printf("File open error (10)\r\n");
sayzyas	14:3a5ae61ab1f4	1814	}
sayzyas	14:3a5ae61ab1f4	1815
sayzyas	14:3a5ae61ab1f4	1816	pc.printf("settig file write completed \r\n ");
sayzyas	14:3a5ae61ab1f4	1817
sayzyas	14:3a5ae61ab1f4	1818
sayzyas	11:ff06edc0219c	1819	return rts;
sayzyas	10:a2bd7d07c7f8	1820	}
sayzyas	10:a2bd7d07c7f8	1821
sayzyas	10:a2bd7d07c7f8	1822	// ======================================================================
sayzyas	10:a2bd7d07c7f8	1823	// Read Network setting value from lpcal file system of mbed
sayzyas	10:a2bd7d07c7f8	1824	// ======================================================================
sayzyas	10:a2bd7d07c7f8	1825	int read_NetSetting_lfs( char* ip_address, char* subnet_mask, char* gateway )
sayzyas	10:a2bd7d07c7f8	1826	{
sayzyas	10:a2bd7d07c7f8	1827	FILE *rfp;
sayzyas	14:3a5ae61ab1f4	1828
sayzyas	14:3a5ae61ab1f4	1829	DEBUG_PRINT_L3("Bd0> Read Network Setting data from local file system \r\n");
sayzyas	14:3a5ae61ab1f4	1830	rfp = fopen("/local/net.txt", "r"); // Open local file "set.txt" for writing
sayzyas	10:a2bd7d07c7f8	1831	if(!rfp){
sayzyas	11:ff06edc0219c	1832	DEBUG_PRINT_L3("Bd0> !!local file open error!!\r\n");
sayzyas	10:a2bd7d07c7f8	1833	return _FAIL_;
sayzyas	10:a2bd7d07c7f8	1834	}
sayzyas	10:a2bd7d07c7f8	1835	else{
sayzyas	14:3a5ae61ab1f4	1836	Thread::wait(50);
sayzyas	10:a2bd7d07c7f8	1837	fscanf(rfp, "%s", ip_address);
sayzyas	10:a2bd7d07c7f8	1838	fscanf(rfp, "%s", subnet_mask);
sayzyas	10:a2bd7d07c7f8	1839	fscanf(rfp, "%s", gateway);
sayzyas	10:a2bd7d07c7f8	1840	fclose(rfp);
sayzyas	10:a2bd7d07c7f8	1841	return _OK_;
sayzyas	10:a2bd7d07c7f8	1842	}
sayzyas	10:a2bd7d07c7f8	1843	}
sayzyas	10:a2bd7d07c7f8	1844	// ======================================================================
sayzyas	10:a2bd7d07c7f8	1845	// Winch control function
sayzyas	10:a2bd7d07c7f8	1846	// ======================================================================
sayzyas	10:a2bd7d07c7f8	1847	void winchMovingControl(
sayzyas	10:a2bd7d07c7f8	1848	int mode, // Operationg mode: Relative / Abslute
sayzyas	10:a2bd7d07c7f8	1849	char* dbufferP, // Date buffer pointer
sayzyas	10:a2bd7d07c7f8	1850	int dbuffer_s, // Date buffer size
sayzyas	10:a2bd7d07c7f8	1851	winchData_t* winchDataP, // Winch data structure pointer
sayzyas	10:a2bd7d07c7f8	1852	int winchData_s, // Winch data structure size
sayzyas	10:a2bd7d07c7f8	1853	char* I2C_cmd
sayzyas	10:a2bd7d07c7f8	1854	){
sayzyas	10:a2bd7d07c7f8	1855	int rcv_data_cnt;
sayzyas	10:a2bd7d07c7f8	1856	// int moving_data;
sayzyas	10:a2bd7d07c7f8	1857	int man_speed;
sayzyas	13:2c70c772fe24	1858
sayzyas	13:2c70c772fe24	1859	int cnt;
sayzyas	13:2c70c772fe24	1860
sayzyas	13:2c70c772fe24	1861
sayzyas	10:a2bd7d07c7f8	1862	bool flg_stop_operation = false;
sayzyas	10:a2bd7d07c7f8	1863
sayzyas	13:2c70c772fe24	1864	int16_t winchTempPosition;
sayzyas	10:a2bd7d07c7f8	1865
sayzyas	10:a2bd7d07c7f8	1866	char I2C_read[NumberOfI2CCommand+1];
sayzyas	11:ff06edc0219c	1867	char I2C_readcmd[NumberOfI2CCommand+1];
sayzyas	10:a2bd7d07c7f8	1868
sayzyas	10:a2bd7d07c7f8	1869	// if (hwbtn_Opeflg == 1){
sayzyas	10:a2bd7d07c7f8	1870	// Thread::wait(1);
sayzyas	10:a2bd7d07c7f8	1871	// }
sayzyas	10:a2bd7d07c7f8	1872	// else{
sayzyas	10:a2bd7d07c7f8	1873
sayzyas	10:a2bd7d07c7f8	1874
sayzyas	10:a2bd7d07c7f8	1875	if( flg_ButtonOn == true ) {Thread::wait(2);}
sayzyas	10:a2bd7d07c7f8	1876
sayzyas	14:3a5ae61ab1f4	1877	I2C_cmd[I2C_CP_M1_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_F;
sayzyas	14:3a5ae61ab1f4	1878	I2C_cmd[I2C_CP_M1_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_RFCM_ith_R;
sayzyas	14:3a5ae61ab1f4	1879	I2C_cmd[I2C_CP_M2_FWD_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_F;
sayzyas	14:3a5ae61ab1f4	1880	I2C_cmd[I2C_CP_M2_RVS_CNTTH_U] = setValue.crawlerCtrl.sv_LBCM_ith_R;
sayzyas	10:a2bd7d07c7f8	1881
sayzyas	10:a2bd7d07c7f8	1882	if( mode == WINCH_POSITION_CLEAR ){
sayzyas	10:a2bd7d07c7f8	1883	led3 = ON;
sayzyas	11:ff06edc0219c	1884	DEBUG_PRINT_L3("Bd0> === WINCH_POSITION_CLEAR ===\r\n");
sayzyas	10:a2bd7d07c7f8	1885	rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
sayzyas	11:ff06edc0219c	1886	DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	1887	if( rcv_data_cnt < 0 ){
sayzyas	11:ff06edc0219c	1888	DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
sayzyas	10:a2bd7d07c7f8	1889	// tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	10:a2bd7d07c7f8	1890	// break;
sayzyas	10:a2bd7d07c7f8	1891	}
sayzyas	10:a2bd7d07c7f8	1892	else{
sayzyas	11:ff06edc0219c	1893	DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
sayzyas	10:a2bd7d07c7f8	1894	// if( !strcmp( dbuffer, "WinchStepDnOf" ) ){
sayzyas	10:a2bd7d07c7f8	1895	// break;
sayzyas	10:a2bd7d07c7f8	1896	// }
sayzyas	10:a2bd7d07c7f8	1897	}
sayzyas	10:a2bd7d07c7f8	1898
sayzyas	14:3a5ae61ab1f4	1899	I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Zero clear
sayzyas	13:2c70c772fe24	1900	i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1901
sayzyas	10:a2bd7d07c7f8	1902	led3 = OFF;
sayzyas	10:a2bd7d07c7f8	1903	}
sayzyas	10:a2bd7d07c7f8	1904	else if (( mode == WINCH_MMODE_RELATIVE ) \|\| ( mode == WINCH_MMODE_ABSOLUTE )){
sayzyas	11:ff06edc0219c	1905	if ( mode == WINCH_MMODE_RELATIVE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_RELATIVE === \r\n");
sayzyas	11:ff06edc0219c	1906	if ( mode == WINCH_MMODE_ABSOLUTE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_ABSOLUTE === \r\n");
sayzyas	10:a2bd7d07c7f8	1907
sayzyas	10:a2bd7d07c7f8	1908	rcv_data_cnt = tcp_client.receive( dbuffer, dbuffer_s);
sayzyas	10:a2bd7d07c7f8	1909
sayzyas	10:a2bd7d07c7f8	1910	*(dbufferP+rcv_data_cnt) = '\0';
sayzyas	11:ff06edc0219c	1911	winchDataP->operation = '\r\n';
sayzyas	10:a2bd7d07c7f8	1912
sayzyas	11:ff06edc0219c	1913	DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	1914	// Copy received data to Winch data structure.
sayzyas	10:a2bd7d07c7f8	1915	memcpy( winchDataP, (winchData_t *)dbuffer, winchData_s);
sayzyas	10:a2bd7d07c7f8	1916	// winchDataP->dt_WinchDstPosition += winchDataP->dt_WinchCntPosition;
sayzyas	11:ff06edc0219c	1917	DEBUG_PRINT_L3("Bd0> Winch Rtv Move [ From %04d >>> To %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
sayzyas	10:a2bd7d07c7f8	1918
sayzyas	10:a2bd7d07c7f8	1919	swbtn_OpeMutex.lock();
sayzyas	10:a2bd7d07c7f8	1920	swbtn_Opeflg = 1;
sayzyas	10:a2bd7d07c7f8	1921	swbtn_OpeMutex.unlock();
sayzyas	10:a2bd7d07c7f8	1922
sayzyas	13:2c70c772fe24	1923	cnt = 0;
sayzyas	13:2c70c772fe24	1924
sayzyas	10:a2bd7d07c7f8	1925	while( true ){
sayzyas	10:a2bd7d07c7f8	1926	while( true ){
sayzyas	10:a2bd7d07c7f8	1927	led3 = ON;
sayzyas	10:a2bd7d07c7f8	1928	////winchDataP->dt_WinchCntPosition = res_position; // Current position.
sayzyas	10:a2bd7d07c7f8	1929
sayzyas	11:ff06edc0219c	1930	DEBUG_PRINT_L3("Bd0> == Winch Position ==============\r\n");
sayzyas	11:ff06edc0219c	1931	DEBUG_PRINT_L3("Bd0> CURRENT : %d\r\n", winchDataP->dt_WinchCntPosition );
sayzyas	11:ff06edc0219c	1932	DEBUG_PRINT_L3("Bd0> DESTINATION: %d\r\n", winchDataP->dt_WinchDstPosition );
sayzyas	11:ff06edc0219c	1933	DEBUG_PRINT_L3("Bd0> ================================\r\n");
sayzyas	10:a2bd7d07c7f8	1934
sayzyas	10:a2bd7d07c7f8	1935	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	11:ff06edc0219c	1936	DEBUG_PRINT_L3("Bd0> Send Winch Rtv data [ %04d >>>> %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
sayzyas	10:a2bd7d07c7f8	1937
sayzyas	10:a2bd7d07c7f8	1938	rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
sayzyas	11:ff06edc0219c	1939	DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	1940	if( rcv_data_cnt < 0 ){
sayzyas	11:ff06edc0219c	1941	DEBUG_PRINT_L3("##ERROR## Data receive\r\n" );
sayzyas	10:a2bd7d07c7f8	1942
sayzyas	10:a2bd7d07c7f8	1943	// tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	10:a2bd7d07c7f8	1944	break;
sayzyas	10:a2bd7d07c7f8	1945	}
sayzyas	10:a2bd7d07c7f8	1946	else{
sayzyas	11:ff06edc0219c	1947	DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
sayzyas	10:a2bd7d07c7f8	1948	if( !strcmp( dbuffer, "WinchRtvStop" ) ){
sayzyas	10:a2bd7d07c7f8	1949	flg_stop_operation = true;
sayzyas	10:a2bd7d07c7f8	1950	break;
sayzyas	10:a2bd7d07c7f8	1951	}
sayzyas	10:a2bd7d07c7f8	1952	}
sayzyas	13:2c70c772fe24	1953	// Forward rotation : winch down
sayzyas	10:a2bd7d07c7f8	1954	if( winchDataP->dt_WinchCntPosition < winchDataP->dt_WinchDstPosition ){
sayzyas	14:3a5ae61ab1f4	1955	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
sayzyas	13:2c70c772fe24	1956	if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
sayzyas	14:3a5ae61ab1f4	1957	I2C_cmd[I2C_CP_M1_SPEED] = (setValue.winchCtrl.sv_WDM_lsrto_F >> 1); // very slow speed
sayzyas	13:2c70c772fe24	1958	}
sayzyas	13:2c70c772fe24	1959	else if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
sayzyas	14:3a5ae61ab1f4	1960	I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_lsrto_F; // slow speed
sayzyas	10:a2bd7d07c7f8	1961	}
sayzyas	10:a2bd7d07c7f8	1962	else{
sayzyas	14:3a5ae61ab1f4	1963	I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_F; // normal speed
sayzyas	10:a2bd7d07c7f8	1964	}
sayzyas	10:a2bd7d07c7f8	1965	}
sayzyas	13:2c70c772fe24	1966	// Reverse rotation : winch up
sayzyas	13:2c70c772fe24	1967	else if ( winchDataP->dt_WinchCntPosition > winchDataP->dt_WinchDstPosition ){
sayzyas	14:3a5ae61ab1f4	1968	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 RVS
sayzyas	13:2c70c772fe24	1969	if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_NEAR_DISTANCE ){
sayzyas	14:3a5ae61ab1f4	1970	I2C_cmd[I2C_CP_M1_SPEED] = (setValue.winchCtrl.sv_WDM_lsrto_R >> 1); // very slow speed
sayzyas	13:2c70c772fe24	1971	}
sayzyas	13:2c70c772fe24	1972	else if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
sayzyas	14:3a5ae61ab1f4	1973	I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_lsrto_R; // slow speed
sayzyas	10:a2bd7d07c7f8	1974	}
sayzyas	10:a2bd7d07c7f8	1975	else{
sayzyas	14:3a5ae61ab1f4	1976	I2C_cmd[I2C_CP_M1_SPEED] = setValue.winchCtrl.sv_WDM_hsrto_R; // normal speed
sayzyas	10:a2bd7d07c7f8	1977	}
sayzyas	10:a2bd7d07c7f8	1978	}
sayzyas	10:a2bd7d07c7f8	1979
sayzyas	13:2c70c772fe24	1980	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	1981	// Read winch current position from Resolver.
sayzyas	13:2c70c772fe24	1982
sayzyas	13:2c70c772fe24	1983	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	1984	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	1985	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	1986	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	1987	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	1988	}
sayzyas	13:2c70c772fe24	1989	mtx_wcp.lock();
sayzyas	10:a2bd7d07c7f8	1990	winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas	13:2c70c772fe24	1991	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	1992	winchDataP->operation = 0x00;
sayzyas	13:2c70c772fe24	1993	//i2c.read(I2C_ADDRESS_RESOLVER, I2C_resdata, 2); // Read
sayzyas	10:a2bd7d07c7f8	1994	//res_position = (I2C_resdata[1] << 8) \| I2C_resdata[0];
sayzyas	10:a2bd7d07c7f8	1995	// --------------------------------------
sayzyas	10:a2bd7d07c7f8	1996	// Read motor current
sayzyas	10:a2bd7d07c7f8	1997	// --------------------------------------
sayzyas	13:2c70c772fe24	1998	read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
sayzyas	10:a2bd7d07c7f8	1999	winchDataP->dt_WinchMotor1Current = I2C_read[0]; // Motor current set
sayzyas	10:a2bd7d07c7f8	2000	winchDataP->dt_WinchMotor2Current = I2C_read[1]; // Motor current set
sayzyas	10:a2bd7d07c7f8	2001	winchDataP->operation = I2C_read[2];
sayzyas	14:3a5ae61ab1f4	2002	DEBUG_PRINT_L3("Bd0> 15: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
sayzyas	13:2c70c772fe24	2003	DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
sayzyas	10:a2bd7d07c7f8	2004	led3 = OFF;
sayzyas	13:2c70c772fe24	2005	if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) == 0 ){
sayzyas	13:2c70c772fe24	2006	// if( winchDataP->dt_WinchCntPosition == winchDataP->dt_WinchDstPosition ){
sayzyas	13:2c70c772fe24	2007	DEBUG_PRINT_L3( "Bd0> Current:%d -> Destination:%d\r\n" , winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition);
sayzyas	10:a2bd7d07c7f8	2008	break;
sayzyas	10:a2bd7d07c7f8	2009	}
sayzyas	11:ff06edc0219c	2010	Thread::wait(2); // Time interval for program debugging
sayzyas	13:2c70c772fe24	2011	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	2012	}
sayzyas	13:2c70c772fe24	2013
sayzyas	13:2c70c772fe24	2014
sayzyas	13:2c70c772fe24	2015
sayzyas	13:2c70c772fe24	2016	DEBUG_PRINT_L3( "Bd0> ! Winch Stop\r\n" );
sayzyas	14:3a5ae61ab1f4	2017	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 STOP
sayzyas	14:3a5ae61ab1f4	2018	I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed
sayzyas	14:3a5ae61ab1f4	2019	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 STOP
sayzyas	14:3a5ae61ab1f4	2020	I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed
sayzyas	10:a2bd7d07c7f8	2021
sayzyas	13:2c70c772fe24	2022	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	13:2c70c772fe24	2023	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	2024
sayzyas	13:2c70c772fe24	2025	// Thread::wait(1000); // Time interval for program debugging
sayzyas	10:a2bd7d07c7f8	2026
sayzyas	13:2c70c772fe24	2027	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	2028	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	2029	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	2030	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	2031	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	2032	}
sayzyas	13:2c70c772fe24	2033
sayzyas	13:2c70c772fe24	2034
sayzyas	13:2c70c772fe24	2035	mtx_wcp.lock();
sayzyas	10:a2bd7d07c7f8	2036	winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas	13:2c70c772fe24	2037	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	2038	winchDataP->operation = 0x00;
sayzyas	11:ff06edc0219c	2039	DEBUG_PRINT_L3("Bd0> Check destination is same to setting point or not: %d\r\n", winchCurrentPosition);
sayzyas	13:2c70c772fe24	2040	if( winchDataP->dt_WinchDstPosition == winchCurrentPosition ){
sayzyas	13:2c70c772fe24	2041	cnt++;
sayzyas	11:ff06edc0219c	2042	DEBUG_PRINT_L3("Bd0> Destination is same to setting point, then stop operation\r\n" );
sayzyas	13:2c70c772fe24	2043	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	2044	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	2045	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	2046	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	2047	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	2048	}
sayzyas	13:2c70c772fe24	2049	if ( cnt >= 5 ){
sayzyas	13:2c70c772fe24	2050	break; // When final destination == set point , then break. else adjust position again.
sayzyas	13:2c70c772fe24	2051	}
sayzyas	10:a2bd7d07c7f8	2052	}
sayzyas	13:2c70c772fe24	2053	// Force Stop by Stop button
sayzyas	10:a2bd7d07c7f8	2054	if( flg_stop_operation == true ){
sayzyas	11:ff06edc0219c	2055	DEBUG_PRINT_L3("Bd0> Winch auto operation force stop\r\n" );
sayzyas	10:a2bd7d07c7f8	2056	flg_stop_operation = false;
sayzyas	10:a2bd7d07c7f8	2057	break;
sayzyas	10:a2bd7d07c7f8	2058	}
sayzyas	10:a2bd7d07c7f8	2059	}
sayzyas	13:2c70c772fe24	2060	/*
sayzyas	13:2c70c772fe24	2061	Thread::wait(300); // Time interval for program debugging
sayzyas	13:2c70c772fe24	2062	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	2063	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	2064	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	2065	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	2066	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	2067	}
sayzyas	13:2c70c772fe24	2068	mtx_wcp.lock();
sayzyas	10:a2bd7d07c7f8	2069	winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas	13:2c70c772fe24	2070	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	2071	winchDataP->operation = 0x00;
sayzyas	13:2c70c772fe24	2072	DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
sayzyas	13:2c70c772fe24	2073	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	13:2c70c772fe24	2074
sayzyas	13:2c70c772fe24	2075	Thread::wait(300); // Time interval for program debugging
sayzyas	13:2c70c772fe24	2076	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	2077	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	2078	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	2079	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	2080	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	2081	}
sayzyas	13:2c70c772fe24	2082	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	2083	winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas	13:2c70c772fe24	2084	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	2085	winchDataP->operation = 0x00;
sayzyas	13:2c70c772fe24	2086	DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
sayzyas	13:2c70c772fe24	2087	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	13:2c70c772fe24	2088	*/
sayzyas	13:2c70c772fe24	2089	Thread::wait(300); // Time interval for program debugging
sayzyas	13:2c70c772fe24	2090	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	2091	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	2092	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	2093	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	2094	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	2095	}
sayzyas	13:2c70c772fe24	2096	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	2097	winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas	13:2c70c772fe24	2098	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	2099	winchDataP->operation = 0x77;
sayzyas	13:2c70c772fe24	2100	DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchDataP->dt_WinchCntPosition);
sayzyas	10:a2bd7d07c7f8	2101	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	10:a2bd7d07c7f8	2102
sayzyas	10:a2bd7d07c7f8	2103	swbtn_OpeMutex.lock();
sayzyas	10:a2bd7d07c7f8	2104	swbtn_Opeflg = 0;
sayzyas	10:a2bd7d07c7f8	2105	swbtn_OpeMutex.unlock();
sayzyas	10:a2bd7d07c7f8	2106
sayzyas	10:a2bd7d07c7f8	2107	led3 = OFF;
sayzyas	10:a2bd7d07c7f8	2108	}
sayzyas	10:a2bd7d07c7f8	2109
sayzyas	10:a2bd7d07c7f8	2110	// ----------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2111	// In case of commad received from PC by TCP connection.
sayzyas	10:a2bd7d07c7f8	2112	// In case of hard ware button pushed is by gamepad task
sayzyas	10:a2bd7d07c7f8	2113	// ----------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2114	else if (( mode == WINCH_STEPDOWN_BTN_ON )\|\|( mode == WINCH_U_STEPDOWN_BTN_ON )) {
sayzyas	10:a2bd7d07c7f8	2115
sayzyas	11:ff06edc0219c	2116	if ( mode == WINCH_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPDOWN_BTN_ON ===\r\n" );
sayzyas	11:ff06edc0219c	2117	if ( mode == WINCH_U_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPDOWN_BTN_ON ===\r\n" );
sayzyas	10:a2bd7d07c7f8	2118
sayzyas	10:a2bd7d07c7f8	2119	swbtn_OpeMutex.lock();
sayzyas	10:a2bd7d07c7f8	2120	swbtn_Opeflg = 1;
sayzyas	10:a2bd7d07c7f8	2121	swbtn_OpeMutex.unlock();
sayzyas	10:a2bd7d07c7f8	2122	while( 1 ){
sayzyas	10:a2bd7d07c7f8	2123	led3 = ON;
sayzyas	11:ff06edc0219c	2124	DEBUG_PRINT_L3("Bd0> WINCH_STEPDOWN_BTN_ON\r\n");
sayzyas	10:a2bd7d07c7f8	2125	rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
sayzyas	11:ff06edc0219c	2126	DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	2127	if( rcv_data_cnt < 0 ){
sayzyas	11:ff06edc0219c	2128	DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
sayzyas	10:a2bd7d07c7f8	2129	// tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	10:a2bd7d07c7f8	2130	break;
sayzyas	10:a2bd7d07c7f8	2131	}
sayzyas	10:a2bd7d07c7f8	2132	else{
sayzyas	11:ff06edc0219c	2133	DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
sayzyas	10:a2bd7d07c7f8	2134	if(( !strcmp( dbuffer, "WinchStepDnOf" ))\|\|( !strcmp( dbuffer, "WinchuStepDnOf" )) ){
sayzyas	10:a2bd7d07c7f8	2135	break;
sayzyas	10:a2bd7d07c7f8	2136	}
sayzyas	10:a2bd7d07c7f8	2137	}
sayzyas	10:a2bd7d07c7f8	2138
sayzyas	10:a2bd7d07c7f8	2139	if ( mode == WINCH_U_STEPDOWN_BTN_ON ) man_speed = 50;
sayzyas	10:a2bd7d07c7f8	2140	else man_speed = 100;
sayzyas	10:a2bd7d07c7f8	2141
sayzyas	14:3a5ae61ab1f4	2142	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_FWD; // Motor1 FWD
sayzyas	14:3a5ae61ab1f4	2143	I2C_cmd[I2C_CP_M1_SPEED] = man_speed; // Speed
sayzyas	10:a2bd7d07c7f8	2144
sayzyas	13:2c70c772fe24	2145	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	2146
sayzyas	10:a2bd7d07c7f8	2147	led3 = OFF;
sayzyas	10:a2bd7d07c7f8	2148
sayzyas	13:2c70c772fe24	2149	read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
sayzyas	10:a2bd7d07c7f8	2150	winchDataP->dt_WinchMotor1Current = I2C_read[0];
sayzyas	10:a2bd7d07c7f8	2151	winchDataP->dt_WinchMotor2Current = I2C_read[1];
sayzyas	10:a2bd7d07c7f8	2152	winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
sayzyas	11:ff06edc0219c	2153	DEBUG_PRINT_L3( "Bd0> 16: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
sayzyas	10:a2bd7d07c7f8	2154	if( winchDataP->operation == 0x88 ){
sayzyas	10:a2bd7d07c7f8	2155	winchDataP->dt_WinchMotor1Current = 0xFF;
sayzyas	10:a2bd7d07c7f8	2156	}
sayzyas	13:2c70c772fe24	2157	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	2158
sayzyas	13:2c70c772fe24	2159	DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
sayzyas	13:2c70c772fe24	2160
sayzyas	13:2c70c772fe24	2161	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	2162	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	2163	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	2164	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	2165	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	2166	}
sayzyas	13:2c70c772fe24	2167	mtx_wcp.lock();
sayzyas	10:a2bd7d07c7f8	2168	winchDataP->dt_WinchCntPosition = winchCurrentPosition;
sayzyas	13:2c70c772fe24	2169	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	2170	winchDataP->operation = 0x00;
sayzyas	13:2c70c772fe24	2171	// ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x00 );
sayzyas	10:a2bd7d07c7f8	2172	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	11:ff06edc0219c	2173	DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
sayzyas	11:ff06edc0219c	2174	// Thread::wait(2); // Time interval for program debugging
sayzyas	13:2c70c772fe24	2175	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	2176	}
sayzyas	11:ff06edc0219c	2177	DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
sayzyas	14:3a5ae61ab1f4	2178	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 FWD
sayzyas	14:3a5ae61ab1f4	2179	I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed
sayzyas	14:3a5ae61ab1f4	2180	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 FWD
sayzyas	14:3a5ae61ab1f4	2181	I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed
sayzyas	10:a2bd7d07c7f8	2182
sayzyas	13:2c70c772fe24	2183	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	2184
sayzyas	11:ff06edc0219c	2185	DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
sayzyas	13:2c70c772fe24	2186
sayzyas	13:2c70c772fe24	2187	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	2188	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	2189	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	2190	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	2191	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	2192	}
sayzyas	13:2c70c772fe24	2193	mtx_wcp.lock();
sayzyas	10:a2bd7d07c7f8	2194	winchDataP->dt_WinchCntPosition = winchCurrentPosition;
sayzyas	13:2c70c772fe24	2195	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	2196	winchDataP->operation = 0x77;
sayzyas	13:2c70c772fe24	2197	//ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x77 );
sayzyas	10:a2bd7d07c7f8	2198	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	13:2c70c772fe24	2199	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	2200
sayzyas	10:a2bd7d07c7f8	2201	swbtn_OpeMutex.lock();
sayzyas	10:a2bd7d07c7f8	2202	swbtn_Opeflg = 0;
sayzyas	10:a2bd7d07c7f8	2203	swbtn_OpeMutex.unlock();
sayzyas	10:a2bd7d07c7f8	2204	led3 = OFF;
sayzyas	10:a2bd7d07c7f8	2205	}
sayzyas	10:a2bd7d07c7f8	2206	// ----------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2207	// In case of commad received from PC by TCP connection.
sayzyas	10:a2bd7d07c7f8	2208	// In case of hard ware button pushed is by gamepad task
sayzyas	10:a2bd7d07c7f8	2209	// ----------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2210	else if (( mode == WINCH_STEPUP_BTN_ON )\|\|( mode == WINCH_U_STEPUP_BTN_ON )) {
sayzyas	10:a2bd7d07c7f8	2211
sayzyas	11:ff06edc0219c	2212	if ( mode == WINCH_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPUP_BTN_ON ===\r\n" );
sayzyas	11:ff06edc0219c	2213	if ( mode == WINCH_U_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPUP_BTN_ON ===\r\n" );
sayzyas	10:a2bd7d07c7f8	2214
sayzyas	10:a2bd7d07c7f8	2215	swbtn_OpeMutex.lock();
sayzyas	10:a2bd7d07c7f8	2216	swbtn_Opeflg = 1;
sayzyas	10:a2bd7d07c7f8	2217	swbtn_OpeMutex.unlock();
sayzyas	10:a2bd7d07c7f8	2218	while( 1 ){
sayzyas	10:a2bd7d07c7f8	2219	led3 = ON;
sayzyas	11:ff06edc0219c	2220	DEBUG_PRINT_L3("Bd0> WINCH_STEPUP_BTN_ON\r\n");
sayzyas	10:a2bd7d07c7f8	2221	rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
sayzyas	11:ff06edc0219c	2222	DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	2223	if( rcv_data_cnt < 0 ){
sayzyas	11:ff06edc0219c	2224	DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
sayzyas	10:a2bd7d07c7f8	2225	// tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	10:a2bd7d07c7f8	2226	break;
sayzyas	10:a2bd7d07c7f8	2227	}
sayzyas	10:a2bd7d07c7f8	2228	else{
sayzyas	11:ff06edc0219c	2229	DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
sayzyas	10:a2bd7d07c7f8	2230	if(
sayzyas	11:ff06edc0219c	2231	( !strcmp( dbuffer, "WinchStepUpOf" ))\|\|(!strcmp( dbuffer, "WinchuStepUpOf" )) ){
sayzyas	10:a2bd7d07c7f8	2232	break;
sayzyas	10:a2bd7d07c7f8	2233	}
sayzyas	10:a2bd7d07c7f8	2234	}
sayzyas	10:a2bd7d07c7f8	2235
sayzyas	10:a2bd7d07c7f8	2236	if ( mode == WINCH_U_STEPUP_BTN_ON ) man_speed = 50;
sayzyas	10:a2bd7d07c7f8	2237	else man_speed = 100;
sayzyas	10:a2bd7d07c7f8	2238
sayzyas	14:3a5ae61ab1f4	2239	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_RVS; // Motor1 FWD
sayzyas	14:3a5ae61ab1f4	2240	I2C_cmd[I2C_CP_M1_SPEED] = man_speed; // Speed
sayzyas	10:a2bd7d07c7f8	2241
sayzyas	13:2c70c772fe24	2242	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	2243
sayzyas	10:a2bd7d07c7f8	2244	led3 = OFF;
sayzyas	10:a2bd7d07c7f8	2245
sayzyas	13:2c70c772fe24	2246	read_motorCurrent( I2C_ADDRESS_WINCH, I2C_readcmd, I2C_read, 3 );
sayzyas	10:a2bd7d07c7f8	2247	winchDataP->dt_WinchMotor1Current = I2C_read[0];
sayzyas	10:a2bd7d07c7f8	2248	winchDataP->dt_WinchMotor2Current = I2C_read[1];
sayzyas	10:a2bd7d07c7f8	2249	winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
sayzyas	11:ff06edc0219c	2250	DEBUG_PRINT_L3( "Bd0> 17: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
sayzyas	10:a2bd7d07c7f8	2251	if( winchDataP->operation == 0x88 ){
sayzyas	10:a2bd7d07c7f8	2252	winchDataP->dt_WinchMotor1Current = 0xFF;
sayzyas	10:a2bd7d07c7f8	2253	}
sayzyas	13:2c70c772fe24	2254	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	2255
sayzyas	13:2c70c772fe24	2256	DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", I2C_ADDRESS_WINCH, I2C_read[0]);
sayzyas	13:2c70c772fe24	2257
sayzyas	13:2c70c772fe24	2258	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	2259	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	2260	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	2261	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	2262	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	2263	}
sayzyas	13:2c70c772fe24	2264	mtx_wcp.lock();
sayzyas	10:a2bd7d07c7f8	2265	winchDataP->dt_WinchCntPosition = winchCurrentPosition;
sayzyas	13:2c70c772fe24	2266	mtx_wcp.unlock();
sayzyas	10:a2bd7d07c7f8	2267	winchDataP->operation = 0x00;
sayzyas	13:2c70c772fe24	2268	// ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x00 );
sayzyas	10:a2bd7d07c7f8	2269	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	11:ff06edc0219c	2270	DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
sayzyas	11:ff06edc0219c	2271	// Thread::wait(2); // Time interval for program debugging
sayzyas	13:2c70c772fe24	2272	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	2273	}
sayzyas	11:ff06edc0219c	2274	DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
sayzyas	14:3a5ae61ab1f4	2275	I2C_cmd[I2C_CP_M1_DIR] = MOTOR_STP; // Motor1 FWD
sayzyas	14:3a5ae61ab1f4	2276	I2C_cmd[I2C_CP_M1_SPEED] = 0; // Speed
sayzyas	14:3a5ae61ab1f4	2277	I2C_cmd[I2C_CP_M2_DIR] = MOTOR_STP; // Motor2 FWD
sayzyas	14:3a5ae61ab1f4	2278	I2C_cmd[I2C_CP_M2_SPEED] = 0; // Speed
sayzyas	10:a2bd7d07c7f8	2279
sayzyas	13:2c70c772fe24	2280	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	11:ff06edc0219c	2281
sayzyas	11:ff06edc0219c	2282	DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
sayzyas	13:2c70c772fe24	2283	if( flg_ButtonOn == false ){
sayzyas	13:2c70c772fe24	2284	winchTempPosition = ReadWinchCurrentPosition(I2C_ADDRESS_RESOLVER);
sayzyas	13:2c70c772fe24	2285	if( winchTempPosition != -1 ){
sayzyas	13:2c70c772fe24	2286	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	2287	winchCurrentPosition = winchTempPosition;
sayzyas	13:2c70c772fe24	2288	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	2289	}
sayzyas	13:2c70c772fe24	2290	mtx_wcp.lock();
sayzyas	13:2c70c772fe24	2291	winchDataP->dt_WinchCntPosition = winchCurrentPosition;
sayzyas	13:2c70c772fe24	2292	mtx_wcp.unlock();
sayzyas	13:2c70c772fe24	2293	winchDataP->operation = 0x77;
sayzyas	13:2c70c772fe24	2294	// ReadCurrentPositon(I2C_ADDRESS_RESOLVER, winchDataP, winchData_s, 0x77 );
sayzyas	13:2c70c772fe24	2295	tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas	13:2c70c772fe24	2296	}
sayzyas	13:2c70c772fe24	2297	i2c.write(I2C_ADDRESS_WINCH, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	10:a2bd7d07c7f8	2298
sayzyas	10:a2bd7d07c7f8	2299	swbtn_OpeMutex.lock();
sayzyas	10:a2bd7d07c7f8	2300	swbtn_Opeflg = 0;
sayzyas	10:a2bd7d07c7f8	2301	swbtn_OpeMutex.unlock();
sayzyas	10:a2bd7d07c7f8	2302
sayzyas	10:a2bd7d07c7f8	2303	led3 = OFF;
sayzyas	10:a2bd7d07c7f8	2304	}
sayzyas	10:a2bd7d07c7f8	2305
sayzyas	10:a2bd7d07c7f8	2306	else {
sayzyas	11:ff06edc0219c	2307	// DEBUG_PRINT_L3("STEPSTEPSTEPSTEPSTEP\r\n");
sayzyas	10:a2bd7d07c7f8	2308	}
sayzyas	10:a2bd7d07c7f8	2309	// }
sayzyas	10:a2bd7d07c7f8	2310	}
sayzyas	10:a2bd7d07c7f8	2311
sayzyas	10:a2bd7d07c7f8	2312	// **********************************************************************
sayzyas	10:a2bd7d07c7f8	2313	//
sayzyas	10:a2bd7d07c7f8	2314	// Main Function of this program
sayzyas	10:a2bd7d07c7f8	2315	//
sayzyas	10:a2bd7d07c7f8	2316	// **********************************************************************
sayzyas	10:a2bd7d07c7f8	2317	int main()
sayzyas	10:a2bd7d07c7f8	2318	{
sayzyas	14:3a5ae61ab1f4	2319	Mutex file_access_mutex;
sayzyas	14:3a5ae61ab1f4	2320	setValue_t new_setValue; // Setting Data
sayzyas	14:3a5ae61ab1f4	2321	winchData_t winchData;
sayzyas	10:a2bd7d07c7f8	2322
sayzyas	10:a2bd7d07c7f8	2323	char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
sayzyas	13:2c70c772fe24	2324	// char I2C_res[NumberOfI2CCommand+1] = "\0";
sayzyas	10:a2bd7d07c7f8	2325
sayzyas	14:3a5ae61ab1f4	2326	char ip_address[20];
sayzyas	14:3a5ae61ab1f4	2327	char subnet_mask[20];
sayzyas	14:3a5ae61ab1f4	2328	char gateway[20];
sayzyas	13:2c70c772fe24	2329
sayzyas	14:3a5ae61ab1f4	2330	// char* ip_address;
sayzyas	14:3a5ae61ab1f4	2331	// char* subnet_mask;
sayzyas	14:3a5ae61ab1f4	2332	// char* gateway;
sayzyas	10:a2bd7d07c7f8	2333
sayzyas	10:a2bd7d07c7f8	2334	int ret;
sayzyas	10:a2bd7d07c7f8	2335	int try_cnt;
sayzyas	10:a2bd7d07c7f8	2336	int rcv_data_cnt;
sayzyas	10:a2bd7d07c7f8	2337
sayzyas	10:a2bd7d07c7f8	2338	bool flg_ethernet = false;
sayzyas	10:a2bd7d07c7f8	2339
sayzyas	10:a2bd7d07c7f8	2340	char ttt[20];
sayzyas	13:2c70c772fe24	2341
sayzyas	10:a2bd7d07c7f8	2342	// Set UART(USB) baudrate
sayzyas	10:a2bd7d07c7f8	2343	pc.baud(115200);
sayzyas	10:a2bd7d07c7f8	2344
sayzyas	14:3a5ae61ab1f4	2345	cf_led_demo( &led1, &led2, &led3, &led4, 10, 15 );
sayzyas	10:a2bd7d07c7f8	2346
sayzyas	10:a2bd7d07c7f8	2347	DEBUG_PRINT_L0("\r\n");
sayzyas	11:ff06edc0219c	2348	DEBUG_PRINT_L0("Bd0> +--------------------------------------------------------------\r\n");
sayzyas	11:ff06edc0219c	2349	DEBUG_PRINT_L0("Bd0> \| Project: B2 Crawler Explorer for 1F-1 PCV internal inspection\r\n");
sayzyas	11:ff06edc0219c	2350	DEBUG_PRINT_L0("Bd0> \|---------\r\n");
sayzyas	11:ff06edc0219c	2351	DEBUG_PRINT_L0("Bd0> \| This is: Main Control Program of Main Controller\r\n");
sayzyas	10:a2bd7d07c7f8	2352	DEBUG_PRINT_L0("Bd0> \| Target MCU: mbed LPC1768\r\n");
sayzyas	10:a2bd7d07c7f8	2353	DEBUG_PRINT_L0("Bd0> \| Letest update: %s\r\n", LatestUpDate);
sayzyas	10:a2bd7d07c7f8	2354	DEBUG_PRINT_L0("Bd0> \| Program Revision: %s\r\n", ProgramRevision);
sayzyas	10:a2bd7d07c7f8	2355	DEBUG_PRINT_L0("Bd0> \| Author: %s\r\n", Author);
sayzyas	10:a2bd7d07c7f8	2356	DEBUG_PRINT_L0("Bd0> \| Copyright(C) 2015 %s Allright Reserved\r\n", Company);
sayzyas	11:ff06edc0219c	2357	DEBUG_PRINT_L0("Bd0> ---------------------------------------------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	2358	sprintf( ttt, "%s", ProgramRevision );
sayzyas	14:3a5ae61ab1f4	2359
sayzyas	11:ff06edc0219c	2360	DEBUG_PRINT_L0("Bd0> Start ststem initializing ...\r\n");
sayzyas	14:3a5ae61ab1f4	2361	DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas	14:3a5ae61ab1f4	2362	DEBUG_PRINT_L0("Bd0> 1. Initalizing Ethernet ...\r\n");
sayzyas	14:3a5ae61ab1f4	2363	DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas	10:a2bd7d07c7f8	2364
sayzyas	14:3a5ae61ab1f4	2365	read_NetSetting_lfs( ip_address, subnet_mask, gateway );
sayzyas	13:2c70c772fe24	2366
sayzyas	14:3a5ae61ab1f4	2367	// ip_address = "192.168.3.24";
sayzyas	14:3a5ae61ab1f4	2368	// subnet_mask = "255.255.255.0";
sayzyas	14:3a5ae61ab1f4	2369	// gateway = "192.168.3.1";
sayzyas	10:a2bd7d07c7f8	2370
sayzyas	11:ff06edc0219c	2371	DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
sayzyas	11:ff06edc0219c	2372	DEBUG_PRINT_L0("Bd0> ip address : %s\r\n", ip_address);
sayzyas	11:ff06edc0219c	2373	DEBUG_PRINT_L0("Bd0> subnet mask : %s\r\n", subnet_mask);
sayzyas	11:ff06edc0219c	2374	DEBUG_PRINT_L0("Bd0> default gateway: %s\r\n", gateway);
sayzyas	11:ff06edc0219c	2375	DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	2376
sayzyas	10:a2bd7d07c7f8	2377	#ifdef __ETERNET_DHCP__
sayzyas	10:a2bd7d07c7f8	2378	ret = eth.init(); // Use DHCP
sayzyas	10:a2bd7d07c7f8	2379	#else // __ETERNET_DHCP__
sayzyas	10:a2bd7d07c7f8	2380	ret = eth.init(
sayzyas	10:a2bd7d07c7f8	2381	ip_address, // const char* ip,
sayzyas	10:a2bd7d07c7f8	2382	subnet_mask, // const char* mask,
sayzyas	10:a2bd7d07c7f8	2383	gateway // const char* gateway
sayzyas	10:a2bd7d07c7f8	2384	);
sayzyas	10:a2bd7d07c7f8	2385	#endif // __ETERNET_DHCP__
sayzyas	10:a2bd7d07c7f8	2386	if( ret == 0 ){
sayzyas	11:ff06edc0219c	2387	DEBUG_PRINT_L0("Bd0> Eternet init ... OK\r\n");
sayzyas	10:a2bd7d07c7f8	2388	ret = eth.connect();
sayzyas	10:a2bd7d07c7f8	2389	if( ret == 0 ){
sayzyas	10:a2bd7d07c7f8	2390	cf_led_onoff( &led1,&led2,&led3,&led4, false, false, false, true );
sayzyas	11:ff06edc0219c	2391	DEBUG_PRINT_L0("Bd0> Eternat connect ... OK\r\n");
sayzyas	11:ff06edc0219c	2392	DEBUG_PRINT_L0("Bd0> [ IP Address : %s ]\r\n", eth.getIPAddress());
sayzyas	10:a2bd7d07c7f8	2393	udp_server.bind(UDP_SERVER_PORT);
sayzyas	10:a2bd7d07c7f8	2394	tcp_server.bind(TCP_SERVER_PORT);
sayzyas	10:a2bd7d07c7f8	2395	tcp_server.listen();
sayzyas	10:a2bd7d07c7f8	2396	flg_ethernet = true;
sayzyas	13:2c70c772fe24	2397	led4 = ON; // Ethernet OK
sayzyas	10:a2bd7d07c7f8	2398	}
sayzyas	10:a2bd7d07c7f8	2399	else{
sayzyas	10:a2bd7d07c7f8	2400	cf_led_error( &led1,&led2,&led3,&led4 );
sayzyas	11:ff06edc0219c	2401	DEBUG_PRINT_L0("Bd0> *ERROR* Eternat connect Fali\r\n");
sayzyas	10:a2bd7d07c7f8	2402	}
sayzyas	10:a2bd7d07c7f8	2403	}
sayzyas	10:a2bd7d07c7f8	2404	else{
sayzyas	11:ff06edc0219c	2405	DEBUG_PRINT_L0("Bd0> * ERROR* Eternat init Fail\r\n");
sayzyas	10:a2bd7d07c7f8	2406	}
sayzyas	10:a2bd7d07c7f8	2407
sayzyas	13:2c70c772fe24	2408	Thread::wait(50);
sayzyas	10:a2bd7d07c7f8	2409
sayzyas	10:a2bd7d07c7f8	2410	//---------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2411	// Read CrExp setting value from Local File System
sayzyas	10:a2bd7d07c7f8	2412	// setting file "SET.DAT".
sayzyas	10:a2bd7d07c7f8	2413	// When error occured, LED1 will be blinking shortly.
sayzyas	10:a2bd7d07c7f8	2414	//---------------------------------------------------
sayzyas	14:3a5ae61ab1f4	2415	DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas	14:3a5ae61ab1f4	2416	DEBUG_PRINT_L0("Bd0> 2. Read setting value from LFS\r\n");
sayzyas	14:3a5ae61ab1f4	2417	DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas	14:3a5ae61ab1f4	2418
sayzyas	14:3a5ae61ab1f4	2419	#ifdef __CREATE_SETTING_FILE__
sayzyas	14:3a5ae61ab1f4	2420	write_LFS(&setValue); // Create and set setting file.
sayzyas	14:3a5ae61ab1f4	2421	#endif // __CREATE_SETTING_FILE__
sayzyas	14:3a5ae61ab1f4	2422
sayzyas	14:3a5ae61ab1f4	2423	// --------------------------------------------------------------------
sayzyas	14:3a5ae61ab1f4	2424	// Read setting from local file system and set to internal structure
sayzyas	14:3a5ae61ab1f4	2425	// --------------------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2426	try_cnt = LFS_READ_COUNT;
sayzyas	10:a2bd7d07c7f8	2427	while( 1 ){
sayzyas	14:3a5ae61ab1f4	2428	if( read_LFS(&setValue) == true ) break;
sayzyas	10:a2bd7d07c7f8	2429	else try_cnt -= 1;
sayzyas	10:a2bd7d07c7f8	2430	if( try_cnt == 0 ){
sayzyas	11:ff06edc0219c	2431	DEBUG_PRINT_L0("Bd0> *ERROR* LFS read error\r\n");
sayzyas	10:a2bd7d07c7f8	2432	while(1){
sayzyas	10:a2bd7d07c7f8	2433	led1 = !led1;
sayzyas	10:a2bd7d07c7f8	2434	Thread::wait(30);
sayzyas	10:a2bd7d07c7f8	2435	}
sayzyas	10:a2bd7d07c7f8	2436	}
sayzyas	10:a2bd7d07c7f8	2437	}
sayzyas	14:3a5ae61ab1f4	2438
sayzyas	11:ff06edc0219c	2439	DEBUG_PRINT_L0("Bd0> LFS read OK\r\n");
sayzyas	13:2c70c772fe24	2440	led3 = ON; // Setting Data Read OK
sayzyas	10:a2bd7d07c7f8	2441
sayzyas	14:3a5ae61ab1f4	2442	#ifdef __TARGET_BOARD_CHECK__
sayzyas	10:a2bd7d07c7f8	2443	//---------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2444	// Checking Targer LCXpresso824-MAX board here.
sayzyas	10:a2bd7d07c7f8	2445	// Send Hello Packet and waiting reply message from
sayzyas	10:a2bd7d07c7f8	2446	// target.
sayzyas	10:a2bd7d07c7f8	2447	// When error occured, LED1 will blinking slowly.
sayzyas	10:a2bd7d07c7f8	2448	//---------------------------------------------------
sayzyas	14:3a5ae61ab1f4	2449	DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas	14:3a5ae61ab1f4	2450	DEBUG_PRINT_L0("Vd0> 3. Check the target controler\r\n");
sayzyas	14:3a5ae61ab1f4	2451	DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas	11:ff06edc0219c	2452
sayzyas	10:a2bd7d07c7f8	2453	try_cnt = TARGET_CHECK_COUNT;
sayzyas	10:a2bd7d07c7f8	2454	while(1){
sayzyas	10:a2bd7d07c7f8	2455	// Check each target motor control 824 board here
sayzyas	13:2c70c772fe24	2456	i2c.read(I2C_ADDRESS_WINCH, I2C_res, NumberOfI2CCommand);
sayzyas	14:3a5ae61ab1f4	2457	if( I2C_res[4] == 'S' ){
sayzyas	14:3a5ae61ab1f4	2458	DEBUG_PRINT_L0("Bd0> Try count : %02d/%02d\r\n", TARGET_CHECK_COUNT - try_cnt, TARGET_CHECK_COUNT );
sayzyas	14:3a5ae61ab1f4	2459	DEBUG_PRINT_L0("Bd0> Return from (0x%02x) : '%c'\r\n", I2C_ADDRESS_WINCH, I2C_res[4]);
sayzyas	10:a2bd7d07c7f8	2460	break;
sayzyas	10:a2bd7d07c7f8	2461	}
sayzyas	10:a2bd7d07c7f8	2462	else try_cnt -= 1;
sayzyas	10:a2bd7d07c7f8	2463	if( try_cnt == 0 ){
sayzyas	14:3a5ae61ab1f4	2464	DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_WINCH);
sayzyas	10:a2bd7d07c7f8	2465	led1 = OFF;
sayzyas	10:a2bd7d07c7f8	2466	while(1){
sayzyas	10:a2bd7d07c7f8	2467	led1 = !led1; // ON
sayzyas	10:a2bd7d07c7f8	2468	Thread::wait(80);
sayzyas	10:a2bd7d07c7f8	2469	}
sayzyas	10:a2bd7d07c7f8	2470	}
sayzyas	10:a2bd7d07c7f8	2471	}
sayzyas	14:3a5ae61ab1f4	2472
sayzyas	14:3a5ae61ab1f4	2473	try_cnt = TARGET_CHECK_COUNT;
sayzyas	14:3a5ae61ab1f4	2474	while(1){
sayzyas	14:3a5ae61ab1f4	2475	// Check each target motor control 824 board here
sayzyas	14:3a5ae61ab1f4	2476	i2c.read(I2C_ADDRESS_TRANSFORM, I2C_res, NumberOfI2CCommand);
sayzyas	14:3a5ae61ab1f4	2477	if( I2C_res[4] == 'S' ){
sayzyas	14:3a5ae61ab1f4	2478	DEBUG_PRINT_L0("Bd0> Try count: %02d/%02d\r\n", TARGET_CHECK_COUNT - try_cnt, TARGET_CHECK_COUNT );
sayzyas	14:3a5ae61ab1f4	2479	DEBUG_PRINT_L0("Bd0> Return from (0x%02x): '%c'\r\n", I2C_ADDRESS_TRANSFORM, I2C_res[4]);
sayzyas	14:3a5ae61ab1f4	2480	break;
sayzyas	14:3a5ae61ab1f4	2481	}
sayzyas	14:3a5ae61ab1f4	2482	else try_cnt -= 1;
sayzyas	14:3a5ae61ab1f4	2483	if( try_cnt == 0 ){
sayzyas	14:3a5ae61ab1f4	2484	DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_TRANSFORM);
sayzyas	14:3a5ae61ab1f4	2485	led1 = OFF;
sayzyas	14:3a5ae61ab1f4	2486	while(1){
sayzyas	14:3a5ae61ab1f4	2487	led2 = !led2; // ON
sayzyas	14:3a5ae61ab1f4	2488	Thread::wait(80);
sayzyas	14:3a5ae61ab1f4	2489	}
sayzyas	14:3a5ae61ab1f4	2490	}
sayzyas	14:3a5ae61ab1f4	2491	}
sayzyas	14:3a5ae61ab1f4	2492	try_cnt = TARGET_CHECK_COUNT;
sayzyas	14:3a5ae61ab1f4	2493	while(1){
sayzyas	14:3a5ae61ab1f4	2494	// Check each target motor control 824 board here
sayzyas	14:3a5ae61ab1f4	2495	i2c.read(I2C_ADDRESS_CRAWLER, I2C_res, NumberOfI2CCommand);
sayzyas	14:3a5ae61ab1f4	2496	if( I2C_res[4] == 'S' ){
sayzyas	14:3a5ae61ab1f4	2497	DEBUG_PRINT_L0("Bd0> Try count : %02d/%02d\r\n", TARGET_CHECK_COUNT - try_cnt, TARGET_CHECK_COUNT );
sayzyas	14:3a5ae61ab1f4	2498	DEBUG_PRINT_L0("Bd0> Return from (0x%02x) : '%c'\r\n", I2C_ADDRESS_CRAWLER, I2C_res[4]);
sayzyas	14:3a5ae61ab1f4	2499	break;
sayzyas	14:3a5ae61ab1f4	2500	}
sayzyas	14:3a5ae61ab1f4	2501	else try_cnt -= 1;
sayzyas	14:3a5ae61ab1f4	2502	if( try_cnt == 0 ){
sayzyas	14:3a5ae61ab1f4	2503	DEBUG_PRINT_L0("Bd0> ##ERROR : Target(0x02)##\r\n",I2C_ADDRESS_CRAWLER);
sayzyas	14:3a5ae61ab1f4	2504	led1 = OFF;
sayzyas	14:3a5ae61ab1f4	2505	while(1){
sayzyas	14:3a5ae61ab1f4	2506	led3 = !led3; // ON
sayzyas	14:3a5ae61ab1f4	2507	Thread::wait(80);
sayzyas	14:3a5ae61ab1f4	2508	}
sayzyas	14:3a5ae61ab1f4	2509	}
sayzyas	14:3a5ae61ab1f4	2510	}
sayzyas	14:3a5ae61ab1f4	2511	DEBUG_PRINT_L0("Bd0> -------------------\r\n");
sayzyas	11:ff06edc0219c	2512	DEBUG_PRINT_L0("Bd0> Target system found\r\n");
sayzyas	14:3a5ae61ab1f4	2513	DEBUG_PRINT_L0("Bd0> -------------------\r\n");
sayzyas	14:3a5ae61ab1f4	2514	#endif // __TARGET_BOARD_CHECK__
sayzyas	14:3a5ae61ab1f4	2515
sayzyas	14:3a5ae61ab1f4	2516
sayzyas	13:2c70c772fe24	2517	led2 = ON; // Check target OK
sayzyas	13:2c70c772fe24	2518
sayzyas	13:2c70c772fe24	2519	/* Set basic function default setting */
sayzyas	13:2c70c772fe24	2520	baseOperation.sv_JS_OpeMode = 0;
sayzyas	13:2c70c772fe24	2521	baseOperation.sv_JS_OpeMode = 0;
sayzyas	13:2c70c772fe24	2522	baseOperation.sv_WinchValid = 0;
sayzyas	11:ff06edc0219c	2523
sayzyas	14:3a5ae61ab1f4	2524
sayzyas	10:a2bd7d07c7f8	2525	/*
sayzyas	10:a2bd7d07c7f8	2526	**************************************************
sayzyas	10:a2bd7d07c7f8	2527	Send Calculation Data to Resolver Controller
sayzyas	10:a2bd7d07c7f8	2528	**************************************************
sayzyas	10:a2bd7d07c7f8	2529	*/
sayzyas	14:3a5ae61ab1f4	2530	DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas	14:3a5ae61ab1f4	2531	DEBUG_PRINT_L0("Bd0> 4. Send the Calculation base data to Resolver Controller");
sayzyas	14:3a5ae61ab1f4	2532	DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas	14:3a5ae61ab1f4	2533	I2C_cmd[I2C_CP_WDRAM_DIA_UPPER] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100>>8)&0xFF); // Dram diameter upper
sayzyas	14:3a5ae61ab1f4	2534	I2C_cmd[I2C_CP_WDRAM_DIA_LOWER] = (uint8_t)((setValue.winchCtrl.sv_WRS_DramDmrx100)&0xFF); // Dram diameter lower
sayzyas	14:3a5ae61ab1f4	2535	I2C_cmd[I2C_CP_CCABLE_DIA_UPPER] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100>>8)&0xFF); // Cable diameter upper
sayzyas	14:3a5ae61ab1f4	2536	I2C_cmd[I2C_CP_CCABLE_DIA_LOWER] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100)&0xFF); // Cable diameter lower
sayzyas	14:3a5ae61ab1f4	2537	I2C_cmd[I2C_CP_RESOLVER_RESO] = setValue.winchCtrl.sv_WRS_RResolution; // Resolver resolution
sayzyas	14:3a5ae61ab1f4	2538
sayzyas	14:3a5ae61ab1f4	2539	for( int j = 0; j < NumberOfI2CCommand; j++)
sayzyas	14:3a5ae61ab1f4	2540	DEBUG_PRINT_L0("%02x ", I2C_cmd[j]);
sayzyas	14:3a5ae61ab1f4	2541	DEBUG_PRINT_L0( "\r\n" );
sayzyas	14:3a5ae61ab1f4	2542
sayzyas	13:2c70c772fe24	2543	i2c.write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	11:ff06edc0219c	2544	DEBUG_PRINT_L0(" ... done\r\n");
sayzyas	10:a2bd7d07c7f8	2545
sayzyas	14:3a5ae61ab1f4	2546	// Thread (
sayzyas	14:3a5ae61ab1f4	2547	// void(task)(void const argument),
sayzyas	14:3a5ae61ab1f4	2548	// void *argument=NULL,
sayzyas	14:3a5ae61ab1f4	2549	// osPriority priority=osPriorityNormal,
sayzyas	14:3a5ae61ab1f4	2550	// uint32_t stack_size=DEFAULT_STACK_SIZE,
sayzyas	14:3a5ae61ab1f4	2551	// unsigned char *stack_pointer=NULL
sayzyas	14:3a5ae61ab1f4	2552	//)
sayzyas	14:3a5ae61ab1f4	2553
sayzyas	14:3a5ae61ab1f4	2554	DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas	14:3a5ae61ab1f4	2555	DEBUG_PRINT_L0("Bd0> 5. Start the task\r\n");
sayzyas	10:a2bd7d07c7f8	2556	/* Max thread count is (may be ..) 2, How can I increase this , I don't know ?? */
sayzyas	10:a2bd7d07c7f8	2557	DEBUG_PRINT_L0("Bd0> Start host interface task ... ");
sayzyas	14:3a5ae61ab1f4	2558	// Thread thread_hif(clientPC_interface_task, NULL, osPriorityHigh, 128*4);
sayzyas	14:3a5ae61ab1f4	2559	Thread thread_hif(clientPC_interface_task, NULL, osPriorityNormal, 128*4);
sayzyas	11:ff06edc0219c	2560	DEBUG_PRINT_L0("\r\nBd0> Start host gamepad task ... ");
sayzyas	14:3a5ae61ab1f4	2561	Thread thread_gpd(gamepad_task, NULL, osPriorityNormal, 128*4);
sayzyas	14:3a5ae61ab1f4	2562	DEBUG_PRINT_L0("\r\n");
sayzyas	14:3a5ae61ab1f4	2563	DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas	10:a2bd7d07c7f8	2564
sayzyas	14:3a5ae61ab1f4	2565	DEBUG_PRINT_L0( "Bd0> ----------------------------------\r\n");
sayzyas	14:3a5ae61ab1f4	2566	DEBUG_PRINT_L0( "Bd0> >>>> Initializing completed ! <<<<\r\n");
sayzyas	14:3a5ae61ab1f4	2567	DEBUG_PRINT_L0( "Bd0> ----------------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	2568
sayzyas	13:2c70c772fe24	2569	led4 = OFF;
sayzyas	13:2c70c772fe24	2570	led3 = OFF;
sayzyas	13:2c70c772fe24	2571	led2 = OFF;
sayzyas	10:a2bd7d07c7f8	2572	led1 = ON; // Initializing is OK then Power Indicator LED ON
sayzyas	10:a2bd7d07c7f8	2573
sayzyas	10:a2bd7d07c7f8	2574	I2C_cmd[4] = 0x00;
sayzyas	10:a2bd7d07c7f8	2575	I2C_cmd[5] = 0x01;
sayzyas	13:2c70c772fe24	2576	i2c.write(I2C_ADDRESS_HANDY, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas	14:3a5ae61ab1f4	2577
sayzyas	10:a2bd7d07c7f8	2578	while( true ) {
sayzyas	10:a2bd7d07c7f8	2579	Thread::wait(10);
sayzyas	10:a2bd7d07c7f8	2580	// -----------------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2581	// Communicate with client PC program.
sayzyas	10:a2bd7d07c7f8	2582	// TCP connection:
sayzyas	10:a2bd7d07c7f8	2583	// -----------------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2584	if( flg_ethernet == true ) // in case of Ethernet OK
sayzyas	10:a2bd7d07c7f8	2585	{
sayzyas	10:a2bd7d07c7f8	2586	tcp_server.accept(tcp_client);
sayzyas	10:a2bd7d07c7f8	2587	tcp_client.set_blocking(false, 3500); // Timeout after (300) msec
sayzyas	11:ff06edc0219c	2588	DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
sayzyas	13:2c70c772fe24	2589	DEBUG_PRINT_L3("Bd0> TCP Connection from: %s\r\n", tcp_client.get_address());
sayzyas	11:ff06edc0219c	2590	DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
sayzyas	10:a2bd7d07c7f8	2591
sayzyas	10:a2bd7d07c7f8	2592	while(true){
sayzyas	10:a2bd7d07c7f8	2593
sayzyas	13:2c70c772fe24	2594	// --------------------------------------------------------------
sayzyas	13:2c70c772fe24	2595	// Following instructions are blocking when no ethernat access
sayzyas	13:2c70c772fe24	2596	// --------------------------------------------------------------
sayzyas	10:a2bd7d07c7f8	2597	rcv_data_cnt = tcp_client.receive(dbuffer, sizeof(dbuffer));
sayzyas	11:ff06edc0219c	2598	DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas	10:a2bd7d07c7f8	2599	if( rcv_data_cnt < 0 ){
sayzyas	11:ff06edc0219c	2600	// DEBUG_PRINT("## TCP Receive packet fail ##\r\n");
sayzyas	10:a2bd7d07c7f8	2601	break;
sayzyas	10:a2bd7d07c7f8	2602	}
sayzyas	10:a2bd7d07c7f8	2603	else{
sayzyas	10:a2bd7d07c7f8	2604	if( !strcmp( dbuffer, "WinchPositionClear" ) ){
sayzyas	10:a2bd7d07c7f8	2605	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2606	WINCH_POSITION_CLEAR,
sayzyas	10:a2bd7d07c7f8	2607	dbuffer,
sayzyas	10:a2bd7d07c7f8	2608	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2609	&winchData,
sayzyas	10:a2bd7d07c7f8	2610	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2611	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2612	);
sayzyas	10:a2bd7d07c7f8	2613	}
sayzyas	10:a2bd7d07c7f8	2614
sayzyas	10:a2bd7d07c7f8	2615	else if( !strcmp( dbuffer, "WinchRtvStart" ) ){
sayzyas	10:a2bd7d07c7f8	2616	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2617	WINCH_MMODE_RELATIVE,
sayzyas	10:a2bd7d07c7f8	2618	dbuffer,
sayzyas	10:a2bd7d07c7f8	2619	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2620	&winchData,
sayzyas	10:a2bd7d07c7f8	2621	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2622	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2623	);
sayzyas	10:a2bd7d07c7f8	2624	}
sayzyas	10:a2bd7d07c7f8	2625	else if( !strcmp( dbuffer, "WinchAbsStart" ) ){
sayzyas	10:a2bd7d07c7f8	2626	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2627	WINCH_MMODE_ABSOLUTE,
sayzyas	10:a2bd7d07c7f8	2628	dbuffer,
sayzyas	10:a2bd7d07c7f8	2629	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2630	&winchData,
sayzyas	10:a2bd7d07c7f8	2631	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2632	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2633	);
sayzyas	10:a2bd7d07c7f8	2634	}
sayzyas	10:a2bd7d07c7f8	2635	else if( !strcmp( dbuffer, "WinchStepUpOn" ) ){
sayzyas	10:a2bd7d07c7f8	2636	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2637	WINCH_STEPUP_BTN_ON,
sayzyas	10:a2bd7d07c7f8	2638	dbuffer,
sayzyas	10:a2bd7d07c7f8	2639	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2640	&winchData,
sayzyas	10:a2bd7d07c7f8	2641	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2642	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2643	);
sayzyas	10:a2bd7d07c7f8	2644	}
sayzyas	10:a2bd7d07c7f8	2645	else if( !strcmp( dbuffer, "WinchStepUpOf" ) ){
sayzyas	10:a2bd7d07c7f8	2646	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2647	WINCH_STEPUP_BTN_OFF,
sayzyas	10:a2bd7d07c7f8	2648	dbuffer,
sayzyas	10:a2bd7d07c7f8	2649	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2650	&winchData,
sayzyas	10:a2bd7d07c7f8	2651	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2652	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2653	);
sayzyas	10:a2bd7d07c7f8	2654	}
sayzyas	10:a2bd7d07c7f8	2655	else if( !strcmp( dbuffer, "WinchStepDnOn" ) ){
sayzyas	10:a2bd7d07c7f8	2656	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2657	WINCH_STEPDOWN_BTN_ON,
sayzyas	10:a2bd7d07c7f8	2658	dbuffer,
sayzyas	10:a2bd7d07c7f8	2659	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2660	&winchData,
sayzyas	10:a2bd7d07c7f8	2661	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2662	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2663	);
sayzyas	10:a2bd7d07c7f8	2664	}
sayzyas	10:a2bd7d07c7f8	2665	else if( !strcmp( dbuffer, "WinchStepDnOf" ) ){
sayzyas	10:a2bd7d07c7f8	2666	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2667	WINCH_STEPDOWN_BTN_OFF,
sayzyas	10:a2bd7d07c7f8	2668	dbuffer,
sayzyas	10:a2bd7d07c7f8	2669	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2670	&winchData,
sayzyas	10:a2bd7d07c7f8	2671	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2672	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2673	);
sayzyas	10:a2bd7d07c7f8	2674	}
sayzyas	10:a2bd7d07c7f8	2675
sayzyas	10:a2bd7d07c7f8	2676	else if( !strcmp( dbuffer, "WinchuStepUpOn" ) ){
sayzyas	10:a2bd7d07c7f8	2677	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2678	WINCH_U_STEPUP_BTN_ON,
sayzyas	10:a2bd7d07c7f8	2679	dbuffer,
sayzyas	10:a2bd7d07c7f8	2680	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2681	&winchData,
sayzyas	10:a2bd7d07c7f8	2682	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2683	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2684	);
sayzyas	10:a2bd7d07c7f8	2685	}
sayzyas	10:a2bd7d07c7f8	2686	else if( !strcmp( dbuffer, "WinchuStepUpOf" ) ){
sayzyas	10:a2bd7d07c7f8	2687	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2688	WINCH_U_STEPUP_BTN_OFF,
sayzyas	10:a2bd7d07c7f8	2689	dbuffer,
sayzyas	10:a2bd7d07c7f8	2690	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2691	&winchData,
sayzyas	10:a2bd7d07c7f8	2692	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2693	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2694	);
sayzyas	10:a2bd7d07c7f8	2695	}
sayzyas	10:a2bd7d07c7f8	2696	else if( !strcmp( dbuffer, "WinchuStepDnOn" ) ){
sayzyas	10:a2bd7d07c7f8	2697	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2698	WINCH_U_STEPDOWN_BTN_ON,
sayzyas	10:a2bd7d07c7f8	2699	dbuffer,
sayzyas	10:a2bd7d07c7f8	2700	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2701	&winchData,
sayzyas	10:a2bd7d07c7f8	2702	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2703	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2704	);
sayzyas	10:a2bd7d07c7f8	2705	}
sayzyas	10:a2bd7d07c7f8	2706	else if( !strcmp( dbuffer, "WinchuStepDnOf" ) ){
sayzyas	10:a2bd7d07c7f8	2707	winchMovingControl(
sayzyas	10:a2bd7d07c7f8	2708	WINCH_U_STEPDOWN_BTN_OFF,
sayzyas	10:a2bd7d07c7f8	2709	dbuffer,
sayzyas	10:a2bd7d07c7f8	2710	sizeof(dbuffer),
sayzyas	10:a2bd7d07c7f8	2711	&winchData,
sayzyas	10:a2bd7d07c7f8	2712	sizeof( winchData ),
sayzyas	10:a2bd7d07c7f8	2713	I2C_cmd
sayzyas	10:a2bd7d07c7f8	2714	);
sayzyas	10:a2bd7d07c7f8	2715	}
sayzyas	10:a2bd7d07c7f8	2716
sayzyas	10:a2bd7d07c7f8	2717	else if( !strcmp( dbuffer, "SetValue" ) ){
sayzyas	11:ff06edc0219c	2718	DEBUG_PRINT_L3("Bd0> SetValue Request from client\r\n");
sayzyas	14:3a5ae61ab1f4	2719	Thread::wait(200);
sayzyas	14:3a5ae61ab1f4	2720	rcv_data_cnt = tcp_client.receive( (char*)&new_setValue, sizeof(new_setValue));
sayzyas	14:3a5ae61ab1f4	2721	Thread::wait(200);
sayzyas	14:3a5ae61ab1f4	2722	// DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
sayzyas	14:3a5ae61ab1f4	2723	// dspSetValue2Console( &pc, &new_setValue );
sayzyas	14:3a5ae61ab1f4	2724	// DEBUG_PRINT_L0("Bd0> write setting file to local file sysytem\r\n");
sayzyas	14:3a5ae61ab1f4	2725
sayzyas	14:3a5ae61ab1f4	2726	thread_hif.terminate();
sayzyas	14:3a5ae61ab1f4	2727	thread_gpd.terminate();
sayzyas	14:3a5ae61ab1f4	2728
sayzyas	14:3a5ae61ab1f4	2729	// file_access_mutex.lock();
sayzyas	14:3a5ae61ab1f4	2730	write_LFS(&new_setValue);
sayzyas	14:3a5ae61ab1f4	2731	// file_access_mutex.unlock();
sayzyas	14:3a5ae61ab1f4	2732
sayzyas	14:3a5ae61ab1f4	2733	Thread::wait(500);
sayzyas	14:3a5ae61ab1f4	2734
sayzyas	14:3a5ae61ab1f4	2735	DEBUG_PRINT_L0("Bd0> SetValue instruction is over\r\n");
sayzyas	10:a2bd7d07c7f8	2736	}
sayzyas	10:a2bd7d07c7f8	2737	else if(!strcmp( dbuffer, "GetValue" )){
sayzyas	14:3a5ae61ab1f4	2738	DEBUG_PRINT_L3("Bd0> GetValue request from TCP client\r\n");
sayzyas	14:3a5ae61ab1f4	2739
sayzyas	14:3a5ae61ab1f4	2740	flg_mutex.lock(); // This is very important
sayzyas	14:3a5ae61ab1f4	2741	memcpy( &new_setValue, &setValue, sizeof( new_setValue ) );
sayzyas	14:3a5ae61ab1f4	2742	flg_mutex.unlock(); // This is very important
sayzyas	14:3a5ae61ab1f4	2743	dspSetValue2Console( &pc, &new_setValue );
sayzyas	14:3a5ae61ab1f4	2744	tcp_client.send_all( (char*)&new_setValue, sizeof(new_setValue) );
sayzyas	14:3a5ae61ab1f4	2745	DEBUG_PRINT_L2("(%d)\r\n", sizeof(new_setValue));
sayzyas	10:a2bd7d07c7f8	2746	}
sayzyas	10:a2bd7d07c7f8	2747	}
sayzyas	10:a2bd7d07c7f8	2748	if( rcv_data_cnt <= 0 ) break;
sayzyas	10:a2bd7d07c7f8	2749	}
sayzyas	10:a2bd7d07c7f8	2750	tcp_client.close();
sayzyas	10:a2bd7d07c7f8	2751	}
sayzyas	10:a2bd7d07c7f8	2752	}
sayzyas	10:a2bd7d07c7f8	2753	}

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@14:3a5ae61ab1f4, 2016-04-14 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning