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@16:1adb3f1e867d, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 04:21:06 2018 +0000
Revision:
16:1adb3f1e867d
Parent:
15:01680ed6b799 
2018.07.26

Who changed what in which revision?

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