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@11:ff06edc0219c, 2016-02-16 (annotated)

Committer:
sayzyas
Date:
Tue Feb 16 16:34:12 2016 +0000
Revision:
11:ff06edc0219c
Parent:
10:a2bd7d07c7f8
Child:
12:3e6b6fcf540b
Rev0.95 160216

Who changed what in which revision?

UserRevisionLine numberNew 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 11:ff06edc0219c 521 i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 522
sayzyas 11:ff06edc0219c 523 read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
sayzyas 11:ff06edc0219c 524 motor1_current_pct = I2C_res[0];
sayzyas 11:ff06edc0219c 525 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 526 DEBUG_PRINT_L2( "Bd0> 1: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 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 11:ff06edc0219c 539 i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 11:ff06edc0219c 540
sayzyas 11:ff06edc0219c 541 read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
sayzyas 11:ff06edc0219c 542 motor1_current_pct = I2C_res[0];
sayzyas 11:ff06edc0219c 543 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 544 DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 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 10:a2bd7d07c7f8 554 i2c.write(i2c_addr_crawler, 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 11:ff06edc0219c 564 i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 11:ff06edc0219c 565
sayzyas 11:ff06edc0219c 566 read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
sayzyas 11:ff06edc0219c 567 motor1_current_pct = I2C_res[0];
sayzyas 11:ff06edc0219c 568 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 569 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 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 11:ff06edc0219c 582 i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 11:ff06edc0219c 583
sayzyas 11:ff06edc0219c 584 read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
sayzyas 11:ff06edc0219c 585 motor1_current_pct = I2C_res[0];
sayzyas 11:ff06edc0219c 586 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 587 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 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 10:a2bd7d07c7f8 597 i2c.write(i2c_addr_crawler, 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 11:ff06edc0219c 632 DEBUG_PRINT_L4( "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 10:a2bd7d07c7f8 639 i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 11:ff06edc0219c 640 read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
sayzyas 11:ff06edc0219c 641 motor1_current_pct = I2C_res[0];
sayzyas 11:ff06edc0219c 642 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 643 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 644 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct ); flg_ButtonOn = false;
sayzyas 10:a2bd7d07c7f8 645 }
sayzyas 10:a2bd7d07c7f8 646 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 647 flg_ButtonOn = true;
sayzyas 10:a2bd7d07c7f8 648 led3 = 1;
sayzyas 11:ff06edc0219c 649 if( setValue.operation.sv_JS_ShapeMode == 0 ){ // I-Shape
sayzyas 11:ff06edc0219c 650 I2C_cmd[2] = MOTOR_STP; // Motor1 Reverse
sayzyas 11:ff06edc0219c 651 I2C_cmd[6] = MOTOR_STP; // Motor1 Reverse
sayzyas 11:ff06edc0219c 652 }
sayzyas 11:ff06edc0219c 653 else{
sayzyas 11:ff06edc0219c 654 I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
sayzyas 11:ff06edc0219c 655 I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
sayzyas 11:ff06edc0219c 656 }
sayzyas 10:a2bd7d07c7f8 657 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 658 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 11:ff06edc0219c 659 DEBUG_PRINT_L4( "Bd0> Single Mode: Dir2 (Speed: %d, %d)\r\n", I2C_cmd[5], I2C_cmd[7]);
sayzyas 10:a2bd7d07c7f8 660 flg_exp_status |= 0x00200000;
sayzyas 10:a2bd7d07c7f8 661 flg_exp_status |= 0x00400000; // 0x00600000
sayzyas 10:a2bd7d07c7f8 662 I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
sayzyas 10:a2bd7d07c7f8 663 I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
sayzyas 10:a2bd7d07c7f8 664 I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
sayzyas 10:a2bd7d07c7f8 665 I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
sayzyas 10:a2bd7d07c7f8 666 i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 11:ff06edc0219c 667 read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
sayzyas 11:ff06edc0219c 668 motor1_current_pct = I2C_res[0];
sayzyas 11:ff06edc0219c 669 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 670 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 671 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas 10:a2bd7d07c7f8 672 flg_ButtonOn = false;
sayzyas 10:a2bd7d07c7f8 673 }
sayzyas 10:a2bd7d07c7f8 674 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 675 flg_ButtonOn = true;
sayzyas 10:a2bd7d07c7f8 676 led3 = 1;
sayzyas 11:ff06edc0219c 677 if( setValue.operation.sv_JS_ShapeMode == 0 ){ // I-Shape
sayzyas 11:ff06edc0219c 678 I2C_cmd[2] = MOTOR_FWD; // Motor1 Rvs
sayzyas 11:ff06edc0219c 679 }
sayzyas 11:ff06edc0219c 680 else{
sayzyas 11:ff06edc0219c 681 I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
sayzyas 11:ff06edc0219c 682 }
sayzyas 11:ff06edc0219c 683 I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
sayzyas 10:a2bd7d07c7f8 684 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 685 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 11:ff06edc0219c 686 DEBUG_PRINT_L4( "Bd0> Single Mode: Dir3 (Speed: %d, %d)\r\n", I2C_cmd[5], I2C_cmd[7]);
sayzyas 10:a2bd7d07c7f8 687 flg_exp_status |= 0x00200000;
sayzyas 10:a2bd7d07c7f8 688 flg_exp_status |= 0x00800000; // 0x00A00000
sayzyas 10:a2bd7d07c7f8 689 I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
sayzyas 10:a2bd7d07c7f8 690 I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
sayzyas 10:a2bd7d07c7f8 691 I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
sayzyas 10:a2bd7d07c7f8 692 I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
sayzyas 10:a2bd7d07c7f8 693 i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 11:ff06edc0219c 694 read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
sayzyas 11:ff06edc0219c 695 motor1_current_pct = I2C_res[0];
sayzyas 11:ff06edc0219c 696 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 697 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 698 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas 10:a2bd7d07c7f8 699 flg_ButtonOn = false;
sayzyas 10:a2bd7d07c7f8 700 }
sayzyas 10:a2bd7d07c7f8 701 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 702 flg_ButtonOn = true;
sayzyas 10:a2bd7d07c7f8 703 led3 = 1;
sayzyas 11:ff06edc0219c 704 if( setValue.operation.sv_JS_ShapeMode == 0 ){ // I-Shape
sayzyas 11:ff06edc0219c 705 I2C_cmd[2] = MOTOR_STP; // Motor1 Reverse
sayzyas 11:ff06edc0219c 706 I2C_cmd[6] = MOTOR_STP; // Motor1 Reverse
sayzyas 11:ff06edc0219c 707 }
sayzyas 11:ff06edc0219c 708 else{
sayzyas 11:ff06edc0219c 709 I2C_cmd[2] = MOTOR_FWD; // Motor1 Fwd
sayzyas 11:ff06edc0219c 710 I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
sayzyas 11:ff06edc0219c 711 }
sayzyas 10:a2bd7d07c7f8 712 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 713 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 11:ff06edc0219c 714 DEBUG_PRINT_L4( "Bd0> Single Mode: Dir4 (Speed: %d, %d)\r\n", I2C_cmd[5], I2C_cmd[7]);
sayzyas 10:a2bd7d07c7f8 715 flg_exp_status |= 0x00100000;
sayzyas 10:a2bd7d07c7f8 716 flg_exp_status |= 0x00800000; // 0x00900000
sayzyas 10:a2bd7d07c7f8 717 I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
sayzyas 10:a2bd7d07c7f8 718 I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
sayzyas 10:a2bd7d07c7f8 719 I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
sayzyas 10:a2bd7d07c7f8 720 I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
sayzyas 10:a2bd7d07c7f8 721 i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 11:ff06edc0219c 722 read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
sayzyas 11:ff06edc0219c 723 motor1_current_pct = I2C_res[0];
sayzyas 11:ff06edc0219c 724 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 725 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 726 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas 10:a2bd7d07c7f8 727 flg_ButtonOn = false;
sayzyas 10:a2bd7d07c7f8 728 }
sayzyas 10:a2bd7d07c7f8 729 // ====================================
sayzyas 10:a2bd7d07c7f8 730 // ALL motor off commmand packet send
sayzyas 10:a2bd7d07c7f8 731 // ====================================
sayzyas 10:a2bd7d07c7f8 732 else {
sayzyas 10:a2bd7d07c7f8 733 led3 = 0;
sayzyas 10:a2bd7d07c7f8 734 #ifdef __IIC_COMAMND_SEND__
sayzyas 10:a2bd7d07c7f8 735 I2C_cmd[2] = MOTOR_STP; // Motor1 Fwd
sayzyas 10:a2bd7d07c7f8 736 I2C_cmd[3] = 0; // Speed=0
sayzyas 10:a2bd7d07c7f8 737 I2C_cmd[6] = MOTOR_STP; // Motor2 Rvs
sayzyas 10:a2bd7d07c7f8 738 I2C_cmd[7] = 0; // Speed=0
sayzyas 10:a2bd7d07c7f8 739 Thread::wait(5);
sayzyas 10:a2bd7d07c7f8 740 #endif
sayzyas 10:a2bd7d07c7f8 741 flg_exp_status &= 0xFF0FFFFF;
sayzyas 10:a2bd7d07c7f8 742 I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
sayzyas 10:a2bd7d07c7f8 743 I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
sayzyas 10:a2bd7d07c7f8 744 I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
sayzyas 10:a2bd7d07c7f8 745 I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
sayzyas 10:a2bd7d07c7f8 746 i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 747 i2c.write(i2c_addr_crawler, 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 10:a2bd7d07c7f8 781 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 782 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 783
sayzyas 11:ff06edc0219c 784 read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_res, 3 );
sayzyas 10:a2bd7d07c7f8 785 motor1_current_pct = I2C_res[0];
sayzyas 10:a2bd7d07c7f8 786 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 787 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 788 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas 10:a2bd7d07c7f8 789 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 790
sayzyas 10:a2bd7d07c7f8 791 flg_ButtonOn = false;
sayzyas 10:a2bd7d07c7f8 792 }
sayzyas 10:a2bd7d07c7f8 793 else if ( btnStatus_WUP == btnID_WUP ) { // Winch Up (Rvs)
sayzyas 10:a2bd7d07c7f8 794 flg_ButtonOn = true;
sayzyas 11:ff06edc0219c 795 DEBUG_PRINT_L3( "Bd0> BTN W-UP(Rvs)\r\n" );
sayzyas 10:a2bd7d07c7f8 796 led3 = 1;
sayzyas 10:a2bd7d07c7f8 797 #ifdef __IIC_COMAMND_SEND__
sayzyas 10:a2bd7d07c7f8 798 I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
sayzyas 10:a2bd7d07c7f8 799 I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_R; // Speed
sayzyas 10:a2bd7d07c7f8 800 // I2C_cmd[6] = MOTOR_RVS; // Motor2 RVS
sayzyas 10:a2bd7d07c7f8 801 // I2C_cmd[7] = setValue.winchCtrl.sv_WRM_sli_R; // Speed
sayzyas 10:a2bd7d07c7f8 802 #endif
sayzyas 10:a2bd7d07c7f8 803 //// Are these part necessary ?? ////
sayzyas 10:a2bd7d07c7f8 804 ////winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
sayzyas 10:a2bd7d07c7f8 805 // ReadCurrentPositon(i2c_addr_resolver, &winchData, sizeof(winchData), 0x00 );
sayzyas 10:a2bd7d07c7f8 806 // winchCurrentPosition = winchData.dt_WinchCntPosition;
sayzyas 11:ff06edc0219c 807 ////DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
sayzyas 10:a2bd7d07c7f8 808
sayzyas 10:a2bd7d07c7f8 809 flg_exp_status |= 0x02000000;
sayzyas 10:a2bd7d07c7f8 810 I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
sayzyas 10:a2bd7d07c7f8 811 I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
sayzyas 10:a2bd7d07c7f8 812 I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
sayzyas 10:a2bd7d07c7f8 813 I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
sayzyas 10:a2bd7d07c7f8 814 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 815
sayzyas 11:ff06edc0219c 816 read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_res, 3 );
sayzyas 10:a2bd7d07c7f8 817 motor1_current_pct = I2C_res[0];
sayzyas 10:a2bd7d07c7f8 818 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 819 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 820 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas 10:a2bd7d07c7f8 821 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 822
sayzyas 10:a2bd7d07c7f8 823 flg_ButtonOn = false;
sayzyas 10:a2bd7d07c7f8 824 }
sayzyas 10:a2bd7d07c7f8 825 // ====================================
sayzyas 10:a2bd7d07c7f8 826 // ALL motor off commmand packet send
sayzyas 10:a2bd7d07c7f8 827 // ====================================
sayzyas 10:a2bd7d07c7f8 828 else {
sayzyas 10:a2bd7d07c7f8 829 led3 = 0;
sayzyas 10:a2bd7d07c7f8 830 #ifdef __IIC_COMAMND_SEND__
sayzyas 10:a2bd7d07c7f8 831 I2C_cmd[2] = MOTOR_STP;
sayzyas 10:a2bd7d07c7f8 832 I2C_cmd[3] = 0;
sayzyas 10:a2bd7d07c7f8 833 I2C_cmd[6] = MOTOR_STP;
sayzyas 10:a2bd7d07c7f8 834 I2C_cmd[7] = 0;
sayzyas 10:a2bd7d07c7f8 835 Thread::wait(5);
sayzyas 10:a2bd7d07c7f8 836 #endif
sayzyas 10:a2bd7d07c7f8 837 flg_exp_status &= 0xF0FFFFFF;
sayzyas 10:a2bd7d07c7f8 838 I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
sayzyas 10:a2bd7d07c7f8 839 I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
sayzyas 10:a2bd7d07c7f8 840 I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
sayzyas 10:a2bd7d07c7f8 841 I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
sayzyas 10:a2bd7d07c7f8 842 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 843 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 844 }
sayzyas 10:a2bd7d07c7f8 845 //// Are these part necessary ?? ////
sayzyas 10:a2bd7d07c7f8 846 ////winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
sayzyas 10:a2bd7d07c7f8 847 //ReadCurrentPositon(i2c_addr_resolver, &winchData, sizeof(winchData), 0x00 );
sayzyas 10:a2bd7d07c7f8 848 //winchCurrentPosition = winchData.dt_WinchCntPosition;
sayzyas 11:ff06edc0219c 849 ////DEBUG_PRINT_L3("Bd0> Winch Current Position (button) : %04d\r\n", winchCurrentPosition );
sayzyas 10:a2bd7d07c7f8 850
sayzyas 10:a2bd7d07c7f8 851 /*
sayzyas 10:a2bd7d07c7f8 852 // ====================================
sayzyas 10:a2bd7d07c7f8 853 // TRANSFORM Motor Control
sayzyas 10:a2bd7d07c7f8 854 // ====================================
sayzyas 10:a2bd7d07c7f8 855 * 7 6 5 4 3 2 1 0
sayzyas 10:a2bd7d07c7f8 856 * +-+-+-+-+-+-+-+-+
sayzyas 10:a2bd7d07c7f8 857 * |o|x|x|x|x|x|x|x| 1: RF-I, 2: RF-K, 4: LB-I, 8: LB-K
sayzyas 10:a2bd7d07c7f8 858 * +-+-+-+-+-+-+-+-+
sayzyas 10:a2bd7d07c7f8 859 */
sayzyas 10:a2bd7d07c7f8 860
sayzyas 10:a2bd7d07c7f8 861 if ( btnStatus_RFK == btnID_RFK ) { // RF KO
sayzyas 10:a2bd7d07c7f8 862 flg_ButtonOn = true;
sayzyas 11:ff06edc0219c 863 DEBUG_PRINT_L3( "Bd0> BTN RF-K\r\n" );
sayzyas 10:a2bd7d07c7f8 864 led3 = 1;
sayzyas 10:a2bd7d07c7f8 865 #ifdef __IIC_COMAMND_SEND__
sayzyas 10:a2bd7d07c7f8 866 I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
sayzyas 10:a2bd7d07c7f8 867 I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_F; // Speed
sayzyas 10:a2bd7d07c7f8 868 #endif
sayzyas 10:a2bd7d07c7f8 869 flg_exp_status |= 0x10000000;
sayzyas 11:ff06edc0219c 870 I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
sayzyas 11:ff06edc0219c 871 I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
sayzyas 11:ff06edc0219c 872 I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
sayzyas 11:ff06edc0219c 873 I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
sayzyas 11:ff06edc0219c 874 i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 11:ff06edc0219c 875 read_motorCurrent( i2c_addr_transform, I2C_readcmd, I2C_res, 3 );
sayzyas 11:ff06edc0219c 876 motor1_current_pct = I2C_res[0];
sayzyas 11:ff06edc0219c 877 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 878 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 879 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas 11:ff06edc0219c 880 i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 11:ff06edc0219c 881 flg_ButtonOn = false;
sayzyas 10:a2bd7d07c7f8 882 }
sayzyas 10:a2bd7d07c7f8 883 else if ( btnStatus_RFI == btnID_RFI ) { // RF I
sayzyas 11:ff06edc0219c 884 DEBUG_PRINT_L3( "Bd0> BTN RF-I\r\n" );
sayzyas 10:a2bd7d07c7f8 885 led3 = 1;
sayzyas 10:a2bd7d07c7f8 886 #ifdef __IIC_COMAMND_SEND__
sayzyas 10:a2bd7d07c7f8 887 I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
sayzyas 10:a2bd7d07c7f8 888 I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_R; // Speed
sayzyas 10:a2bd7d07c7f8 889 #endif
sayzyas 10:a2bd7d07c7f8 890 flg_exp_status |= 0x20000000;
sayzyas 10:a2bd7d07c7f8 891 I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
sayzyas 10:a2bd7d07c7f8 892 I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
sayzyas 10:a2bd7d07c7f8 893 I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
sayzyas 10:a2bd7d07c7f8 894 I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
sayzyas 10:a2bd7d07c7f8 895 i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 11:ff06edc0219c 896 read_motorCurrent( i2c_addr_transform, I2C_readcmd, I2C_res, 3 );
sayzyas 11:ff06edc0219c 897 motor1_current_pct = I2C_res[0];
sayzyas 11:ff06edc0219c 898 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 899 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 900 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas 10:a2bd7d07c7f8 901 i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 902 flg_ButtonOn = false;
sayzyas 10:a2bd7d07c7f8 903 }
sayzyas 10:a2bd7d07c7f8 904 else if ( btnStatus_LBK == btnID_LBK ) { // LB KO
sayzyas 10:a2bd7d07c7f8 905 flg_ButtonOn = true;
sayzyas 11:ff06edc0219c 906 DEBUG_PRINT_L3( "Bd0> BTN LB-K\r\n" );
sayzyas 10:a2bd7d07c7f8 907 led3 = 1;
sayzyas 10:a2bd7d07c7f8 908 #ifdef __IIC_COMAMND_SEND__
sayzyas 10:a2bd7d07c7f8 909 I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
sayzyas 10:a2bd7d07c7f8 910 I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_F; // Speed
sayzyas 10:a2bd7d07c7f8 911 #endif
sayzyas 10:a2bd7d07c7f8 912 flg_exp_status |= 0x40000000;
sayzyas 10:a2bd7d07c7f8 913
sayzyas 10:a2bd7d07c7f8 914 I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
sayzyas 10:a2bd7d07c7f8 915 I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
sayzyas 10:a2bd7d07c7f8 916 I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
sayzyas 10:a2bd7d07c7f8 917 I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
sayzyas 10:a2bd7d07c7f8 918 i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 11:ff06edc0219c 919 read_motorCurrent( i2c_addr_transform, I2C_readcmd, I2C_res, 3 );
sayzyas 11:ff06edc0219c 920 motor1_current_pct = I2C_res[0];
sayzyas 11:ff06edc0219c 921 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 922 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 923 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas 10:a2bd7d07c7f8 924 i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 925 flg_ButtonOn = false;
sayzyas 10:a2bd7d07c7f8 926 }
sayzyas 10:a2bd7d07c7f8 927 else if ( btnStatus_LBI == btnID_LBI ) { // LB I
sayzyas 10:a2bd7d07c7f8 928 flg_ButtonOn = true;
sayzyas 11:ff06edc0219c 929 DEBUG_PRINT_L3( "Bd0> BTN LB-I\r\n" );
sayzyas 10:a2bd7d07c7f8 930 led3 = 1;
sayzyas 10:a2bd7d07c7f8 931 #ifdef __IIC_COMAMND_SEND__
sayzyas 10:a2bd7d07c7f8 932 I2C_cmd[6] = MOTOR_RVS; // Motor1 RVS
sayzyas 10:a2bd7d07c7f8 933 I2C_cmd[7] = setValue.tfmCtrl.sv_LBTM_sli_R; // Speed
sayzyas 10:a2bd7d07c7f8 934 #endif
sayzyas 10:a2bd7d07c7f8 935 flg_exp_status |= 0x80000000;
sayzyas 10:a2bd7d07c7f8 936
sayzyas 10:a2bd7d07c7f8 937 I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
sayzyas 10:a2bd7d07c7f8 938 I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
sayzyas 10:a2bd7d07c7f8 939 I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
sayzyas 10:a2bd7d07c7f8 940 I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
sayzyas 10:a2bd7d07c7f8 941 i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 11:ff06edc0219c 942 read_motorCurrent( i2c_addr_transform, I2C_readcmd, I2C_res, 3 );
sayzyas 11:ff06edc0219c 943 motor1_current_pct = I2C_res[0];
sayzyas 11:ff06edc0219c 944 motor2_current_pct = I2C_res[1];
sayzyas 11:ff06edc0219c 945 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
sayzyas 11:ff06edc0219c 946 DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
sayzyas 10:a2bd7d07c7f8 947 i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 948 flg_ButtonOn = false;
sayzyas 10:a2bd7d07c7f8 949 }
sayzyas 10:a2bd7d07c7f8 950 // ====================================
sayzyas 10:a2bd7d07c7f8 951 // ALL motor off commmand packet send
sayzyas 10:a2bd7d07c7f8 952 // ====================================
sayzyas 10:a2bd7d07c7f8 953 else {
sayzyas 10:a2bd7d07c7f8 954 led3 = 0;
sayzyas 10:a2bd7d07c7f8 955 #ifdef __IIC_COMAMND_SEND__
sayzyas 10:a2bd7d07c7f8 956 I2C_cmd[2] = MOTOR_STP;
sayzyas 10:a2bd7d07c7f8 957 I2C_cmd[3] = 0;
sayzyas 10:a2bd7d07c7f8 958 I2C_cmd[6] = MOTOR_STP;
sayzyas 10:a2bd7d07c7f8 959 I2C_cmd[7] = 0;
sayzyas 10:a2bd7d07c7f8 960 Thread::wait(5);
sayzyas 10:a2bd7d07c7f8 961 #endif
sayzyas 10:a2bd7d07c7f8 962 flg_exp_status &= 0x0FFFFFFF;
sayzyas 10:a2bd7d07c7f8 963 I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
sayzyas 10:a2bd7d07c7f8 964 I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
sayzyas 10:a2bd7d07c7f8 965 I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
sayzyas 10:a2bd7d07c7f8 966 I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
sayzyas 10:a2bd7d07c7f8 967 i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 968 i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 969
sayzyas 10:a2bd7d07c7f8 970 }
sayzyas 10:a2bd7d07c7f8 971 }
sayzyas 10:a2bd7d07c7f8 972 }
sayzyas 10:a2bd7d07c7f8 973
sayzyas 10:a2bd7d07c7f8 974 uint32_t getc_fromHost( uint8_t *c ){
sayzyas 10:a2bd7d07c7f8 975
sayzyas 10:a2bd7d07c7f8 976 uint32_t rts = 0;
sayzyas 10:a2bd7d07c7f8 977
sayzyas 10:a2bd7d07c7f8 978 if(pc.readable()){
sayzyas 10:a2bd7d07c7f8 979 *c = pc.getc();
sayzyas 10:a2bd7d07c7f8 980 rts = 0;
sayzyas 10:a2bd7d07c7f8 981 }
sayzyas 10:a2bd7d07c7f8 982 else{
sayzyas 10:a2bd7d07c7f8 983 rts = 1;
sayzyas 10:a2bd7d07c7f8 984 }
sayzyas 10:a2bd7d07c7f8 985 return rts;
sayzyas 10:a2bd7d07c7f8 986 }
sayzyas 10:a2bd7d07c7f8 987
sayzyas 10:a2bd7d07c7f8 988 // **************************************************************
sayzyas 10:a2bd7d07c7f8 989 // TASK: Hoat Interface Task
sayzyas 10:a2bd7d07c7f8 990 //
sayzyas 10:a2bd7d07c7f8 991 // **************************************************************
sayzyas 10:a2bd7d07c7f8 992 void clientPC_interface_task(void const *)
sayzyas 10:a2bd7d07c7f8 993 {
sayzyas 10:a2bd7d07c7f8 994 int rcv_data_cnt;
sayzyas 10:a2bd7d07c7f8 995 //winchData_t winchData;
sayzyas 10:a2bd7d07c7f8 996
sayzyas 10:a2bd7d07c7f8 997 char I2C_cmd[NumberOfI2CCommand+1] = "#010000";
sayzyas 10:a2bd7d07c7f8 998
sayzyas 10:a2bd7d07c7f8 999 // winchData_t winchData;
sayzyas 10:a2bd7d07c7f8 1000 int16_t winchCurrentPosition;
sayzyas 11:ff06edc0219c 1001
sayzyas 11:ff06edc0219c 1002 int cnt = 0;
sayzyas 10:a2bd7d07c7f8 1003
sayzyas 10:a2bd7d07c7f8 1004 while(1){
sayzyas 10:a2bd7d07c7f8 1005
sayzyas 11:ff06edc0219c 1006 // DEBUG_PRINT("\r\nWaiting for UDP packet...\r\n");
sayzyas 10:a2bd7d07c7f8 1007 rcv_data_cnt = udp_server.receiveFrom(client, dbuffer, sizeof(dbuffer));
sayzyas 10:a2bd7d07c7f8 1008 Thread::wait(10);
sayzyas 10:a2bd7d07c7f8 1009 if( rcv_data_cnt < 0 ){
sayzyas 11:ff06edc0219c 1010 DEBUG_PRINT_L0("Bd0> ## Receive packet fail ##\r\n");
sayzyas 10:a2bd7d07c7f8 1011 }
sayzyas 10:a2bd7d07c7f8 1012 else{
sayzyas 10:a2bd7d07c7f8 1013 dbuffer[rcv_data_cnt] = '\0';
sayzyas 10:a2bd7d07c7f8 1014 led4 = 1;
sayzyas 10:a2bd7d07c7f8 1015
sayzyas 11:ff06edc0219c 1016 if(!strcmp( dbuffer, "Hello Z\r\n" )){
sayzyas 10:a2bd7d07c7f8 1017 DEBUG_PRINT_L2("Bd0> Hello Z Packet received from client by UDP\n");
sayzyas 10:a2bd7d07c7f8 1018 char snd_data[] = "Hello I'm CrExoB2";
sayzyas 10:a2bd7d07c7f8 1019 udp_server.sendTo(client, snd_data, sizeof(snd_data));
sayzyas 10:a2bd7d07c7f8 1020 Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas 10:a2bd7d07c7f8 1021 }
sayzyas 11:ff06edc0219c 1022 else if(!strcmp( dbuffer, "status\r\n")){
sayzyas 11:ff06edc0219c 1023 DEBUG_PRINT_L2("Bd0> Hello Status req received from client by UDP\r\n");
sayzyas 11:ff06edc0219c 1024 strcpy(dbuffer,"XXXX\r\n");
sayzyas 10:a2bd7d07c7f8 1025 udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
sayzyas 10:a2bd7d07c7f8 1026 Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas 10:a2bd7d07c7f8 1027 }
sayzyas 10:a2bd7d07c7f8 1028 else if(!strcmp( dbuffer, "Hello")){
sayzyas 11:ff06edc0219c 1029 DEBUG_PRINT_L2( "Bd0> Hello Packet received from [ 0x%02x ]\r\n", flg_exp_status );
sayzyas 10:a2bd7d07c7f8 1030
sayzyas 10:a2bd7d07c7f8 1031 /* ***************************************** */
sayzyas 10:a2bd7d07c7f8 1032 /* Read Winch Current Position from Resolver */
sayzyas 10:a2bd7d07c7f8 1033 /* ***************************************** */
sayzyas 10:a2bd7d07c7f8 1034 winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
sayzyas 10:a2bd7d07c7f8 1035 Thread::wait(10);
sayzyas 10:a2bd7d07c7f8 1036
sayzyas 11:ff06edc0219c 1037 sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
sayzyas 10:a2bd7d07c7f8 1038 // -------------------------------------
sayzyas 10:a2bd7d07c7f8 1039 // Crawler Moving
sayzyas 10:a2bd7d07c7f8 1040 // -------------------------------------
sayzyas 10:a2bd7d07c7f8 1041 if( flg_exp_status & 0x00F00000 ){
sayzyas 10:a2bd7d07c7f8 1042 // Forward move 5
sayzyas 10:a2bd7d07c7f8 1043 // Reverce move a
sayzyas 10:a2bd7d07c7f8 1044 // Right rotation 6
sayzyas 10:a2bd7d07c7f8 1045 // Left rotation 9
sayzyas 11:ff06edc0219c 1046
sayzyas 10:a2bd7d07c7f8 1047 if((flg_exp_status & 0x00F00000) == 0x00500000 ){
sayzyas 11:ff06edc0219c 1048 // 01234 5678 9012 34569
sayzyas 11:ff06edc0219c 1049 sprintf( dbuffer, "WCFW %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
sayzyas 10:a2bd7d07c7f8 1050 // Thread::wait(10);
sayzyas 10:a2bd7d07c7f8 1051 }
sayzyas 10:a2bd7d07c7f8 1052 else if((flg_exp_status & 0x00F00000) == 0x00a00000 ){
sayzyas 11:ff06edc0219c 1053 sprintf( dbuffer, "WCRV %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
sayzyas 10:a2bd7d07c7f8 1054 // Thread::wait(10);
sayzyas 10:a2bd7d07c7f8 1055 }
sayzyas 10:a2bd7d07c7f8 1056 else if((flg_exp_status & 0x00F00000) == 0x00600000 ){
sayzyas 11:ff06edc0219c 1057 sprintf( dbuffer, "WCRR %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
sayzyas 10:a2bd7d07c7f8 1058 // Thread::wait(10);
sayzyas 10:a2bd7d07c7f8 1059 }
sayzyas 10:a2bd7d07c7f8 1060 else if((flg_exp_status & 0x00F00000) == 0x00900000 ){
sayzyas 11:ff06edc0219c 1061 sprintf( dbuffer, "WCLR %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
sayzyas 10:a2bd7d07c7f8 1062 // Thread::wait(10);
sayzyas 10:a2bd7d07c7f8 1063 }
sayzyas 10:a2bd7d07c7f8 1064 else if((flg_exp_status & 0x00F00000) == 0x00800000 ){
sayzyas 11:ff06edc0219c 1065 sprintf( dbuffer, "LCRV %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
sayzyas 10:a2bd7d07c7f8 1066 // Thread::wait(10);
sayzyas 10:a2bd7d07c7f8 1067 }
sayzyas 10:a2bd7d07c7f8 1068 else if((flg_exp_status & 0x00F00000) == 0x00400000 ){
sayzyas 11:ff06edc0219c 1069 sprintf( dbuffer, "LCFW %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
sayzyas 10:a2bd7d07c7f8 1070 // Thread::wait(10);
sayzyas 10:a2bd7d07c7f8 1071 }
sayzyas 10:a2bd7d07c7f8 1072 else if((flg_exp_status & 0x00F00000) == 0x00200000 ){
sayzyas 11:ff06edc0219c 1073 sprintf( dbuffer, "RCRV %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
sayzyas 10:a2bd7d07c7f8 1074 // Thread::wait(10);
sayzyas 10:a2bd7d07c7f8 1075 }
sayzyas 10:a2bd7d07c7f8 1076 else if((flg_exp_status & 0x00F00000) == 0x00100000 ){
sayzyas 11:ff06edc0219c 1077 sprintf( dbuffer, "RCFW %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
sayzyas 10:a2bd7d07c7f8 1078 // Thread::wait(10);
sayzyas 10:a2bd7d07c7f8 1079 }
sayzyas 11:ff06edc0219c 1080 DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas 10:a2bd7d07c7f8 1081 Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas 10:a2bd7d07c7f8 1082 }
sayzyas 10:a2bd7d07c7f8 1083 // -------------------------------------
sayzyas 10:a2bd7d07c7f8 1084
sayzyas 10:a2bd7d07c7f8 1085 // Transform
sayzyas 10:a2bd7d07c7f8 1086 // -------------------------------------
sayzyas 10:a2bd7d07c7f8 1087 else if( flg_exp_status & 0x20000000 ){
sayzyas 11:ff06edc0219c 1088 sprintf(dbuffer,"RF2I %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // RF Crawler Tfm I
sayzyas 11:ff06edc0219c 1089 DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas 10:a2bd7d07c7f8 1090 Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas 10:a2bd7d07c7f8 1091 }
sayzyas 10:a2bd7d07c7f8 1092 else if( flg_exp_status & 0x10000000 ){
sayzyas 11:ff06edc0219c 1093 sprintf(dbuffer,"RF2K %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // RF Crawler Tfm K
sayzyas 11:ff06edc0219c 1094 DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas 10:a2bd7d07c7f8 1095 Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas 10:a2bd7d07c7f8 1096 }
sayzyas 10:a2bd7d07c7f8 1097 else if( flg_exp_status & 0x80000000 ){
sayzyas 11:ff06edc0219c 1098 sprintf(dbuffer,"LB2I %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // LB Crawler Tfm I
sayzyas 11:ff06edc0219c 1099 DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas 10:a2bd7d07c7f8 1100 Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas 10:a2bd7d07c7f8 1101 }
sayzyas 10:a2bd7d07c7f8 1102 else if( flg_exp_status & 0x40000000 ){
sayzyas 11:ff06edc0219c 1103 sprintf(dbuffer,"LB2K %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // LB Crawler Tfm K
sayzyas 11:ff06edc0219c 1104 DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas 10:a2bd7d07c7f8 1105 Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas 10:a2bd7d07c7f8 1106 }
sayzyas 10:a2bd7d07c7f8 1107 // -------------------------------------
sayzyas 10:a2bd7d07c7f8 1108 // Winch Moving
sayzyas 10:a2bd7d07c7f8 1109 // -------------------------------------
sayzyas 10:a2bd7d07c7f8 1110 else if( flg_exp_status & 0x01000000 ){ // Wincd down (FWD)
sayzyas 11:ff06edc0219c 1111 sprintf(dbuffer,"WCDN %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
sayzyas 11:ff06edc0219c 1112 DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas 10:a2bd7d07c7f8 1113 Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas 10:a2bd7d07c7f8 1114 }
sayzyas 10:a2bd7d07c7f8 1115 else if( flg_exp_status & 0x02000000 ){ // Winch up (RVS)
sayzyas 11:ff06edc0219c 1116 sprintf(dbuffer,"WCUP %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
sayzyas 11:ff06edc0219c 1117 DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas 10:a2bd7d07c7f8 1118 Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas 10:a2bd7d07c7f8 1119 }
sayzyas 10:a2bd7d07c7f8 1120 else if( flg_exp_status & 0x0000000f ){
sayzyas 11:ff06edc0219c 1121 if( cnt == 5 ){
sayzyas 11:ff06edc0219c 1122 sprintf( dbuffer,"JSMD %03d %03d %04d\0", setValue.operation.sv_JS_ShapeMode, setValue.operation.sv_JS_OpeMode );
sayzyas 11:ff06edc0219c 1123 cnt = 0;
sayzyas 11:ff06edc0219c 1124 }
sayzyas 11:ff06edc0219c 1125 else{
sayzyas 11:ff06edc0219c 1126 sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
sayzyas 11:ff06edc0219c 1127 }
sayzyas 11:ff06edc0219c 1128 cnt++;
sayzyas 11:ff06edc0219c 1129 DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
sayzyas 10:a2bd7d07c7f8 1130 Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas 10:a2bd7d07c7f8 1131 }
sayzyas 10:a2bd7d07c7f8 1132 else{
sayzyas 11:ff06edc0219c 1133 sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
sayzyas 10:a2bd7d07c7f8 1134 Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas 10:a2bd7d07c7f8 1135 }
sayzyas 10:a2bd7d07c7f8 1136 /* Send data to client PC */
sayzyas 10:a2bd7d07c7f8 1137 udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
sayzyas 10:a2bd7d07c7f8 1138 Thread::wait(10);
sayzyas 10:a2bd7d07c7f8 1139 }
sayzyas 10:a2bd7d07c7f8 1140 }
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 led4 = 0;
sayzyas 10:a2bd7d07c7f8 1143 Thread::wait(100); // When this period seto to short time, gamepad command can not get. 50msec OK
sayzyas 10:a2bd7d07c7f8 1144 }
sayzyas 10:a2bd7d07c7f8 1145 }
sayzyas 10:a2bd7d07c7f8 1146
sayzyas 10:a2bd7d07c7f8 1147 // **************************************************************
sayzyas 10:a2bd7d07c7f8 1148 // TASK: GamaPad Task
sayzyas 10:a2bd7d07c7f8 1149 //
sayzyas 10:a2bd7d07c7f8 1150 // **************************************************************
sayzyas 10:a2bd7d07c7f8 1151 void gamepad_task(void const *)
sayzyas 10:a2bd7d07c7f8 1152 {
sayzyas 10:a2bd7d07c7f8 1153
sayzyas 10:a2bd7d07c7f8 1154 char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
sayzyas 10:a2bd7d07c7f8 1155
sayzyas 10:a2bd7d07c7f8 1156 // int counter = 0;
sayzyas 10:a2bd7d07c7f8 1157 // USB HOST GAMEPAD
sayzyas 10:a2bd7d07c7f8 1158 USBHostGamepad gamepad;
sayzyas 10:a2bd7d07c7f8 1159
sayzyas 10:a2bd7d07c7f8 1160 led1 = 1;
sayzyas 10:a2bd7d07c7f8 1161
sayzyas 10:a2bd7d07c7f8 1162 while(1) {
sayzyas 10:a2bd7d07c7f8 1163
sayzyas 10:a2bd7d07c7f8 1164 // try to connect a USB Gamepad
sayzyas 10:a2bd7d07c7f8 1165 while(!gamepad.connect()) {
sayzyas 10:a2bd7d07c7f8 1166 flg_gamePad_Connected = 0;
sayzyas 10:a2bd7d07c7f8 1167 led2 = OFF;
sayzyas 10:a2bd7d07c7f8 1168 // led1 = 1;
sayzyas 10:a2bd7d07c7f8 1169 Thread::wait(500);
sayzyas 10:a2bd7d07c7f8 1170 }
sayzyas 10:a2bd7d07c7f8 1171
sayzyas 10:a2bd7d07c7f8 1172 // when connected, attach handler called on gamepad event
sayzyas 10:a2bd7d07c7f8 1173 gamepad.attachEvent(onControl);
sayzyas 10:a2bd7d07c7f8 1174
sayzyas 10:a2bd7d07c7f8 1175 // wait until the Gamepad is disconnected
sayzyas 10:a2bd7d07c7f8 1176 while(gamepad.connected()) {
sayzyas 10:a2bd7d07c7f8 1177 flg_gamePad_Connected = 1;
sayzyas 10:a2bd7d07c7f8 1178 led2 = !led2;
sayzyas 10:a2bd7d07c7f8 1179 led2 = ON;
sayzyas 10:a2bd7d07c7f8 1180 // led1 = 1;
sayzyas 10:a2bd7d07c7f8 1181
sayzyas 11:ff06edc0219c 1182 // Send status to Handy Ctrl controller, but currently this is only for Main Controller.
sayzyas 10:a2bd7d07c7f8 1183 I2C_cmd[4] = 0x00;
sayzyas 10:a2bd7d07c7f8 1184 I2C_cmd[5] = 0x01;
sayzyas 11:ff06edc0219c 1185 i2c.write(i2c_addr_handy, I2C_cmd, NumberOfI2CCommand);
sayzyas 11:ff06edc0219c 1186 // DEBUG_PRINT_L2("Bd0> Send Handy Controller(%02x) >>>>>>>>>>\r\n",i2c_addr_handy );
sayzyas 10:a2bd7d07c7f8 1187
sayzyas 10:a2bd7d07c7f8 1188 Thread::wait(500);
sayzyas 10:a2bd7d07c7f8 1189 }
sayzyas 10:a2bd7d07c7f8 1190 }
sayzyas 10:a2bd7d07c7f8 1191 }
sayzyas 10:a2bd7d07c7f8 1192
sayzyas 10:a2bd7d07c7f8 1193 // ======================================================================
sayzyas 10:a2bd7d07c7f8 1194 // Write setting value from lpcal file system of mbed
sayzyas 10:a2bd7d07c7f8 1195 // ======================================================================
sayzyas 10:a2bd7d07c7f8 1196 void write_Setting_lfs( void )
sayzyas 10:a2bd7d07c7f8 1197 {
sayzyas 10:a2bd7d07c7f8 1198 FILE *wfp;
sayzyas 10:a2bd7d07c7f8 1199
sayzyas 11:ff06edc0219c 1200 flg_mutex.lock();
sayzyas 11:ff06edc0219c 1201 DEBUG_PRINT_L3("\r\nBd0> Local file system write ... ");
sayzyas 10:a2bd7d07c7f8 1202 wfp = fopen("/local/set.dat", "wb"); // Open local file "set.txt" for writing
sayzyas 10:a2bd7d07c7f8 1203 if(!wfp){
sayzyas 11:ff06edc0219c 1204 DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
sayzyas 10:a2bd7d07c7f8 1205 }
sayzyas 10:a2bd7d07c7f8 1206 else{
sayzyas 10:a2bd7d07c7f8 1207 Thread::wait(200);
sayzyas 10:a2bd7d07c7f8 1208 fwrite( &setValue, sizeof( int8_t ),sizeof(setValue), wfp );
sayzyas 10:a2bd7d07c7f8 1209 Thread::wait(200);
sayzyas 10:a2bd7d07c7f8 1210 fclose(wfp);
sayzyas 10:a2bd7d07c7f8 1211
sayzyas 11:ff06edc0219c 1212 DEBUG_PRINT_L3("file writen\r\n");
sayzyas 10:a2bd7d07c7f8 1213 }
sayzyas 11:ff06edc0219c 1214 flg_mutex.unlock();
sayzyas 10:a2bd7d07c7f8 1215 }
sayzyas 10:a2bd7d07c7f8 1216
sayzyas 10:a2bd7d07c7f8 1217 // ======================================================================
sayzyas 10:a2bd7d07c7f8 1218 // Read setting value from lpcal file system of mbed
sayzyas 10:a2bd7d07c7f8 1219 // ======================================================================
sayzyas 10:a2bd7d07c7f8 1220 int read_Setting_lfs( void )
sayzyas 10:a2bd7d07c7f8 1221 {
sayzyas 10:a2bd7d07c7f8 1222 FILE *rfp;
sayzyas 11:ff06edc0219c 1223 int rts;
sayzyas 10:a2bd7d07c7f8 1224
sayzyas 11:ff06edc0219c 1225 //setValue_t lsetValue; // Setting Data
sayzyas 11:ff06edc0219c 1226
sayzyas 11:ff06edc0219c 1227 DEBUG_PRINT_L3("\r\nBd0> Read Setting data from local file system \r\n");
sayzyas 11:ff06edc0219c 1228 DEBUG_PRINT_L3( "Bd0> ---------------------------------------- \r\n");
sayzyas 10:a2bd7d07c7f8 1229 rfp = fopen("/local/set.dat", "rb"); // Open local file "set.txt" for writing
sayzyas 10:a2bd7d07c7f8 1230 if(!rfp){
sayzyas 11:ff06edc0219c 1231 DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
sayzyas 11:ff06edc0219c 1232 rts = _FAIL_;
sayzyas 10:a2bd7d07c7f8 1233 }
sayzyas 10:a2bd7d07c7f8 1234 else{
sayzyas 11:ff06edc0219c 1235 Thread::wait(100);
sayzyas 11:ff06edc0219c 1236 DEBUG_PRINT_L3("fread\r\n");
sayzyas 11:ff06edc0219c 1237 flg_mutex.lock();
sayzyas 10:a2bd7d07c7f8 1238 fread( (int8_t*)&setValue, sizeof(int8_t), sizeof(setValue), rfp );
sayzyas 11:ff06edc0219c 1239 DEBUG_PRINT_L3("fread done\r\n");
sayzyas 10:a2bd7d07c7f8 1240 dspSetValue2Console(&pc, &setValue);
sayzyas 11:ff06edc0219c 1241 flg_mutex.unlock();
sayzyas 10:a2bd7d07c7f8 1242
sayzyas 11:ff06edc0219c 1243 // Thread::wait(500);
sayzyas 11:ff06edc0219c 1244 DEBUG_PRINT_L3("fclose\r\n");
sayzyas 10:a2bd7d07c7f8 1245 fclose(rfp);
sayzyas 11:ff06edc0219c 1246 DEBUG_PRINT_L3("\r\n");
sayzyas 11:ff06edc0219c 1247
sayzyas 11:ff06edc0219c 1248 rts = _OK_;
sayzyas 11:ff06edc0219c 1249 }
sayzyas 11:ff06edc0219c 1250 flg_mutex.unlock();
sayzyas 11:ff06edc0219c 1251 return rts;
sayzyas 11:ff06edc0219c 1252 }
sayzyas 11:ff06edc0219c 1253
sayzyas 11:ff06edc0219c 1254 // ======================================================================
sayzyas 11:ff06edc0219c 1255 // Read setting value from lpcal file system of mbed
sayzyas 11:ff06edc0219c 1256 // ======================================================================
sayzyas 11:ff06edc0219c 1257 int read_Setting2_lfs( void )
sayzyas 11:ff06edc0219c 1258 {
sayzyas 11:ff06edc0219c 1259 FILE *rfp;
sayzyas 11:ff06edc0219c 1260 int rts;
sayzyas 11:ff06edc0219c 1261
sayzyas 11:ff06edc0219c 1262 // setValue_t lsetValue; // Setting Data
sayzyas 11:ff06edc0219c 1263
sayzyas 11:ff06edc0219c 1264 DEBUG_PRINT_L3("\r\nBd0> Read Setting data from local file system \r\n");
sayzyas 11:ff06edc0219c 1265 DEBUG_PRINT_L3( "Bd0> ---------------------------------------- \r\n");
sayzyas 11:ff06edc0219c 1266 rfp = fopen("/local/set.dat", "rb"); // Open local file "set.txt" for writing
sayzyas 11:ff06edc0219c 1267 if(!rfp){
sayzyas 11:ff06edc0219c 1268 DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
sayzyas 11:ff06edc0219c 1269 rts = _FAIL_;
sayzyas 11:ff06edc0219c 1270 }
sayzyas 11:ff06edc0219c 1271 else{
sayzyas 11:ff06edc0219c 1272 Thread::wait(100);
sayzyas 11:ff06edc0219c 1273 DEBUG_PRINT_L3("fread\r\n");
sayzyas 11:ff06edc0219c 1274 flg_mutex.lock();
sayzyas 11:ff06edc0219c 1275 fread( (int8_t*)&setValue, sizeof(int8_t), sizeof(setValue), rfp );
sayzyas 11:ff06edc0219c 1276 DEBUG_PRINT_L3("fread done\r\n");
sayzyas 11:ff06edc0219c 1277 dspSetValue2Console(&pc, &setValue);
sayzyas 11:ff06edc0219c 1278 flg_mutex.unlock();
sayzyas 11:ff06edc0219c 1279
sayzyas 11:ff06edc0219c 1280 // Thread::wait(500);
sayzyas 11:ff06edc0219c 1281 DEBUG_PRINT_L3("fclose\r\n");
sayzyas 11:ff06edc0219c 1282 fclose(rfp);
sayzyas 11:ff06edc0219c 1283 DEBUG_PRINT_L3("\r\n");
sayzyas 11:ff06edc0219c 1284
sayzyas 11:ff06edc0219c 1285 rts = _OK_;
sayzyas 11:ff06edc0219c 1286 }
sayzyas 11:ff06edc0219c 1287 flg_mutex.unlock();
sayzyas 11:ff06edc0219c 1288 return rts;
sayzyas 10:a2bd7d07c7f8 1289 }
sayzyas 10:a2bd7d07c7f8 1290
sayzyas 10:a2bd7d07c7f8 1291 // ======================================================================
sayzyas 10:a2bd7d07c7f8 1292 // Read Network setting value from lpcal file system of mbed
sayzyas 10:a2bd7d07c7f8 1293 // ======================================================================
sayzyas 10:a2bd7d07c7f8 1294 int read_NetSetting_lfs( char* ip_address, char* subnet_mask, char* gateway )
sayzyas 10:a2bd7d07c7f8 1295 {
sayzyas 10:a2bd7d07c7f8 1296 FILE *rfp;
sayzyas 10:a2bd7d07c7f8 1297 // char ip_address[20];
sayzyas 10:a2bd7d07c7f8 1298 // char subnet_mask[20];
sayzyas 10:a2bd7d07c7f8 1299 // char gateway[20];
sayzyas 10:a2bd7d07c7f8 1300
sayzyas 11:ff06edc0219c 1301 DEBUG_PRINT_L3("\r\nBd0> Read Network Setting data from local file system \r\n");
sayzyas 10:a2bd7d07c7f8 1302 rfp = fopen("/local/net.dat", "r"); // Open local file "set.txt" for writing
sayzyas 10:a2bd7d07c7f8 1303 if(!rfp){
sayzyas 11:ff06edc0219c 1304 DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
sayzyas 10:a2bd7d07c7f8 1305 return _FAIL_;
sayzyas 10:a2bd7d07c7f8 1306 }
sayzyas 10:a2bd7d07c7f8 1307 else{
sayzyas 10:a2bd7d07c7f8 1308 Thread::wait(500);
sayzyas 10:a2bd7d07c7f8 1309
sayzyas 10:a2bd7d07c7f8 1310 fscanf(rfp, "%s", ip_address);
sayzyas 10:a2bd7d07c7f8 1311 fscanf(rfp, "%s", subnet_mask);
sayzyas 10:a2bd7d07c7f8 1312 fscanf(rfp, "%s", gateway);
sayzyas 10:a2bd7d07c7f8 1313
sayzyas 11:ff06edc0219c 1314 DEBUG_PRINT_L3("Bd0> ---------------------------------------\r\n");
sayzyas 11:ff06edc0219c 1315 DEBUG_PRINT_L3("Bd0> ip address : %s\r\n", ip_address);
sayzyas 11:ff06edc0219c 1316 DEBUG_PRINT_L3("Bd0> subnet mask : %s\r\n", subnet_mask);
sayzyas 11:ff06edc0219c 1317 DEBUG_PRINT_L3("Bd0> default gateway: %s\r\n", gateway);
sayzyas 11:ff06edc0219c 1318 DEBUG_PRINT_L3("Bd0> ---------------------------------------\r\n");
sayzyas 10:a2bd7d07c7f8 1319
sayzyas 10:a2bd7d07c7f8 1320 Thread::wait(500);
sayzyas 10:a2bd7d07c7f8 1321 fclose(rfp);
sayzyas 11:ff06edc0219c 1322 DEBUG_PRINT_L3("\r\n");
sayzyas 10:a2bd7d07c7f8 1323
sayzyas 10:a2bd7d07c7f8 1324 return _OK_;
sayzyas 10:a2bd7d07c7f8 1325 }
sayzyas 10:a2bd7d07c7f8 1326 }
sayzyas 10:a2bd7d07c7f8 1327 // ======================================================================
sayzyas 10:a2bd7d07c7f8 1328 // Winch control function
sayzyas 10:a2bd7d07c7f8 1329 // ======================================================================
sayzyas 10:a2bd7d07c7f8 1330 void winchMovingControl(
sayzyas 10:a2bd7d07c7f8 1331 int mode, // Operationg mode: Relative / Abslute
sayzyas 10:a2bd7d07c7f8 1332 char* dbufferP, // Date buffer pointer
sayzyas 10:a2bd7d07c7f8 1333 int dbuffer_s, // Date buffer size
sayzyas 10:a2bd7d07c7f8 1334 winchData_t* winchDataP, // Winch data structure pointer
sayzyas 10:a2bd7d07c7f8 1335 int winchData_s, // Winch data structure size
sayzyas 10:a2bd7d07c7f8 1336 char* I2C_cmd
sayzyas 10:a2bd7d07c7f8 1337 ){
sayzyas 10:a2bd7d07c7f8 1338 int rcv_data_cnt;
sayzyas 10:a2bd7d07c7f8 1339 // int moving_data;
sayzyas 10:a2bd7d07c7f8 1340 int man_speed;
sayzyas 10:a2bd7d07c7f8 1341
sayzyas 10:a2bd7d07c7f8 1342 bool flg_stop_operation = false;
sayzyas 10:a2bd7d07c7f8 1343
sayzyas 10:a2bd7d07c7f8 1344 uint16_t winchCurrentPosition;
sayzyas 10:a2bd7d07c7f8 1345
sayzyas 10:a2bd7d07c7f8 1346 char I2C_read[NumberOfI2CCommand+1];
sayzyas 11:ff06edc0219c 1347 char I2C_readcmd[NumberOfI2CCommand+1];
sayzyas 10:a2bd7d07c7f8 1348
sayzyas 10:a2bd7d07c7f8 1349 // if (hwbtn_Opeflg == 1){
sayzyas 10:a2bd7d07c7f8 1350 // Thread::wait(1);
sayzyas 10:a2bd7d07c7f8 1351 // }
sayzyas 10:a2bd7d07c7f8 1352 // else{
sayzyas 10:a2bd7d07c7f8 1353
sayzyas 10:a2bd7d07c7f8 1354
sayzyas 10:a2bd7d07c7f8 1355 if( flg_ButtonOn == true ) {Thread::wait(2);}
sayzyas 10:a2bd7d07c7f8 1356
sayzyas 10:a2bd7d07c7f8 1357 I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
sayzyas 10:a2bd7d07c7f8 1358 I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
sayzyas 10:a2bd7d07c7f8 1359 I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
sayzyas 10:a2bd7d07c7f8 1360 I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
sayzyas 10:a2bd7d07c7f8 1361
sayzyas 10:a2bd7d07c7f8 1362 if( mode == WINCH_POSITION_CLEAR ){
sayzyas 10:a2bd7d07c7f8 1363 led3 = ON;
sayzyas 11:ff06edc0219c 1364 DEBUG_PRINT_L3("Bd0> === WINCH_POSITION_CLEAR ===\r\n");
sayzyas 10:a2bd7d07c7f8 1365 rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
sayzyas 11:ff06edc0219c 1366 DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas 10:a2bd7d07c7f8 1367 if( rcv_data_cnt < 0 ){
sayzyas 11:ff06edc0219c 1368 DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
sayzyas 10:a2bd7d07c7f8 1369 // tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas 10:a2bd7d07c7f8 1370 // break;
sayzyas 10:a2bd7d07c7f8 1371 }
sayzyas 10:a2bd7d07c7f8 1372 else{
sayzyas 11:ff06edc0219c 1373 DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
sayzyas 10:a2bd7d07c7f8 1374 // if( !strcmp( dbuffer, "WinchStepDnOf" ) ){
sayzyas 10:a2bd7d07c7f8 1375 // break;
sayzyas 10:a2bd7d07c7f8 1376 // }
sayzyas 10:a2bd7d07c7f8 1377 }
sayzyas 10:a2bd7d07c7f8 1378
sayzyas 10:a2bd7d07c7f8 1379 I2C_cmd[1] = 'Z'; // Zero clear
sayzyas 10:a2bd7d07c7f8 1380 i2c.write(i2c_addr_resolver, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1381
sayzyas 10:a2bd7d07c7f8 1382 led3 = OFF;
sayzyas 10:a2bd7d07c7f8 1383 }
sayzyas 10:a2bd7d07c7f8 1384 else if (( mode == WINCH_MMODE_RELATIVE ) || ( mode == WINCH_MMODE_ABSOLUTE )){
sayzyas 11:ff06edc0219c 1385 if ( mode == WINCH_MMODE_RELATIVE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_RELATIVE === \r\n");
sayzyas 11:ff06edc0219c 1386 if ( mode == WINCH_MMODE_ABSOLUTE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_ABSOLUTE === \r\n");
sayzyas 10:a2bd7d07c7f8 1387
sayzyas 10:a2bd7d07c7f8 1388 rcv_data_cnt = tcp_client.receive( dbuffer, dbuffer_s);
sayzyas 10:a2bd7d07c7f8 1389
sayzyas 10:a2bd7d07c7f8 1390 *(dbufferP+rcv_data_cnt) = '\0';
sayzyas 11:ff06edc0219c 1391 winchDataP->operation = '\r\n';
sayzyas 10:a2bd7d07c7f8 1392
sayzyas 11:ff06edc0219c 1393 DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
sayzyas 10:a2bd7d07c7f8 1394 // Copy received data to Winch data structure.
sayzyas 10:a2bd7d07c7f8 1395 memcpy( winchDataP, (winchData_t *)dbuffer, winchData_s);
sayzyas 10:a2bd7d07c7f8 1396 // winchDataP->dt_WinchDstPosition += winchDataP->dt_WinchCntPosition;
sayzyas 11:ff06edc0219c 1397 DEBUG_PRINT_L3("Bd0> Winch Rtv Move [ From %04d >>> To %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
sayzyas 10:a2bd7d07c7f8 1398
sayzyas 10:a2bd7d07c7f8 1399 swbtn_OpeMutex.lock();
sayzyas 10:a2bd7d07c7f8 1400 swbtn_Opeflg = 1;
sayzyas 10:a2bd7d07c7f8 1401 swbtn_OpeMutex.unlock();
sayzyas 10:a2bd7d07c7f8 1402
sayzyas 10:a2bd7d07c7f8 1403 while( true ){
sayzyas 10:a2bd7d07c7f8 1404 while( true ){
sayzyas 10:a2bd7d07c7f8 1405 led3 = ON;
sayzyas 10:a2bd7d07c7f8 1406 ////winchDataP->dt_WinchCntPosition = res_position; // Current position.
sayzyas 10:a2bd7d07c7f8 1407
sayzyas 11:ff06edc0219c 1408 DEBUG_PRINT_L3("Bd0> == Winch Position ==============\r\n");
sayzyas 11:ff06edc0219c 1409 DEBUG_PRINT_L3("Bd0> CURRENT : %d\r\n", winchDataP->dt_WinchCntPosition );
sayzyas 11:ff06edc0219c 1410 DEBUG_PRINT_L3("Bd0> DESTINATION: %d\r\n", winchDataP->dt_WinchDstPosition );
sayzyas 11:ff06edc0219c 1411 DEBUG_PRINT_L3("Bd0> ================================\r\n");
sayzyas 10:a2bd7d07c7f8 1412
sayzyas 10:a2bd7d07c7f8 1413 tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas 11:ff06edc0219c 1414 DEBUG_PRINT_L3("Bd0> Send Winch Rtv data [ %04d >>>> %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
sayzyas 10:a2bd7d07c7f8 1415
sayzyas 10:a2bd7d07c7f8 1416 rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
sayzyas 11:ff06edc0219c 1417 DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas 10:a2bd7d07c7f8 1418 if( rcv_data_cnt < 0 ){
sayzyas 11:ff06edc0219c 1419 DEBUG_PRINT_L3("##ERROR## Data receive\r\n" );
sayzyas 10:a2bd7d07c7f8 1420
sayzyas 10:a2bd7d07c7f8 1421 // tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas 10:a2bd7d07c7f8 1422 break;
sayzyas 10:a2bd7d07c7f8 1423 }
sayzyas 10:a2bd7d07c7f8 1424 else{
sayzyas 11:ff06edc0219c 1425 DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
sayzyas 10:a2bd7d07c7f8 1426 if( !strcmp( dbuffer, "WinchRtvStop" ) ){
sayzyas 10:a2bd7d07c7f8 1427 flg_stop_operation = true;
sayzyas 10:a2bd7d07c7f8 1428 break;
sayzyas 10:a2bd7d07c7f8 1429 }
sayzyas 10:a2bd7d07c7f8 1430 }
sayzyas 10:a2bd7d07c7f8 1431 if( winchDataP->dt_WinchCntPosition < winchDataP->dt_WinchDstPosition ){
sayzyas 10:a2bd7d07c7f8 1432 I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
sayzyas 10:a2bd7d07c7f8 1433 if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
sayzyas 10:a2bd7d07c7f8 1434 I2C_cmd[3] = setValue.winchCtrl.sv_WDM_mst; // Speed
sayzyas 10:a2bd7d07c7f8 1435 }
sayzyas 10:a2bd7d07c7f8 1436 else{
sayzyas 10:a2bd7d07c7f8 1437 I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_F; // Speed
sayzyas 10:a2bd7d07c7f8 1438 }
sayzyas 10:a2bd7d07c7f8 1439 }
sayzyas 10:a2bd7d07c7f8 1440 else{
sayzyas 10:a2bd7d07c7f8 1441 I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
sayzyas 10:a2bd7d07c7f8 1442 if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) < SLOWDOWN_DISTANCE ){
sayzyas 10:a2bd7d07c7f8 1443 I2C_cmd[3] = setValue.winchCtrl.sv_WDM_mrt;; // Speed
sayzyas 10:a2bd7d07c7f8 1444 }
sayzyas 10:a2bd7d07c7f8 1445 else{
sayzyas 10:a2bd7d07c7f8 1446 I2C_cmd[3] = setValue.winchCtrl.sv_WDM_sli_R; // Speed
sayzyas 10:a2bd7d07c7f8 1447 }
sayzyas 10:a2bd7d07c7f8 1448 }
sayzyas 10:a2bd7d07c7f8 1449
sayzyas 10:a2bd7d07c7f8 1450 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1451 // Read winch current position from Resolver.
sayzyas 10:a2bd7d07c7f8 1452 winchCurrentPosition = ReadWinchCurrentPosition( i2c_addr_resolver );
sayzyas 10:a2bd7d07c7f8 1453 winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas 10:a2bd7d07c7f8 1454 winchDataP->operation = 0x00;
sayzyas 10:a2bd7d07c7f8 1455 //i2c.read(i2c_addr_resolver, I2C_resdata, 2); // Read
sayzyas 10:a2bd7d07c7f8 1456 //res_position = (I2C_resdata[1] << 8) | I2C_resdata[0];
sayzyas 10:a2bd7d07c7f8 1457 // --------------------------------------
sayzyas 10:a2bd7d07c7f8 1458 // Read motor current
sayzyas 10:a2bd7d07c7f8 1459 // --------------------------------------
sayzyas 11:ff06edc0219c 1460 read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_read, 3 );
sayzyas 10:a2bd7d07c7f8 1461 winchDataP->dt_WinchMotor1Current = I2C_read[0]; // Motor current set
sayzyas 10:a2bd7d07c7f8 1462 winchDataP->dt_WinchMotor2Current = I2C_read[1]; // Motor current set
sayzyas 10:a2bd7d07c7f8 1463 winchDataP->operation = I2C_read[2];
sayzyas 11:ff06edc0219c 1464 DEBUG_PRINT_L3( "Bd0> 15: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
sayzyas 11:ff06edc0219c 1465 DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", i2c_addr_winch, I2C_read[0]);
sayzyas 10:a2bd7d07c7f8 1466 led3 = OFF;
sayzyas 10:a2bd7d07c7f8 1467 if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) <= 0 ){
sayzyas 10:a2bd7d07c7f8 1468 break;
sayzyas 10:a2bd7d07c7f8 1469 }
sayzyas 11:ff06edc0219c 1470 Thread::wait(2); // Time interval for program debugging
sayzyas 11:ff06edc0219c 1471 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1472 }
sayzyas 10:a2bd7d07c7f8 1473 I2C_cmd[2] = MOTOR_STP; // Motor1 STOP
sayzyas 10:a2bd7d07c7f8 1474 I2C_cmd[3] = 0; // Speed
sayzyas 10:a2bd7d07c7f8 1475 I2C_cmd[6] = MOTOR_STP; // Motor2 STOP
sayzyas 10:a2bd7d07c7f8 1476 I2C_cmd[7] = 0; // Speed
sayzyas 10:a2bd7d07c7f8 1477
sayzyas 10:a2bd7d07c7f8 1478 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1479 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1480
sayzyas 10:a2bd7d07c7f8 1481 Thread::wait(500); // Time interval for program debugging
sayzyas 10:a2bd7d07c7f8 1482
sayzyas 10:a2bd7d07c7f8 1483 winchCurrentPosition = ReadWinchCurrentPosition( i2c_addr_resolver );
sayzyas 10:a2bd7d07c7f8 1484 winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas 10:a2bd7d07c7f8 1485 winchDataP->operation = 0x00;
sayzyas 10:a2bd7d07c7f8 1486 //winchCurrentPosition = res_position; // Set curretn winch position that send for Host PC here when auto move mode.
sayzyas 11:ff06edc0219c 1487 DEBUG_PRINT_L3("Bd0> Check destination is same to setting point or not: %d\r\n", winchCurrentPosition);
sayzyas 10:a2bd7d07c7f8 1488 if( winchDataP->dt_WinchDstPosition == winchDataP->dt_WinchCntPosition ){
sayzyas 11:ff06edc0219c 1489 DEBUG_PRINT_L3("Bd0> Destination is same to setting point, then stop operation\r\n" );
sayzyas 10:a2bd7d07c7f8 1490 break; // When final destination == set point , then break. else adjust position again.
sayzyas 10:a2bd7d07c7f8 1491 }
sayzyas 10:a2bd7d07c7f8 1492 if( flg_stop_operation == true ){
sayzyas 11:ff06edc0219c 1493 DEBUG_PRINT_L3("Bd0> Winch auto operation force stop\r\n" );
sayzyas 10:a2bd7d07c7f8 1494 flg_stop_operation = false;
sayzyas 10:a2bd7d07c7f8 1495 break;
sayzyas 10:a2bd7d07c7f8 1496 }
sayzyas 10:a2bd7d07c7f8 1497 }
sayzyas 10:a2bd7d07c7f8 1498
sayzyas 10:a2bd7d07c7f8 1499 // Thread::wait(500); // Time interval for program debugging
sayzyas 10:a2bd7d07c7f8 1500
sayzyas 10:a2bd7d07c7f8 1501 winchCurrentPosition = ReadWinchCurrentPosition( i2c_addr_resolver );
sayzyas 10:a2bd7d07c7f8 1502 winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
sayzyas 10:a2bd7d07c7f8 1503 winchDataP->operation = 0x77;
sayzyas 11:ff06edc0219c 1504 DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchCurrentPosition);
sayzyas 10:a2bd7d07c7f8 1505
sayzyas 10:a2bd7d07c7f8 1506 tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas 10:a2bd7d07c7f8 1507
sayzyas 10:a2bd7d07c7f8 1508 swbtn_OpeMutex.lock();
sayzyas 10:a2bd7d07c7f8 1509 swbtn_Opeflg = 0;
sayzyas 10:a2bd7d07c7f8 1510 swbtn_OpeMutex.unlock();
sayzyas 10:a2bd7d07c7f8 1511
sayzyas 10:a2bd7d07c7f8 1512 led3 = OFF;
sayzyas 10:a2bd7d07c7f8 1513 }
sayzyas 10:a2bd7d07c7f8 1514
sayzyas 10:a2bd7d07c7f8 1515 // ----------------------------------------------------------
sayzyas 10:a2bd7d07c7f8 1516 // In case of commad received from PC by TCP connection.
sayzyas 10:a2bd7d07c7f8 1517 // In case of hard ware button pushed is by gamepad task
sayzyas 10:a2bd7d07c7f8 1518 // ----------------------------------------------------------
sayzyas 10:a2bd7d07c7f8 1519 else if (( mode == WINCH_STEPDOWN_BTN_ON )||( mode == WINCH_U_STEPDOWN_BTN_ON )) {
sayzyas 10:a2bd7d07c7f8 1520
sayzyas 11:ff06edc0219c 1521 if ( mode == WINCH_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPDOWN_BTN_ON ===\r\n" );
sayzyas 11:ff06edc0219c 1522 if ( mode == WINCH_U_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPDOWN_BTN_ON ===\r\n" );
sayzyas 10:a2bd7d07c7f8 1523
sayzyas 10:a2bd7d07c7f8 1524 swbtn_OpeMutex.lock();
sayzyas 10:a2bd7d07c7f8 1525 swbtn_Opeflg = 1;
sayzyas 10:a2bd7d07c7f8 1526 swbtn_OpeMutex.unlock();
sayzyas 10:a2bd7d07c7f8 1527 while( 1 ){
sayzyas 10:a2bd7d07c7f8 1528 led3 = ON;
sayzyas 11:ff06edc0219c 1529 DEBUG_PRINT_L3("Bd0> WINCH_STEPDOWN_BTN_ON\r\n");
sayzyas 10:a2bd7d07c7f8 1530 rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
sayzyas 11:ff06edc0219c 1531 DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas 10:a2bd7d07c7f8 1532 if( rcv_data_cnt < 0 ){
sayzyas 11:ff06edc0219c 1533 DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
sayzyas 10:a2bd7d07c7f8 1534 // tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas 10:a2bd7d07c7f8 1535 break;
sayzyas 10:a2bd7d07c7f8 1536 }
sayzyas 10:a2bd7d07c7f8 1537 else{
sayzyas 11:ff06edc0219c 1538 DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
sayzyas 10:a2bd7d07c7f8 1539 if(( !strcmp( dbuffer, "WinchStepDnOf" ))||( !strcmp( dbuffer, "WinchuStepDnOf" )) ){
sayzyas 10:a2bd7d07c7f8 1540 break;
sayzyas 10:a2bd7d07c7f8 1541 }
sayzyas 10:a2bd7d07c7f8 1542 }
sayzyas 10:a2bd7d07c7f8 1543
sayzyas 10:a2bd7d07c7f8 1544 if ( mode == WINCH_U_STEPDOWN_BTN_ON ) man_speed = 50;
sayzyas 10:a2bd7d07c7f8 1545 else man_speed = 100;
sayzyas 10:a2bd7d07c7f8 1546
sayzyas 10:a2bd7d07c7f8 1547 I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
sayzyas 10:a2bd7d07c7f8 1548 I2C_cmd[3] = man_speed; // Speed
sayzyas 10:a2bd7d07c7f8 1549
sayzyas 10:a2bd7d07c7f8 1550 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1551
sayzyas 10:a2bd7d07c7f8 1552 led3 = OFF;
sayzyas 10:a2bd7d07c7f8 1553
sayzyas 11:ff06edc0219c 1554 read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_read, 3 );
sayzyas 10:a2bd7d07c7f8 1555 winchDataP->dt_WinchMotor1Current = I2C_read[0];
sayzyas 10:a2bd7d07c7f8 1556 winchDataP->dt_WinchMotor2Current = I2C_read[1];
sayzyas 10:a2bd7d07c7f8 1557 winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
sayzyas 11:ff06edc0219c 1558 DEBUG_PRINT_L3( "Bd0> 16: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
sayzyas 10:a2bd7d07c7f8 1559 if( winchDataP->operation == 0x88 ){
sayzyas 10:a2bd7d07c7f8 1560 winchDataP->dt_WinchMotor1Current = 0xFF;
sayzyas 10:a2bd7d07c7f8 1561 }
sayzyas 11:ff06edc0219c 1562 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1563
sayzyas 11:ff06edc0219c 1564 DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", i2c_addr_winch, I2C_read[0]);
sayzyas 10:a2bd7d07c7f8 1565 winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
sayzyas 10:a2bd7d07c7f8 1566 winchDataP->dt_WinchCntPosition = winchCurrentPosition;
sayzyas 10:a2bd7d07c7f8 1567 winchDataP->operation = 0x00;
sayzyas 10:a2bd7d07c7f8 1568 // ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x00 );
sayzyas 10:a2bd7d07c7f8 1569 tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas 11:ff06edc0219c 1570 DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
sayzyas 11:ff06edc0219c 1571 // Thread::wait(2); // Time interval for program debugging
sayzyas 11:ff06edc0219c 1572 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1573 }
sayzyas 11:ff06edc0219c 1574 DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
sayzyas 10:a2bd7d07c7f8 1575 I2C_cmd[2] = MOTOR_STP; // Motor1 FWD
sayzyas 10:a2bd7d07c7f8 1576 I2C_cmd[3] = 0; // Speed
sayzyas 10:a2bd7d07c7f8 1577 I2C_cmd[6] = MOTOR_STP; // Motor2 FWD
sayzyas 10:a2bd7d07c7f8 1578 I2C_cmd[7] = 0; // Speed
sayzyas 10:a2bd7d07c7f8 1579
sayzyas 10:a2bd7d07c7f8 1580 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1581
sayzyas 11:ff06edc0219c 1582 DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
sayzyas 10:a2bd7d07c7f8 1583 winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
sayzyas 10:a2bd7d07c7f8 1584 winchDataP->dt_WinchCntPosition = winchCurrentPosition;
sayzyas 10:a2bd7d07c7f8 1585 winchDataP->operation = 0x77;
sayzyas 10:a2bd7d07c7f8 1586 //ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x77 );
sayzyas 10:a2bd7d07c7f8 1587 tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas 11:ff06edc0219c 1588 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1589
sayzyas 10:a2bd7d07c7f8 1590 swbtn_OpeMutex.lock();
sayzyas 10:a2bd7d07c7f8 1591 swbtn_Opeflg = 0;
sayzyas 10:a2bd7d07c7f8 1592 swbtn_OpeMutex.unlock();
sayzyas 10:a2bd7d07c7f8 1593 led3 = OFF;
sayzyas 10:a2bd7d07c7f8 1594 }
sayzyas 10:a2bd7d07c7f8 1595 // ----------------------------------------------------------
sayzyas 10:a2bd7d07c7f8 1596 // In case of commad received from PC by TCP connection.
sayzyas 10:a2bd7d07c7f8 1597 // In case of hard ware button pushed is by gamepad task
sayzyas 10:a2bd7d07c7f8 1598 // ----------------------------------------------------------
sayzyas 10:a2bd7d07c7f8 1599 else if (( mode == WINCH_STEPUP_BTN_ON )||( mode == WINCH_U_STEPUP_BTN_ON )) {
sayzyas 10:a2bd7d07c7f8 1600
sayzyas 11:ff06edc0219c 1601 if ( mode == WINCH_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPUP_BTN_ON ===\r\n" );
sayzyas 11:ff06edc0219c 1602 if ( mode == WINCH_U_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPUP_BTN_ON ===\r\n" );
sayzyas 10:a2bd7d07c7f8 1603
sayzyas 10:a2bd7d07c7f8 1604 swbtn_OpeMutex.lock();
sayzyas 10:a2bd7d07c7f8 1605 swbtn_Opeflg = 1;
sayzyas 10:a2bd7d07c7f8 1606 swbtn_OpeMutex.unlock();
sayzyas 10:a2bd7d07c7f8 1607 while( 1 ){
sayzyas 10:a2bd7d07c7f8 1608 led3 = ON;
sayzyas 11:ff06edc0219c 1609 DEBUG_PRINT_L3("Bd0> WINCH_STEPUP_BTN_ON\r\n");
sayzyas 10:a2bd7d07c7f8 1610 rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
sayzyas 11:ff06edc0219c 1611 DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
sayzyas 10:a2bd7d07c7f8 1612 if( rcv_data_cnt < 0 ){
sayzyas 11:ff06edc0219c 1613 DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
sayzyas 10:a2bd7d07c7f8 1614 // tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas 10:a2bd7d07c7f8 1615 break;
sayzyas 10:a2bd7d07c7f8 1616 }
sayzyas 10:a2bd7d07c7f8 1617 else{
sayzyas 11:ff06edc0219c 1618 DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
sayzyas 10:a2bd7d07c7f8 1619 if(
sayzyas 11:ff06edc0219c 1620 ( !strcmp( dbuffer, "WinchStepUpOf" ))||(!strcmp( dbuffer, "WinchuStepUpOf" )) ){
sayzyas 10:a2bd7d07c7f8 1621 break;
sayzyas 10:a2bd7d07c7f8 1622 }
sayzyas 10:a2bd7d07c7f8 1623 }
sayzyas 10:a2bd7d07c7f8 1624
sayzyas 10:a2bd7d07c7f8 1625 if ( mode == WINCH_U_STEPUP_BTN_ON ) man_speed = 50;
sayzyas 10:a2bd7d07c7f8 1626 else man_speed = 100;
sayzyas 10:a2bd7d07c7f8 1627
sayzyas 10:a2bd7d07c7f8 1628 I2C_cmd[2] = MOTOR_RVS; // Motor1 FWD
sayzyas 10:a2bd7d07c7f8 1629 I2C_cmd[3] = man_speed; // Speed
sayzyas 10:a2bd7d07c7f8 1630
sayzyas 10:a2bd7d07c7f8 1631 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1632
sayzyas 10:a2bd7d07c7f8 1633 led3 = OFF;
sayzyas 10:a2bd7d07c7f8 1634
sayzyas 11:ff06edc0219c 1635 read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_read, 3 );
sayzyas 10:a2bd7d07c7f8 1636 winchDataP->dt_WinchMotor1Current = I2C_read[0];
sayzyas 10:a2bd7d07c7f8 1637 winchDataP->dt_WinchMotor2Current = I2C_read[1];
sayzyas 10:a2bd7d07c7f8 1638 winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
sayzyas 11:ff06edc0219c 1639 DEBUG_PRINT_L3( "Bd0> 17: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
sayzyas 10:a2bd7d07c7f8 1640 if( winchDataP->operation == 0x88 ){
sayzyas 10:a2bd7d07c7f8 1641 winchDataP->dt_WinchMotor1Current = 0xFF;
sayzyas 10:a2bd7d07c7f8 1642 }
sayzyas 11:ff06edc0219c 1643 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1644
sayzyas 11:ff06edc0219c 1645 DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", i2c_addr_winch, I2C_read[0]);
sayzyas 10:a2bd7d07c7f8 1646 winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
sayzyas 10:a2bd7d07c7f8 1647 winchDataP->dt_WinchCntPosition = winchCurrentPosition;
sayzyas 10:a2bd7d07c7f8 1648 winchDataP->operation = 0x00;
sayzyas 10:a2bd7d07c7f8 1649 // ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x00 );
sayzyas 10:a2bd7d07c7f8 1650 tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas 11:ff06edc0219c 1651 DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
sayzyas 11:ff06edc0219c 1652 // Thread::wait(2); // Time interval for program debugging
sayzyas 11:ff06edc0219c 1653 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1654 }
sayzyas 11:ff06edc0219c 1655 DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
sayzyas 10:a2bd7d07c7f8 1656 I2C_cmd[2] = MOTOR_STP; // Motor1 FWD
sayzyas 10:a2bd7d07c7f8 1657 I2C_cmd[3] = 0; // Speed
sayzyas 10:a2bd7d07c7f8 1658 I2C_cmd[6] = MOTOR_STP; // Motor2 FWD
sayzyas 10:a2bd7d07c7f8 1659 I2C_cmd[7] = 0; // Speed
sayzyas 10:a2bd7d07c7f8 1660
sayzyas 10:a2bd7d07c7f8 1661 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 11:ff06edc0219c 1662
sayzyas 11:ff06edc0219c 1663 DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
sayzyas 10:a2bd7d07c7f8 1664 winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
sayzyas 10:a2bd7d07c7f8 1665 winchDataP->dt_WinchCntPosition = winchCurrentPosition;
sayzyas 10:a2bd7d07c7f8 1666 winchDataP->operation = 0x77;
sayzyas 10:a2bd7d07c7f8 1667 // ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x77 );
sayzyas 10:a2bd7d07c7f8 1668 tcp_client.send( (char*)winchDataP, winchData_s);
sayzyas 11:ff06edc0219c 1669 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
sayzyas 10:a2bd7d07c7f8 1670
sayzyas 10:a2bd7d07c7f8 1671 swbtn_OpeMutex.lock();
sayzyas 10:a2bd7d07c7f8 1672 swbtn_Opeflg = 0;
sayzyas 10:a2bd7d07c7f8 1673 swbtn_OpeMutex.unlock();
sayzyas 10:a2bd7d07c7f8 1674
sayzyas 10:a2bd7d07c7f8 1675 led3 = OFF;
sayzyas 10:a2bd7d07c7f8 1676 }
sayzyas 10:a2bd7d07c7f8 1677
sayzyas 10:a2bd7d07c7f8 1678 else {
sayzyas 11:ff06edc0219c 1679 // DEBUG_PRINT_L3("STEPSTEPSTEPSTEPSTEP\r\n");
sayzyas 10:a2bd7d07c7f8 1680 }
sayzyas 10:a2bd7d07c7f8 1681 // }
sayzyas 10:a2bd7d07c7f8 1682 }
sayzyas 10:a2bd7d07c7f8 1683
sayzyas 10:a2bd7d07c7f8 1684 // **********************************************************************
sayzyas 10:a2bd7d07c7f8 1685 //
sayzyas 10:a2bd7d07c7f8 1686 // Main Function of this program
sayzyas 10:a2bd7d07c7f8 1687 //
sayzyas 10:a2bd7d07c7f8 1688 // **********************************************************************
sayzyas 10:a2bd7d07c7f8 1689 int main()
sayzyas 10:a2bd7d07c7f8 1690 {
sayzyas 10:a2bd7d07c7f8 1691 winchData_t winchData;
sayzyas 10:a2bd7d07c7f8 1692
sayzyas 10:a2bd7d07c7f8 1693 char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
sayzyas 11:ff06edc0219c 1694 char I2C_res[NumberOfI2CCommand+1] = "\0";
sayzyas 10:a2bd7d07c7f8 1695
sayzyas 10:a2bd7d07c7f8 1696 char ip_address[20];;
sayzyas 10:a2bd7d07c7f8 1697 char subnet_mask[20];
sayzyas 10:a2bd7d07c7f8 1698 char gateway[20];
sayzyas 10:a2bd7d07c7f8 1699
sayzyas 10:a2bd7d07c7f8 1700 int ret;
sayzyas 10:a2bd7d07c7f8 1701 int try_cnt;
sayzyas 10:a2bd7d07c7f8 1702 int rcv_data_cnt;
sayzyas 10:a2bd7d07c7f8 1703
sayzyas 10:a2bd7d07c7f8 1704 bool flg_ethernet = false;
sayzyas 10:a2bd7d07c7f8 1705
sayzyas 10:a2bd7d07c7f8 1706 char ttt[20];
sayzyas 10:a2bd7d07c7f8 1707
sayzyas 10:a2bd7d07c7f8 1708 // Set UART(USB) baudrate
sayzyas 10:a2bd7d07c7f8 1709 pc.baud(115200);
sayzyas 10:a2bd7d07c7f8 1710
sayzyas 10:a2bd7d07c7f8 1711 cf_led_demo( &led1, &led2, &led3, &led4, 15, 35 );
sayzyas 10:a2bd7d07c7f8 1712
sayzyas 10:a2bd7d07c7f8 1713 DEBUG_PRINT_L0("\r\n");
sayzyas 11:ff06edc0219c 1714 DEBUG_PRINT_L0("Bd0> +--------------------------------------------------------------\r\n");
sayzyas 11:ff06edc0219c 1715 DEBUG_PRINT_L0("Bd0> | Project: B2 Crawler Explorer for 1F-1 PCV internal inspection\r\n");
sayzyas 11:ff06edc0219c 1716 DEBUG_PRINT_L0("Bd0> |---------\r\n");
sayzyas 11:ff06edc0219c 1717 DEBUG_PRINT_L0("Bd0> | This is: Main Control Program of Main Controller\r\n");
sayzyas 10:a2bd7d07c7f8 1718 DEBUG_PRINT_L0("Bd0> | Target MCU: mbed LPC1768\r\n");
sayzyas 10:a2bd7d07c7f8 1719 DEBUG_PRINT_L0("Bd0> | Letest update: %s\r\n", LatestUpDate);
sayzyas 10:a2bd7d07c7f8 1720 DEBUG_PRINT_L0("Bd0> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 10:a2bd7d07c7f8 1721 DEBUG_PRINT_L0("Bd0> | Author: %s\r\n", Author);
sayzyas 10:a2bd7d07c7f8 1722 DEBUG_PRINT_L0("Bd0> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
sayzyas 11:ff06edc0219c 1723 DEBUG_PRINT_L0("Bd0> ---------------------------------------------------------------\r\n");
sayzyas 10:a2bd7d07c7f8 1724 sprintf( ttt, "%s", ProgramRevision );
sayzyas 10:a2bd7d07c7f8 1725
sayzyas 11:ff06edc0219c 1726 DEBUG_PRINT_L0("Bd0> Start ststem initializing ...\r\n");
sayzyas 11:ff06edc0219c 1727 DEBUG_PRINT_L0("Bd0> ------------------------\r\n");
sayzyas 11:ff06edc0219c 1728 DEBUG_PRINT_L0("Bd0> Initalizing Ethernet ...\r\n");
sayzyas 11:ff06edc0219c 1729 DEBUG_PRINT_L0("Bd0> ------------------------\r\n");
sayzyas 10:a2bd7d07c7f8 1730
sayzyas 10:a2bd7d07c7f8 1731 read_NetSetting_lfs( ip_address, subnet_mask, gateway );
sayzyas 10:a2bd7d07c7f8 1732
sayzyas 11:ff06edc0219c 1733 DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
sayzyas 11:ff06edc0219c 1734 DEBUG_PRINT_L0("Bd0> ip address : %s\r\n", ip_address);
sayzyas 11:ff06edc0219c 1735 DEBUG_PRINT_L0("Bd0> subnet mask : %s\r\n", subnet_mask);
sayzyas 11:ff06edc0219c 1736 DEBUG_PRINT_L0("Bd0> default gateway: %s\r\n", gateway);
sayzyas 11:ff06edc0219c 1737 DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
sayzyas 10:a2bd7d07c7f8 1738
sayzyas 10:a2bd7d07c7f8 1739 #ifdef __ETERNET_DHCP__
sayzyas 10:a2bd7d07c7f8 1740 ret = eth.init(); // Use DHCP
sayzyas 10:a2bd7d07c7f8 1741 #else // __ETERNET_DHCP__
sayzyas 10:a2bd7d07c7f8 1742 ret = eth.init(
sayzyas 10:a2bd7d07c7f8 1743 ip_address, // const char* ip,
sayzyas 10:a2bd7d07c7f8 1744 subnet_mask, // const char* mask,
sayzyas 10:a2bd7d07c7f8 1745 gateway // const char* gateway
sayzyas 10:a2bd7d07c7f8 1746 );
sayzyas 10:a2bd7d07c7f8 1747 #endif // __ETERNET_DHCP__
sayzyas 10:a2bd7d07c7f8 1748 if( ret == 0 ){
sayzyas 11:ff06edc0219c 1749 DEBUG_PRINT_L0("Bd0> Eternet init ... OK\r\n");
sayzyas 10:a2bd7d07c7f8 1750 ret = eth.connect();
sayzyas 10:a2bd7d07c7f8 1751 if( ret == 0 ){
sayzyas 10:a2bd7d07c7f8 1752 cf_led_onoff( &led1,&led2,&led3,&led4, false, false, false, true );
sayzyas 11:ff06edc0219c 1753 DEBUG_PRINT_L0("Bd0> Eternat connect ... OK\r\n");
sayzyas 11:ff06edc0219c 1754 DEBUG_PRINT_L0("Bd0> [ IP Address : %s ]\r\n", eth.getIPAddress());
sayzyas 10:a2bd7d07c7f8 1755 udp_server.bind(UDP_SERVER_PORT);
sayzyas 10:a2bd7d07c7f8 1756 tcp_server.bind(TCP_SERVER_PORT);
sayzyas 10:a2bd7d07c7f8 1757 tcp_server.listen();
sayzyas 10:a2bd7d07c7f8 1758 flg_ethernet = true;
sayzyas 10:a2bd7d07c7f8 1759 }
sayzyas 10:a2bd7d07c7f8 1760 else{
sayzyas 10:a2bd7d07c7f8 1761 cf_led_error( &led1,&led2,&led3,&led4 );
sayzyas 11:ff06edc0219c 1762 DEBUG_PRINT_L0("Bd0> ***ERROR*** Eternat connect Fali\r\n");
sayzyas 10:a2bd7d07c7f8 1763 }
sayzyas 10:a2bd7d07c7f8 1764 }
sayzyas 10:a2bd7d07c7f8 1765 else{
sayzyas 11:ff06edc0219c 1766 DEBUG_PRINT_L0("Bd0> *** ERROR*** Eternat init Fail\r\n");
sayzyas 10:a2bd7d07c7f8 1767 }
sayzyas 10:a2bd7d07c7f8 1768
sayzyas 10:a2bd7d07c7f8 1769 Thread::wait(500);
sayzyas 10:a2bd7d07c7f8 1770
sayzyas 10:a2bd7d07c7f8 1771 //---------------------------------------------------
sayzyas 10:a2bd7d07c7f8 1772 // Read CrExp setting value from Local File System
sayzyas 10:a2bd7d07c7f8 1773 // setting file "SET.DAT".
sayzyas 10:a2bd7d07c7f8 1774 // When error occured, LED1 will be blinking shortly.
sayzyas 10:a2bd7d07c7f8 1775 //---------------------------------------------------
sayzyas 11:ff06edc0219c 1776 DEBUG_PRINT_L0("Bd0> ---------------------------\r\n");
sayzyas 11:ff06edc0219c 1777 DEBUG_PRINT_L0("Bd0> Read setting value from LFS\r\n");
sayzyas 11:ff06edc0219c 1778 DEBUG_PRINT_L0("Bd0> ---------------------------\r\n");
sayzyas 10:a2bd7d07c7f8 1779 try_cnt = LFS_READ_COUNT;
sayzyas 10:a2bd7d07c7f8 1780 while( 1 ){
sayzyas 10:a2bd7d07c7f8 1781 if( read_Setting_lfs() == _OK_ ) break;
sayzyas 10:a2bd7d07c7f8 1782 else try_cnt -= 1;
sayzyas 10:a2bd7d07c7f8 1783 if( try_cnt == 0 ){
sayzyas 11:ff06edc0219c 1784 DEBUG_PRINT_L0("Bd0> ***ERROR*** LFS read error\r\n");
sayzyas 10:a2bd7d07c7f8 1785 while(1){
sayzyas 10:a2bd7d07c7f8 1786 led1 = !led1;
sayzyas 10:a2bd7d07c7f8 1787 Thread::wait(30);
sayzyas 10:a2bd7d07c7f8 1788 }
sayzyas 10:a2bd7d07c7f8 1789 }
sayzyas 10:a2bd7d07c7f8 1790 }
sayzyas 11:ff06edc0219c 1791 DEBUG_PRINT_L0("Bd0> LFS read OK\r\n");
sayzyas 10:a2bd7d07c7f8 1792
sayzyas 10:a2bd7d07c7f8 1793 //---------------------------------------------------
sayzyas 10:a2bd7d07c7f8 1794 // Checking Targer LCXpresso824-MAX board here.
sayzyas 10:a2bd7d07c7f8 1795 // Send Hello Packet and waiting reply message from
sayzyas 10:a2bd7d07c7f8 1796 // target.
sayzyas 10:a2bd7d07c7f8 1797 // When error occured, LED1 will blinking slowly.
sayzyas 10:a2bd7d07c7f8 1798 //---------------------------------------------------
sayzyas 11:ff06edc0219c 1799 DEBUG_PRINT_L0("--------------------------\r\n");
sayzyas 11:ff06edc0219c 1800 DEBUG_PRINT_L0("Check the target controler\r\n");
sayzyas 11:ff06edc0219c 1801 DEBUG_PRINT_L0("--------------------------\r\n");
sayzyas 11:ff06edc0219c 1802
sayzyas 10:a2bd7d07c7f8 1803 try_cnt = TARGET_CHECK_COUNT;
sayzyas 10:a2bd7d07c7f8 1804 while(1){
sayzyas 10:a2bd7d07c7f8 1805 // **********************************************************************
sayzyas 10:a2bd7d07c7f8 1806 // **********************************************************************
sayzyas 10:a2bd7d07c7f8 1807 // Check each target motor control 824 board here
sayzyas 10:a2bd7d07c7f8 1808 // currently only one target checked for debugging ...
sayzyas 10:a2bd7d07c7f8 1809 // **********************************************************************
sayzyas 10:a2bd7d07c7f8 1810 // **********************************************************************
sayzyas 10:a2bd7d07c7f8 1811 I2C_cmd[1] = MOTOR_FWD;
sayzyas 10:a2bd7d07c7f8 1812 i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
sayzyas 10:a2bd7d07c7f8 1813 Thread::wait(100);
sayzyas 10:a2bd7d07c7f8 1814 i2c.read(i2c_addr_winch, I2C_res, NumberOfI2CCommand);
sayzyas 11:ff06edc0219c 1815 DEBUG_PRINT_L0("Bd0> Try count down: %d/%d\r\n", try_cnt, TARGET_CHECK_COUNT );
sayzyas 11:ff06edc0219c 1816 DEBUG_PRINT_L0("Bd0> Return from [ i2c address = 0x%2x : %s ]\r\n", i2c_addr_winch>>1, I2C_res);
sayzyas 11:ff06edc0219c 1817 DEBUG_PRINT_L0("Bd0> MCTR1[Winch motor controller] found\r\n");
sayzyas 10:a2bd7d07c7f8 1818 if( I2C_res[0] == 'S' ){
sayzyas 10:a2bd7d07c7f8 1819 break;
sayzyas 10:a2bd7d07c7f8 1820 }
sayzyas 10:a2bd7d07c7f8 1821 else try_cnt -= 1;
sayzyas 10:a2bd7d07c7f8 1822 if( try_cnt == 0 ){
sayzyas 11:ff06edc0219c 1823 DEBUG_PRINT_L0("Bd0> ##ERROR##\r\n");
sayzyas 10:a2bd7d07c7f8 1824 led1 = OFF;
sayzyas 10:a2bd7d07c7f8 1825 while(1){
sayzyas 10:a2bd7d07c7f8 1826 led1 = !led1; // ON
sayzyas 10:a2bd7d07c7f8 1827 Thread::wait(80);
sayzyas 10:a2bd7d07c7f8 1828 led1 = !led1; // OFF
sayzyas 10:a2bd7d07c7f8 1829 Thread::wait(80);
sayzyas 10:a2bd7d07c7f8 1830 led1 = !led1; // ON
sayzyas 10:a2bd7d07c7f8 1831 Thread::wait(80);
sayzyas 10:a2bd7d07c7f8 1832 led1 = !led1; // OFF
sayzyas 10:a2bd7d07c7f8 1833 Thread::wait(80);
sayzyas 10:a2bd7d07c7f8 1834 led1 = !led1; // ON
sayzyas 10:a2bd7d07c7f8 1835 Thread::wait(80);
sayzyas 10:a2bd7d07c7f8 1836 led1 = !led1; // OFF
sayzyas 10:a2bd7d07c7f8 1837 Thread::wait(80);
sayzyas 10:a2bd7d07c7f8 1838 led1 = !led1; // ON
sayzyas 10:a2bd7d07c7f8 1839 Thread::wait(80);
sayzyas 10:a2bd7d07c7f8 1840 led1 = !led1; // OFF
sayzyas 10:a2bd7d07c7f8 1841 Thread::wait(500);
sayzyas 10:a2bd7d07c7f8 1842 }
sayzyas 10:a2bd7d07c7f8 1843 }
sayzyas 10:a2bd7d07c7f8 1844 }
sayzyas 11:ff06edc0219c 1845 DEBUG_PRINT_L0("Bd0> Target system found\r\n");
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 }