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@12:3e6b6fcf540b, 2016-02-19 (annotated)

Committer:: sayzyas
Date:: Fri Feb 19 07:02:35 2016 +0000
Revision:: 12:3e6b6fcf540b
Parent:: 11:ff06edc0219c
Child:: 13:2c70c772fe24

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

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@12:3e6b6fcf540b, 2016-02-19 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning