z ysaito / Mbed 2 deprecated CPro_DemoB1B2_2_Handy

2018.07.26

Dependencies:   EthernetInterface TextLCD USBDevice USBHost2 mbed

P1_2Y_handy.cpp@1:392f3708c3e2, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:29:55 2018 +0000
Revision:
1:392f3708c3e2
Parent:
0:2cb1be240f50 
2018.07.26

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:2cb1be240f50 1 /*
sayzyas 0:2cb1be240f50 2 ////////////////////////////////////////////////////////////////////////////
sayzyas 0:2cb1be240f50 3 Project: B1/B2 DebrisProbe Demonstration model
sayzyas 0:2cb1be240f50 4 Title: Debris Prober demo HandyCtrl Main
sayzyas 0:2cb1be240f50 5 Target: mbed LPC1768
sayzyas 0:2cb1be240f50 6 Data: 2017.06.30
sayzyas 0:2cb1be240f50 7 Author: sayzyas as ZNR
sayzyas 0:2cb1be240f50 8 ------------------------------------------------
sayzyas 0:2cb1be240f50 9 The Final Project of this company ! .... may be.
sayzyas 0:2cb1be240f50 10 ------------------------------------------------
sayzyas 0:2cb1be240f50 11
sayzyas 0:2cb1be240f50 12 ## ##### ## #####
sayzyas 0:2cb1be240f50 13 ## ## ## ## ## ##
sayzyas 0:2cb1be240f50 14 ## ## ## ##### #####
sayzyas 0:2cb1be240f50 15 ## ## ## ## ## ##
sayzyas 0:2cb1be240f50 16 ## ## ##### #####
sayzyas 0:2cb1be240f50 17
sayzyas 0:2cb1be240f50 18 mbed LPC1768
sayzyas 0:2cb1be240f50 19 +-------------USB-----------+
sayzyas 0:2cb1be240f50 20 | GND VOUT(3.3V) |
sayzyas 0:2cb1be240f50 21 | VIN VU(5.0V OUT)|
sayzyas 0:2cb1be240f50 22 | VB IF- |
sayzyas 0:2cb1be240f50 23 | mR IF+ |
sayzyas 0:2cb1be240f50 24 | p5 mosi Ether RD- |
sayzyas 0:2cb1be240f50 25 | p6 miso Ether RD+ |
sayzyas 0:2cb1be240f50 26 | p7 sck Ether TD- |
sayzyas 0:2cb1be240f50 27 | p8 Ether TD+ |
sayzyas 0:2cb1be240f50 28 | p9 tx sdi USB D- |
sayzyas 0:2cb1be240f50 29 | p10 rx scl USB D+ |
sayzyas 0:2cb1be240f50 30 | p11 mosi CAN rd p30 |
sayzyas 0:2cb1be240f50 31 | p12 miso CAN td p29 |
sayzyas 0:2cb1be240f50 32 | p13 tx sck sda tx p28 |
sayzyas 0:2cb1be240f50 33 | p14 rx scl rx P27 |
sayzyas 0:2cb1be240f50 34 | p15 AIn PWM P26 |
sayzyas 0:2cb1be240f50 35 | p16 AIn PWM P25 |
sayzyas 0:2cb1be240f50 36 | p17 AIn PWM p24 |
sayzyas 0:2cb1be240f50 37 | p18 AIn AOut PWM p23 |
sayzyas 0:2cb1be240f50 38 | p19 AIn PWM p22 |
sayzyas 0:2cb1be240f50 39 | p20 AIn PWM p21 |
sayzyas 0:2cb1be240f50 40 +---------------------------+
sayzyas 0:2cb1be240f50 41
sayzyas 0:2cb1be240f50 42 ////////////////////////////////////////////////////////////////////////////
sayzyas 0:2cb1be240f50 43 */
sayzyas 0:2cb1be240f50 44
sayzyas 0:2cb1be240f50 45 #include "mbed.h"
sayzyas 0:2cb1be240f50 46 #include "USBHostGamepad.h"
sayzyas 0:2cb1be240f50 47 #include "USBSerial.h"
sayzyas 0:2cb1be240f50 48 #include "rtos.h"
sayzyas 0:2cb1be240f50 49 #include "EthernetInterface.h"
sayzyas 0:2cb1be240f50 50 #include "common.h"
sayzyas 0:2cb1be240f50 51 #include "stdio.h"
sayzyas 0:2cb1be240f50 52 //#include "TextLCD.h"
sayzyas 0:2cb1be240f50 53 #include "com_func.h"
sayzyas 0:2cb1be240f50 54 #include "USBHID.h"
sayzyas 0:2cb1be240f50 55 #include "TextLCD.h"
sayzyas 0:2cb1be240f50 56
sayzyas 0:2cb1be240f50 57
sayzyas 0:2cb1be240f50 58 // USBSerial serial setting
sayzyas 0:2cb1be240f50 59 Serial pc(USBTX, USBRX); // UART
sayzyas 0:2cb1be240f50 60
sayzyas 0:2cb1be240f50 61 // Digital I/O setting
sayzyas 0:2cb1be240f50 62 DigitalOut led1(LED1); // 1:on,0:off System is OK then ON.
sayzyas 0:2cb1be240f50 63 DigitalOut led2(LED2); // 1:on,0:off GamePad is connected.
sayzyas 0:2cb1be240f50 64 DigitalOut led3(LED3); // 1:on,0:off When got the GamePas switch input then ON
sayzyas 0:2cb1be240f50 65 DigitalOut led4(LED4); // 1:on,0:off Access indicator with PC
sayzyas 0:2cb1be240f50 66
sayzyas 0:2cb1be240f50 67 // LED indicator
sayzyas 0:2cb1be240f50 68 DigitalOut led_ind_ptwc(p14); // LED indicator pan/tilt, winch, crawler limit
sayzyas 0:2cb1be240f50 69 DigitalOut led_ind_tfm(p15); // LED indicator tfm motor current limit
sayzyas 0:2cb1be240f50 70 DigitalOut led_valid_part1(p25); // LED transform, crawler valid
sayzyas 0:2cb1be240f50 71 DigitalOut led_valid_part2(p26); // LED pan/tilt, winch valid
sayzyas 0:2cb1be240f50 72
sayzyas 0:2cb1be240f50 73 int16_t led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 74 int16_t led_ind_cnt_t = 0;
sayzyas 0:2cb1be240f50 75
sayzyas 0:2cb1be240f50 76 // Switch Matrix 3bit SW ON = 0 input
sayzyas 0:2cb1be240f50 77 // bit 2 1 0 Send data
sayzyas 0:2cb1be240f50 78 // 0 1 1 1 :
sayzyas 0:2cb1be240f50 79 // 1 1 1 0 : 0x01 RF Tfm K
sayzyas 0:2cb1be240f50 80 // 2 1 0 1 : 0x02 RF Tfm I
sayzyas 0:2cb1be240f50 81 // 3 1 0 0 : 0x04 LB Tfm K
sayzyas 0:2cb1be240f50 82 // 4 0 1 1 : 0x08 LB Tfm I
sayzyas 0:2cb1be240f50 83 // 5 0 1 0 : 0x10 Winch Down
sayzyas 0:2cb1be240f50 84 // 6 0 0 1 : 0x20 Winch Up
sayzyas 0:2cb1be240f50 85 // 7 0 0 0 :
sayzyas 0:2cb1be240f50 86 DigitalIn sw1(p5); // Bit 0
sayzyas 0:2cb1be240f50 87 DigitalIn sw2(p6); // Bit 1
sayzyas 0:2cb1be240f50 88 DigitalIn sw3(p7); // Bit 2
sayzyas 0:2cb1be240f50 89
sayzyas 0:2cb1be240f50 90 // Digital Input 1:OFF, 0:ON
sayzyas 0:2cb1be240f50 91 DigitalIn sw_JS_ShapeMode(p29); // SW: i \ k ,pde ( 1: K, 0: I )
sayzyas 0:2cb1be240f50 92 DigitalIn sw_validpart(p30); // SW: CrExp Shape ( 1: Left-Winch. 0: Right-tfmcrwler )
sayzyas 0:2cb1be240f50 93
sayzyas 0:2cb1be240f50 94 // Analig Joystick input for crawler control
sayzyas 0:2cb1be240f50 95 AnalogIn Rjsin_UD(p16); // Crawler R-UD
sayzyas 0:2cb1be240f50 96 AnalogIn Rjsin_LR(p17); // Crawler R-LR
sayzyas 0:2cb1be240f50 97 //AnalogIn Ljsin_UD(p19); // Crawler L-UD
sayzyas 0:2cb1be240f50 98 //AnalogIn Ljsin_LR(p20); // Crawler L-LR
sayzyas 0:2cb1be240f50 99
sayzyas 0:2cb1be240f50 100 const char* TCP_SERVER_ADDRESS = "192.168.0.24";
sayzyas 0:2cb1be240f50 101 // Ethernet
sayzyas 0:2cb1be240f50 102 EthernetInterface eth;
sayzyas 0:2cb1be240f50 103 // TCP
sayzyas 0:2cb1be240f50 104 TCPSocketConnection tsocket; // TCP client
sayzyas 0:2cb1be240f50 105 // UDP
sayzyas 0:2cb1be240f50 106 UDPSocket usocket; // UDP client
sayzyas 0:2cb1be240f50 107 Endpoint client;
sayzyas 0:2cb1be240f50 108
sayzyas 0:2cb1be240f50 109 // Local File System
sayzyas 0:2cb1be240f50 110 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
sayzyas 0:2cb1be240f50 111
sayzyas 0:2cb1be240f50 112 TextLCD lcd(p11, p12, p24, p23, p22, p21); // rs, e, d4-d7
sayzyas 0:2cb1be240f50 113
sayzyas 0:2cb1be240f50 114 uint8_t js_center_value_r_ud = 0x80;
sayzyas 0:2cb1be240f50 115 uint8_t js_center_value_r_lr = 0x80;
sayzyas 0:2cb1be240f50 116 uint8_t js_center_value_l_ud = 0x80;
sayzyas 0:2cb1be240f50 117 uint8_t js_center_value_l_lr = 0x80;
sayzyas 0:2cb1be240f50 118
sayzyas 0:2cb1be240f50 119 // Global
sayzyas 0:2cb1be240f50 120 uint32_t flg_gamePad_Connected = 0;
sayzyas 0:2cb1be240f50 121 char PC_cmd[11+1] = "&0100000000";
sayzyas 0:2cb1be240f50 122 basic_operation_t baseOperation;
sayzyas 0:2cb1be240f50 123 char dbuffer[128];
sayzyas 0:2cb1be240f50 124
sayzyas 0:2cb1be240f50 125 // Global Parameter of setting
sayzyas 0:2cb1be240f50 126 setValue_t setValue; // Setting Data
sayzyas 0:2cb1be240f50 127
sayzyas 0:2cb1be240f50 128 /* Status flag */
sayzyas 0:2cb1be240f50 129 /*
sayzyas 0:2cb1be240f50 130 0000 0000 : button LI LK RI RK PCW PCCW TU TD
sayzyas 0:2cb1be240f50 131 0000 0000 : limit switch
sayzyas 0:2cb1be240f50 132 0000 0000 : res
sayzyas 0:2cb1be240f50 133 0000 0000 : res
sayzyas 0:2cb1be240f50 134 */
sayzyas 0:2cb1be240f50 135 uint32_t flg_exp_status = 0;
sayzyas 0:2cb1be240f50 136 Mutex lcdMutex;
sayzyas 0:2cb1be240f50 137 Mutex flg_mutex;
sayzyas 0:2cb1be240f50 138 int swbtn_Opeflg = 0;
sayzyas 0:2cb1be240f50 139
sayzyas 0:2cb1be240f50 140 int16_t winchCurrentPosition;
sayzyas 0:2cb1be240f50 141 int16_t winchOffsetValue = 0; // 2016.10.07 added
sayzyas 0:2cb1be240f50 142 int16_t winchDramDiameter = 5985; // 2017.01.06 added
sayzyas 0:2cb1be240f50 143
sayzyas 0:2cb1be240f50 144 Mutex mtx_wcp;
sayzyas 0:2cb1be240f50 145
sayzyas 0:2cb1be240f50 146 bool flg_ButtonOn = false;
sayzyas 0:2cb1be240f50 147 bool flg_lsw_valid = false;
sayzyas 0:2cb1be240f50 148
sayzyas 0:2cb1be240f50 149 int flg_JS_shape_mode = 0;
sayzyas 0:2cb1be240f50 150 int flg_JS_ope_mode = 0;
sayzyas 0:2cb1be240f50 151 int motor1_current_pct;
sayzyas 0:2cb1be240f50 152 int motor2_current_pct;
sayzyas 0:2cb1be240f50 153 uint8_t limitSw_Sts = 0;
sayzyas 0:2cb1be240f50 154 char motorLock_sts = '\0';
sayzyas 0:2cb1be240f50 155
sayzyas 0:2cb1be240f50 156 void led_winch_valid( int counter )
sayzyas 0:2cb1be240f50 157 {
sayzyas 0:2cb1be240f50 158 if( counter < 25)
sayzyas 0:2cb1be240f50 159 {
sayzyas 0:2cb1be240f50 160 led_valid_part2 = 0;
sayzyas 0:2cb1be240f50 161 }
sayzyas 0:2cb1be240f50 162 else
sayzyas 0:2cb1be240f50 163 {
sayzyas 0:2cb1be240f50 164 led_valid_part2 = 1;
sayzyas 0:2cb1be240f50 165 }
sayzyas 0:2cb1be240f50 166 }
sayzyas 0:2cb1be240f50 167
sayzyas 0:2cb1be240f50 168 void led_crawler_valid( int counter )
sayzyas 0:2cb1be240f50 169 {
sayzyas 0:2cb1be240f50 170 if( counter < 25)
sayzyas 0:2cb1be240f50 171 {
sayzyas 0:2cb1be240f50 172 led_valid_part2 = 1;
sayzyas 0:2cb1be240f50 173 }
sayzyas 0:2cb1be240f50 174 else
sayzyas 0:2cb1be240f50 175 {
sayzyas 0:2cb1be240f50 176 led_valid_part2 = 0;
sayzyas 0:2cb1be240f50 177 }
sayzyas 0:2cb1be240f50 178 }
sayzyas 0:2cb1be240f50 179
sayzyas 0:2cb1be240f50 180 void lcd_dsp( int column, int row, char* msg, int cnt)
sayzyas 0:2cb1be240f50 181 {
sayzyas 0:2cb1be240f50 182 for( int i = 0; i < cnt; i++ )
sayzyas 0:2cb1be240f50 183 {
sayzyas 0:2cb1be240f50 184 lcd.locate(column+i,row);
sayzyas 0:2cb1be240f50 185 lcd.putc(*msg++);
sayzyas 0:2cb1be240f50 186 }
sayzyas 0:2cb1be240f50 187 Thread::wait(10);
sayzyas 0:2cb1be240f50 188 }
sayzyas 0:2cb1be240f50 189
sayzyas 0:2cb1be240f50 190 // ============================================================
sayzyas 0:2cb1be240f50 191 // Send Status to PC
sayzyas 0:2cb1be240f50 192 // ============================================================
sayzyas 0:2cb1be240f50 193 void sendStatus2PC( char *cmd, int32_t numberOfCmd ){
sayzyas 0:2cb1be240f50 194 int i;
sayzyas 0:2cb1be240f50 195 led4 = 1;
sayzyas 0:2cb1be240f50 196 for ( i = 0; i < numberOfCmd; i++ ) {
sayzyas 0:2cb1be240f50 197 pc.putc(*cmd++);
sayzyas 0:2cb1be240f50 198 }
sayzyas 0:2cb1be240f50 199 led4 = 0;
sayzyas 0:2cb1be240f50 200 }
sayzyas 0:2cb1be240f50 201
sayzyas 0:2cb1be240f50 202 uint8_t adj_crawlerSpeed( uint8_t in )
sayzyas 0:2cb1be240f50 203 {
sayzyas 0:2cb1be240f50 204 uint8_t out;
sayzyas 0:2cb1be240f50 205 /*
sayzyas 0:2cb1be240f50 206 pc.printf( "#### %d ####\r\n", in );
sayzyas 0:2cb1be240f50 207
sayzyas 0:2cb1be240f50 208 if( ( in > 0 ) && ( in < 98 ))
sayzyas 0:2cb1be240f50 209 {
sayzyas 0:2cb1be240f50 210 out = (uint8_t)((float)in * 0.8);
sayzyas 0:2cb1be240f50 211 }
sayzyas 0:2cb1be240f50 212 else
sayzyas 0:2cb1be240f50 213 {
sayzyas 0:2cb1be240f50 214 out = 100;
sayzyas 0:2cb1be240f50 215 }
sayzyas 0:2cb1be240f50 216 */
sayzyas 0:2cb1be240f50 217 out = in;
sayzyas 0:2cb1be240f50 218 return out;
sayzyas 0:2cb1be240f50 219 }
sayzyas 0:2cb1be240f50 220
sayzyas 0:2cb1be240f50 221
sayzyas 0:2cb1be240f50 222 bool cmd_snd2serverZ(
sayzyas 0:2cb1be240f50 223 char* cmd, // command send to server
sayzyas 0:2cb1be240f50 224 int cmdSize
sayzyas 0:2cb1be240f50 225 ){
sayzyas 0:2cb1be240f50 226 tsocket.send(cmd, cmdSize);
sayzyas 0:2cb1be240f50 227 DEBUG_PRINT_L0("Snd cmd 2 server [ %s (%d) ]\r\n", cmd, cmdSize );
sayzyas 0:2cb1be240f50 228 // Clean up
sayzyas 0:2cb1be240f50 229
sayzyas 0:2cb1be240f50 230 return true;
sayzyas 0:2cb1be240f50 231 }
sayzyas 0:2cb1be240f50 232
sayzyas 0:2cb1be240f50 233
sayzyas 0:2cb1be240f50 234 bool flg_mc_tfmcrw = false;
sayzyas 0:2cb1be240f50 235 bool flg_mc_winch = false;
sayzyas 0:2cb1be240f50 236 char I2C_res[NumberOfI2CCommand+1] = "\0";
sayzyas 0:2cb1be240f50 237
sayzyas 0:2cb1be240f50 238 // *****************************************************************
sayzyas 0:2cb1be240f50 239 // calibration joystick data
sayzyas 0:2cb1be240f50 240 // *****************************************************************
sayzyas 0:2cb1be240f50 241 bool calibrate_joystick( int js, uint8_t data_ud, uint8_t data_lr ){
sayzyas 0:2cb1be240f50 242 uint16_t tmp;
sayzyas 0:2cb1be240f50 243
sayzyas 0:2cb1be240f50 244 if( js == 0 ) // R-JS
sayzyas 0:2cb1be240f50 245 {
sayzyas 0:2cb1be240f50 246 tmp = (uint16_t)js_center_value_r_ud;
sayzyas 0:2cb1be240f50 247 tmp += (uint16_t)data_ud;
sayzyas 0:2cb1be240f50 248 tmp /= 2;
sayzyas 0:2cb1be240f50 249 js_center_value_r_ud = (uint8_t)tmp;
sayzyas 0:2cb1be240f50 250
sayzyas 0:2cb1be240f50 251 tmp = (uint16_t)js_center_value_r_lr;
sayzyas 0:2cb1be240f50 252 tmp += (uint16_t)data_lr;
sayzyas 0:2cb1be240f50 253 tmp /= 2;
sayzyas 0:2cb1be240f50 254 js_center_value_r_lr = (uint8_t)tmp;
sayzyas 0:2cb1be240f50 255
sayzyas 0:2cb1be240f50 256 if
sayzyas 0:2cb1be240f50 257 (
sayzyas 0:2cb1be240f50 258 (((127-10) < js_center_value_r_ud )&&(js_center_value_r_ud < (127+10))) &&
sayzyas 0:2cb1be240f50 259 (((127-10) < js_center_value_r_lr )&&(js_center_value_r_lr < (127+10)))
sayzyas 0:2cb1be240f50 260 )
sayzyas 0:2cb1be240f50 261 {
sayzyas 0:2cb1be240f50 262 return true;
sayzyas 0:2cb1be240f50 263 }
sayzyas 0:2cb1be240f50 264 else
sayzyas 0:2cb1be240f50 265 {
sayzyas 0:2cb1be240f50 266 return false;
sayzyas 0:2cb1be240f50 267 }
sayzyas 0:2cb1be240f50 268 }
sayzyas 0:2cb1be240f50 269 else // Other
sayzyas 0:2cb1be240f50 270 {
sayzyas 0:2cb1be240f50 271 tmp = (uint16_t)js_center_value_l_ud;
sayzyas 0:2cb1be240f50 272 tmp += (uint16_t)data_ud;
sayzyas 0:2cb1be240f50 273 tmp /= 2;
sayzyas 0:2cb1be240f50 274 js_center_value_l_ud = (uint8_t)tmp;
sayzyas 0:2cb1be240f50 275
sayzyas 0:2cb1be240f50 276 tmp = (uint16_t)js_center_value_l_lr;
sayzyas 0:2cb1be240f50 277 tmp += (uint16_t)data_lr;
sayzyas 0:2cb1be240f50 278 tmp /= 2;
sayzyas 0:2cb1be240f50 279 js_center_value_l_lr = (uint8_t)tmp;
sayzyas 0:2cb1be240f50 280
sayzyas 0:2cb1be240f50 281 if
sayzyas 0:2cb1be240f50 282 (
sayzyas 0:2cb1be240f50 283 (((127-10) < js_center_value_l_ud )&&(js_center_value_l_ud < (127+10))) &&
sayzyas 0:2cb1be240f50 284 (((127-10) < js_center_value_l_lr )&&(js_center_value_l_lr < (127+10)))
sayzyas 0:2cb1be240f50 285 ) {
sayzyas 0:2cb1be240f50 286 return true;
sayzyas 0:2cb1be240f50 287 }
sayzyas 0:2cb1be240f50 288 else
sayzyas 0:2cb1be240f50 289 {
sayzyas 0:2cb1be240f50 290 return false;
sayzyas 0:2cb1be240f50 291 }
sayzyas 0:2cb1be240f50 292
sayzyas 0:2cb1be240f50 293 }
sayzyas 0:2cb1be240f50 294 }
sayzyas 0:2cb1be240f50 295
sayzyas 0:2cb1be240f50 296 // LED current indicator blink control when
sayzyas 0:2cb1be240f50 297 // each motor is moving.
sayzyas 0:2cb1be240f50 298 void led_ind_run
sayzyas 0:2cb1be240f50 299 (
sayzyas 0:2cb1be240f50 300 int led_number,
sayzyas 0:2cb1be240f50 301 int count,
sayzyas 0:2cb1be240f50 302 int period
sayzyas 0:2cb1be240f50 303 ){
sayzyas 0:2cb1be240f50 304 if(( count >= 0 )&&( count < period ))
sayzyas 0:2cb1be240f50 305 {
sayzyas 0:2cb1be240f50 306 if( led_number == 1 )
sayzyas 0:2cb1be240f50 307 {
sayzyas 0:2cb1be240f50 308 led_ind_ptwc = ON;
sayzyas 0:2cb1be240f50 309 }
sayzyas 0:2cb1be240f50 310 else
sayzyas 0:2cb1be240f50 311 {
sayzyas 0:2cb1be240f50 312 led_ind_tfm = ON;
sayzyas 0:2cb1be240f50 313 }
sayzyas 0:2cb1be240f50 314 }
sayzyas 0:2cb1be240f50 315 else if(( count >= period )&&( count < period*2 ))
sayzyas 0:2cb1be240f50 316 {
sayzyas 0:2cb1be240f50 317 if( led_number == 1 )
sayzyas 0:2cb1be240f50 318 {
sayzyas 0:2cb1be240f50 319 led_ind_ptwc = OFF;
sayzyas 0:2cb1be240f50 320 }
sayzyas 0:2cb1be240f50 321 else
sayzyas 0:2cb1be240f50 322 {
sayzyas 0:2cb1be240f50 323 led_ind_tfm = OFF;
sayzyas 0:2cb1be240f50 324 }
sayzyas 0:2cb1be240f50 325 }
sayzyas 0:2cb1be240f50 326 else
sayzyas 0:2cb1be240f50 327 {
sayzyas 0:2cb1be240f50 328 led_ind_ptwc = OFF;
sayzyas 0:2cb1be240f50 329 led_ind_tfm = OFF;
sayzyas 0:2cb1be240f50 330 }
sayzyas 0:2cb1be240f50 331 }
sayzyas 0:2cb1be240f50 332
sayzyas 0:2cb1be240f50 333
sayzyas 0:2cb1be240f50 334 void led_error_blink( int no, int times )
sayzyas 0:2cb1be240f50 335 {
sayzyas 0:2cb1be240f50 336 for( int i = 0; i < times; i++ ) {
sayzyas 0:2cb1be240f50 337 if( no == 1 )
sayzyas 0:2cb1be240f50 338 {
sayzyas 0:2cb1be240f50 339 led_ind_ptwc = 1;
sayzyas 0:2cb1be240f50 340 }
sayzyas 0:2cb1be240f50 341 else
sayzyas 0:2cb1be240f50 342 {
sayzyas 0:2cb1be240f50 343 led_ind_tfm = 1;
sayzyas 0:2cb1be240f50 344 }
sayzyas 0:2cb1be240f50 345 Thread::wait(50);
sayzyas 0:2cb1be240f50 346 if( no == 1 )
sayzyas 0:2cb1be240f50 347 {
sayzyas 0:2cb1be240f50 348 led_ind_ptwc = 0;
sayzyas 0:2cb1be240f50 349 }
sayzyas 0:2cb1be240f50 350 else
sayzyas 0:2cb1be240f50 351 {
sayzyas 0:2cb1be240f50 352 led_ind_tfm = 0;
sayzyas 0:2cb1be240f50 353 }
sayzyas 0:2cb1be240f50 354 Thread::wait(50);
sayzyas 0:2cb1be240f50 355 }
sayzyas 0:2cb1be240f50 356 }
sayzyas 0:2cb1be240f50 357
sayzyas 0:2cb1be240f50 358 bool flg_B2 = false;
sayzyas 0:2cb1be240f50 359 bool flg_system_ok = true;
sayzyas 0:2cb1be240f50 360
sayzyas 0:2cb1be240f50 361 // *****************************************************************
sayzyas 0:2cb1be240f50 362 // switch control task
sayzyas 0:2cb1be240f50 363 // ----------------------
sayzyas 0:2cb1be240f50 364 //
sayzyas 0:2cb1be240f50 365 // Up: center -> 255
sayzyas 0:2cb1be240f50 366 // Down: 002 -> center
sayzyas 0:2cb1be240f50 367 // Right: center -> 255
sayzyas 0:2cb1be240f50 368 // Left : 0 -> center
sayzyas 0:2cb1be240f50 369 //
sayzyas 0:2cb1be240f50 370 // *****************************************************************
sayzyas 0:2cb1be240f50 371 int16_t tmpPosition;
sayzyas 0:2cb1be240f50 372 void sw_task( void const *){
sayzyas 0:2cb1be240f50 373
sayzyas 0:2cb1be240f50 374 char msg[16] = "";
sayzyas 0:2cb1be240f50 375
sayzyas 0:2cb1be240f50 376 uint16_t jsr_ud_data;
sayzyas 0:2cb1be240f50 377 uint16_t jsr_lr_data;
sayzyas 0:2cb1be240f50 378 uint8_t jsr_ud_undata;
sayzyas 0:2cb1be240f50 379 uint8_t jsr_lr_undata;
sayzyas 0:2cb1be240f50 380
sayzyas 0:2cb1be240f50 381 uint8_t m1_speed;
sayzyas 0:2cb1be240f50 382 uint8_t m2_speed;
sayzyas 0:2cb1be240f50 383 char sbuf[128] = ""; // TCP send buffer
sayzyas 0:2cb1be240f50 384 char rbuf[128]; // TCP receive buffer
sayzyas 0:2cb1be240f50 385 int flg_tfmcrwpart_1st = 0;
sayzyas 0:2cb1be240f50 386 int flg_wchcptpart_1st = 0;
sayzyas 0:2cb1be240f50 387
sayzyas 0:2cb1be240f50 388 int rts;
sayzyas 0:2cb1be240f50 389 bool jsjg = true;
sayzyas 0:2cb1be240f50 390
sayzyas 0:2cb1be240f50 391 bool led_tfm_lock = false;
sayzyas 0:2cb1be240f50 392 bool led_ptwc_lock = false;
sayzyas 0:2cb1be240f50 393
sayzyas 0:2cb1be240f50 394 int runcount = 0;
sayzyas 0:2cb1be240f50 395
sayzyas 0:2cb1be240f50 396 DEBUG_PRINT_L0("Handy(Bd2)>> Start ststem initializing ...\r\n");
sayzyas 0:2cb1be240f50 397 DEBUG_PRINT_L0("Handy(Bd2)>> =============================================================\r\n");
sayzyas 0:2cb1be240f50 398 DEBUG_PRINT_L0("Handy(Bd2)>> Initalizing Ethernet ...\r\n");
sayzyas 0:2cb1be240f50 399 DEBUG_PRINT_L0("Handy(Bd2)>> =============================================================\r\n");
sayzyas 0:2cb1be240f50 400
sayzyas 0:2cb1be240f50 401 const char* ip_address = "192.168.0.31";
sayzyas 0:2cb1be240f50 402 const char* subnet_mask = "255.255.255.0";
sayzyas 0:2cb1be240f50 403 const char* gateway = "192.168.0.0";
sayzyas 0:2cb1be240f50 404
sayzyas 0:2cb1be240f50 405 DEBUG_PRINT_L0("Handy(Bd2)> --------------------------------------\r\n");
sayzyas 0:2cb1be240f50 406 DEBUG_PRINT_L0("Handy(Bd2)> ip address : %s\r\n", ip_address);
sayzyas 0:2cb1be240f50 407 DEBUG_PRINT_L0("Handy(Bd2)> subnet mask : %s\r\n", subnet_mask);
sayzyas 0:2cb1be240f50 408 DEBUG_PRINT_L0("Handy(Bd2)> default gateway: %s\r\n", gateway);
sayzyas 0:2cb1be240f50 409 DEBUG_PRINT_L0("Handy(Bd2)> --------------------------------------\r\n");
sayzyas 0:2cb1be240f50 410
sayzyas 0:2cb1be240f50 411 char sss[24];
sayzyas 0:2cb1be240f50 412
sayzyas 0:2cb1be240f50 413 int ret = eth.init(
sayzyas 0:2cb1be240f50 414 ip_address, // const char* ip,
sayzyas 0:2cb1be240f50 415 subnet_mask, // const char* mask,
sayzyas 0:2cb1be240f50 416 gateway // const char* gateway
sayzyas 0:2cb1be240f50 417 );
sayzyas 0:2cb1be240f50 418 if( ret == 0 ){
sayzyas 0:2cb1be240f50 419
sayzyas 0:2cb1be240f50 420 DEBUG_PRINT_L0("Handy(Bd2)> Eternet init ... OK\r\n");
sayzyas 0:2cb1be240f50 421 ret = eth.connect(7000); // Connection time out: 7000msec
sayzyas 0:2cb1be240f50 422
sayzyas 0:2cb1be240f50 423 if( ret == 0 ){
sayzyas 0:2cb1be240f50 424 cf_led_onoff( &led1,&led2,&led3,&led4, false, false, false, true );
sayzyas 0:2cb1be240f50 425 DEBUG_PRINT_L0("Handy(Bd2)> [ IP Address : %s ]\r\n", eth.getIPAddress());
sayzyas 0:2cb1be240f50 426 while( true )
sayzyas 0:2cb1be240f50 427 {
sayzyas 0:2cb1be240f50 428 if (tsocket.connect(TCP_SERVER_ADDRESS, TCP_SERVER_PORT) < 0) {
sayzyas 0:2cb1be240f50 429 DEBUG_PRINT_L0("Handy(Bd2)> ###ERROR### : Unable to connect to (%s) on port (%d)\r\n", TCP_SERVER_ADDRESS, TCP_SERVER_PORT);
sayzyas 0:2cb1be240f50 430 flg_system_ok = false;
sayzyas 0:2cb1be240f50 431 wait(0.5);
sayzyas 0:2cb1be240f50 432 break;
sayzyas 0:2cb1be240f50 433 }
sayzyas 0:2cb1be240f50 434 flg_system_ok = true;
sayzyas 0:2cb1be240f50 435 sprintf(sss, "IP:%s", ip_address );
sayzyas 0:2cb1be240f50 436 lcdMutex.lock();
sayzyas 0:2cb1be240f50 437 lcd_dsp(0,0,"Network .. OK ",16);
sayzyas 0:2cb1be240f50 438 sss[15] = ' ';
sayzyas 0:2cb1be240f50 439 lcd_dsp(0,1,sss,16);
sayzyas 0:2cb1be240f50 440 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 441 sprintf( sbuf, "Hello" );
sayzyas 0:2cb1be240f50 442 DEBUG_PRINT_L0( "Handy(Bd2)> Send [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 443 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 444 wait_ms(5);
sayzyas 0:2cb1be240f50 445 // Receive winch position from host
sayzyas 0:2cb1be240f50 446 tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 447
sayzyas 0:2cb1be240f50 448 //
sayzyas 0:2cb1be240f50 449 // Check target (B1 or B2) here !
sayzyas 0:2cb1be240f50 450 //
sayzyas 0:2cb1be240f50 451 if( !strncmp( rbuf, "B2Demo", 6 ) )
sayzyas 0:2cb1be240f50 452 {
sayzyas 0:2cb1be240f50 453 DEBUG_PRINT_L0( "Handy(Bd2)> Echo back [%s]\r\n", rbuf );
sayzyas 0:2cb1be240f50 454 flg_B2 = true;
sayzyas 0:2cb1be240f50 455 }
sayzyas 0:2cb1be240f50 456 wait_ms(300);
sayzyas 0:2cb1be240f50 457
sayzyas 0:2cb1be240f50 458 DEBUG_PRINT_L1("Calibrating joystick ... ");
sayzyas 0:2cb1be240f50 459 for( int i = 0; i < CALIBRATION_COUNT; i++){
sayzyas 0:2cb1be240f50 460 jsr_ud_data = Rjsin_UD.read_u16();
sayzyas 0:2cb1be240f50 461 jsr_lr_data = Rjsin_LR.read_u16();
sayzyas 0:2cb1be240f50 462 jsr_ud_undata = (uint8_t)(jsr_ud_data >> 8);
sayzyas 0:2cb1be240f50 463 jsr_lr_undata = (uint8_t)(jsr_lr_data >> 8);
sayzyas 0:2cb1be240f50 464 jsjg = calibrate_joystick( 0, jsr_ud_undata, jsr_lr_undata); // Under constraction
sayzyas 0:2cb1be240f50 465 }
sayzyas 0:2cb1be240f50 466
sayzyas 0:2cb1be240f50 467 if( jsjg == false ){
sayzyas 0:2cb1be240f50 468 flg_system_ok = false;
sayzyas 0:2cb1be240f50 469 wait(0.5);
sayzyas 0:2cb1be240f50 470 break;
sayzyas 0:2cb1be240f50 471 }
sayzyas 0:2cb1be240f50 472
sayzyas 0:2cb1be240f50 473 DEBUG_PRINT_L1("done\r\n");
sayzyas 0:2cb1be240f50 474
sayzyas 0:2cb1be240f50 475 lcdMutex.lock();
sayzyas 0:2cb1be240f50 476 wait_ms(500);
sayzyas 0:2cb1be240f50 477 lcd_dsp(0,0,"System Boot up ",16);
sayzyas 0:2cb1be240f50 478
sayzyas 0:2cb1be240f50 479 if( flg_B2 == true )
sayzyas 0:2cb1be240f50 480 {
sayzyas 0:2cb1be240f50 481 sprintf( msg, "B2Wch [**** mm] "); // B2
sayzyas 0:2cb1be240f50 482 }
sayzyas 0:2cb1be240f50 483 else
sayzyas 0:2cb1be240f50 484 {
sayzyas 0:2cb1be240f50 485 sprintf( msg, "------ B1 ------"); // B1
sayzyas 0:2cb1be240f50 486 }
sayzyas 0:2cb1be240f50 487 lcdMutex.lock();
sayzyas 0:2cb1be240f50 488 lcd_dsp(0,0," ",16);
sayzyas 0:2cb1be240f50 489 lcd_dsp(0,1,msg,16);
sayzyas 0:2cb1be240f50 490 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 491
sayzyas 0:2cb1be240f50 492 while(1){
sayzyas 0:2cb1be240f50 493 jsr_ud_data = Rjsin_UD.read_u16();
sayzyas 0:2cb1be240f50 494 jsr_lr_data = Rjsin_LR.read_u16();
sayzyas 0:2cb1be240f50 495
sayzyas 0:2cb1be240f50 496 jsr_ud_undata = (uint8_t)(jsr_ud_data >> 8);
sayzyas 0:2cb1be240f50 497 jsr_lr_undata = (uint8_t)(jsr_lr_data >> 8);
sayzyas 0:2cb1be240f50 498
sayzyas 0:2cb1be240f50 499 DEBUG_PRINT_SW("Handy SW> ");
sayzyas 0:2cb1be240f50 500 DEBUG_PRINT_SW("R:%03d/%3d,%03d/%3d ", jsr_ud_undata, js_center_value_r_ud, jsr_lr_undata, js_center_value_r_ud );
sayzyas 0:2cb1be240f50 501 DEBUG_PRINT_SW("SW: %d %d %d ", sw1.read(), sw2.read(), sw3.read() );
sayzyas 0:2cb1be240f50 502 DEBUG_PRINT_SW("JS: %d %d\r\n", sw_JS_ShapeMode.read(), sw_validpart.read() );
sayzyas 0:2cb1be240f50 503
sayzyas 0:2cb1be240f50 504 // **********************************************************
sayzyas 0:2cb1be240f50 505 // Transform and crawler control part
sayzyas 0:2cb1be240f50 506 // **********************************************************
sayzyas 0:2cb1be240f50 507 if( sw_validpart == 0 ){ // sw on
sayzyas 0:2cb1be240f50 508 flg_wchcptpart_1st = 0;
sayzyas 0:2cb1be240f50 509 led_valid_part2 = 0;
sayzyas 0:2cb1be240f50 510 led_valid_part1 = 1;
sayzyas 0:2cb1be240f50 511 if( flg_tfmcrwpart_1st == 0 )
sayzyas 0:2cb1be240f50 512 {
sayzyas 0:2cb1be240f50 513 sprintf( sbuf, "OX_PART_000_0000" );
sayzyas 0:2cb1be240f50 514 DEBUG_PRINT_L0( "Handy(Bd2)> VALID PART [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 515 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 516 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 517 Thread::wait(5);
sayzyas 0:2cb1be240f50 518 flg_tfmcrwpart_1st = 1;
sayzyas 0:2cb1be240f50 519 }
sayzyas 0:2cb1be240f50 520 else
sayzyas 0:2cb1be240f50 521 {
sayzyas 0:2cb1be240f50 522 // ---------------------------------------------
sayzyas 0:2cb1be240f50 523 // Right JoyStick Control
sayzyas 0:2cb1be240f50 524 // ---------------------------------------------
sayzyas 0:2cb1be240f50 525 if( sw_JS_ShapeMode == 1 ){ // OFF: KO-Shape
sayzyas 0:2cb1be240f50 526
sayzyas 0:2cb1be240f50 527 if( // Dead zone
sayzyas 0:2cb1be240f50 528 ( jsr_ud_undata >= (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
sayzyas 0:2cb1be240f50 529 ( jsr_ud_undata <= (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
sayzyas 0:2cb1be240f50 530 ( jsr_lr_undata >= (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
sayzyas 0:2cb1be240f50 531 ( jsr_lr_undata <= (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
sayzyas 0:2cb1be240f50 532 ){
sayzyas 0:2cb1be240f50 533 sprintf( sbuf, "XX_CLRF_002_0000" ); // M1 Stop
sayzyas 0:2cb1be240f50 534 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 535 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 536 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 537 sprintf( sbuf, "XX_CLLB_002_0000" ); // M2 Stop
sayzyas 0:2cb1be240f50 538 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 539 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 540 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 541 lcdMutex.lock();
sayzyas 0:2cb1be240f50 542 if( runcount < 15 )
sayzyas 0:2cb1be240f50 543 {
sayzyas 0:2cb1be240f50 544 lcd_dsp(0,0,"OX [^] ",16);
sayzyas 0:2cb1be240f50 545 }
sayzyas 0:2cb1be240f50 546 else if ( runcount < 30 )
sayzyas 0:2cb1be240f50 547 {
sayzyas 0:2cb1be240f50 548 lcd_dsp(0,0,"OX [>] ",16);
sayzyas 0:2cb1be240f50 549 }
sayzyas 0:2cb1be240f50 550 else if(runcount < 45 )
sayzyas 0:2cb1be240f50 551 {
sayzyas 0:2cb1be240f50 552 lcd_dsp(0,0,"OX [v] ",16);
sayzyas 0:2cb1be240f50 553 }
sayzyas 0:2cb1be240f50 554 else{
sayzyas 0:2cb1be240f50 555 lcd_dsp(0,0,"OX [<] ",16);
sayzyas 0:2cb1be240f50 556 }
sayzyas 0:2cb1be240f50 557 if( runcount >= 60 )
sayzyas 0:2cb1be240f50 558 {
sayzyas 0:2cb1be240f50 559 runcount = 0;
sayzyas 0:2cb1be240f50 560 }
sayzyas 0:2cb1be240f50 561 else
sayzyas 0:2cb1be240f50 562 {
sayzyas 0:2cb1be240f50 563 runcount++;
sayzyas 0:2cb1be240f50 564 }
sayzyas 0:2cb1be240f50 565 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 566 // Receive winch position from host
sayzyas 0:2cb1be240f50 567 // rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 568 // if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 569 led4 = 0;
sayzyas 0:2cb1be240f50 570 led_ind_ptwc = 0;
sayzyas 0:2cb1be240f50 571 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 572 Thread::wait(15); // Don't delete, important!
sayzyas 0:2cb1be240f50 573 }
sayzyas 0:2cb1be240f50 574 else if( // Forward zone
sayzyas 0:2cb1be240f50 575 ( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
sayzyas 0:2cb1be240f50 576 ( jsr_lr_undata > (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
sayzyas 0:2cb1be240f50 577 ( jsr_lr_undata < (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
sayzyas 0:2cb1be240f50 578 ){
sayzyas 0:2cb1be240f50 579 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
sayzyas 0:2cb1be240f50 580 if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
sayzyas 0:2cb1be240f50 581 {
sayzyas 0:2cb1be240f50 582 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 583 }
sayzyas 0:2cb1be240f50 584 else
sayzyas 0:2cb1be240f50 585 {
sayzyas 0:2cb1be240f50 586 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 587 }
sayzyas 0:2cb1be240f50 588 led4 = ON;
sayzyas 0:2cb1be240f50 589 m1_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
sayzyas 0:2cb1be240f50 590 m2_speed = m1_speed;
sayzyas 0:2cb1be240f50 591 sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
sayzyas 0:2cb1be240f50 592 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
sayzyas 0:2cb1be240f50 593 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 594 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 595
sayzyas 0:2cb1be240f50 596 sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
sayzyas 0:2cb1be240f50 597 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 598 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 599 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 600
sayzyas 0:2cb1be240f50 601 DEBUG_PRINT_L2("Handy> R-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
sayzyas 0:2cb1be240f50 602 sprintf(msg, "OX Cr-F:%03d", (255 - jsr_ud_undata));
sayzyas 0:2cb1be240f50 603 lcdMutex.lock();
sayzyas 0:2cb1be240f50 604 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 605 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 606
sayzyas 0:2cb1be240f50 607 /*
sayzyas 0:2cb1be240f50 608 // Receive winch position from host
sayzyas 0:2cb1be240f50 609 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 610 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 611
sayzyas 0:2cb1be240f50 612 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 613 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 614 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 615 {
sayzyas 0:2cb1be240f50 616 DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 617 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 618 }
sayzyas 0:2cb1be240f50 619 */
sayzyas 0:2cb1be240f50 620 led4 = OFF;
sayzyas 0:2cb1be240f50 621 Thread::wait(15); // Don't delete, important!
sayzyas 0:2cb1be240f50 622 }
sayzyas 0:2cb1be240f50 623 else if( // Reverse zone
sayzyas 0:2cb1be240f50 624 ( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
sayzyas 0:2cb1be240f50 625 ( jsr_lr_undata > (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
sayzyas 0:2cb1be240f50 626 ( jsr_lr_undata < (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
sayzyas 0:2cb1be240f50 627 ){
sayzyas 0:2cb1be240f50 628 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
sayzyas 0:2cb1be240f50 629 if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
sayzyas 0:2cb1be240f50 630 {
sayzyas 0:2cb1be240f50 631 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 632 }
sayzyas 0:2cb1be240f50 633 else
sayzyas 0:2cb1be240f50 634 {
sayzyas 0:2cb1be240f50 635 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 636 }
sayzyas 0:2cb1be240f50 637 led4 = ON;
sayzyas 0:2cb1be240f50 638 m1_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
sayzyas 0:2cb1be240f50 639 m2_speed = m1_speed;
sayzyas 0:2cb1be240f50 640 sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
sayzyas 0:2cb1be240f50 641 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
sayzyas 0:2cb1be240f50 642 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 643 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 644
sayzyas 0:2cb1be240f50 645 sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
sayzyas 0:2cb1be240f50 646 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 647 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 648 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 649
sayzyas 0:2cb1be240f50 650 DEBUG_PRINT_L2("Handy> R-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
sayzyas 0:2cb1be240f50 651 sprintf(msg, "OX Cr-R:%03d", (255 - jsr_ud_undata));
sayzyas 0:2cb1be240f50 652 lcdMutex.lock();
sayzyas 0:2cb1be240f50 653 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 654 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 655 /*
sayzyas 0:2cb1be240f50 656 // Receive winch position from host
sayzyas 0:2cb1be240f50 657 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 658 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 659 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 660 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 661 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 662 {
sayzyas 0:2cb1be240f50 663 DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 664 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 665 }
sayzyas 0:2cb1be240f50 666 */
sayzyas 0:2cb1be240f50 667 led4 = OFF;
sayzyas 0:2cb1be240f50 668 Thread::wait(15); // Don't delete, important!
sayzyas 0:2cb1be240f50 669 }
sayzyas 0:2cb1be240f50 670
sayzyas 0:2cb1be240f50 671 else if( // Right turn zone
sayzyas 0:2cb1be240f50 672 ( jsr_lr_undata > (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone)) &&
sayzyas 0:2cb1be240f50 673 ( jsr_ud_undata > (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
sayzyas 0:2cb1be240f50 674 ( jsr_ud_undata < (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone))
sayzyas 0:2cb1be240f50 675 ){
sayzyas 0:2cb1be240f50 676 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
sayzyas 0:2cb1be240f50 677 if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
sayzyas 0:2cb1be240f50 678 {
sayzyas 0:2cb1be240f50 679 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 680 }
sayzyas 0:2cb1be240f50 681 else
sayzyas 0:2cb1be240f50 682 {
sayzyas 0:2cb1be240f50 683 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 684 }
sayzyas 0:2cb1be240f50 685 led4 = ON;
sayzyas 0:2cb1be240f50 686 m1_speed = ( jsr_lr_undata - js_center_value_r_lr ) * 100 / js_center_value_r_lr; // Speed ratio
sayzyas 0:2cb1be240f50 687 m2_speed = m1_speed; // Speed
sayzyas 0:2cb1be240f50 688 sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
sayzyas 0:2cb1be240f50 689 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
sayzyas 0:2cb1be240f50 690 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 691 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 692
sayzyas 0:2cb1be240f50 693 sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
sayzyas 0:2cb1be240f50 694 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 695 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 696 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 697
sayzyas 0:2cb1be240f50 698 DEBUG_PRINT_L2("Handy> R-JS Right: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_lr_undata, send_report.data[2], js_center_value_r_ud);
sayzyas 0:2cb1be240f50 699 sprintf(msg,"OX Cr->:%03d", jsr_lr_undata);
sayzyas 0:2cb1be240f50 700 lcdMutex.lock();
sayzyas 0:2cb1be240f50 701 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 702 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 703 /*
sayzyas 0:2cb1be240f50 704 // Receive winch position from host
sayzyas 0:2cb1be240f50 705 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 706 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 707 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 708 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 709 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 710 {
sayzyas 0:2cb1be240f50 711 DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 712 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 713 }
sayzyas 0:2cb1be240f50 714 */
sayzyas 0:2cb1be240f50 715 led4 = OFF;
sayzyas 0:2cb1be240f50 716 Thread::wait(15); // Don't delete, important!
sayzyas 0:2cb1be240f50 717 }
sayzyas 0:2cb1be240f50 718 else if( // Left turn zone
sayzyas 0:2cb1be240f50 719 ( jsr_lr_undata < (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
sayzyas 0:2cb1be240f50 720 ( jsr_ud_undata > (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
sayzyas 0:2cb1be240f50 721 ( jsr_ud_undata < (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone))
sayzyas 0:2cb1be240f50 722 ){
sayzyas 0:2cb1be240f50 723 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
sayzyas 0:2cb1be240f50 724 if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
sayzyas 0:2cb1be240f50 725 {
sayzyas 0:2cb1be240f50 726 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 727 }
sayzyas 0:2cb1be240f50 728 else
sayzyas 0:2cb1be240f50 729 {
sayzyas 0:2cb1be240f50 730 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 731 }
sayzyas 0:2cb1be240f50 732 led4 = ON;
sayzyas 0:2cb1be240f50 733 m1_speed = ( js_center_value_r_lr - jsr_lr_undata ) * 100 / js_center_value_r_lr; // Speed ratio
sayzyas 0:2cb1be240f50 734 m2_speed = m1_speed;
sayzyas 0:2cb1be240f50 735 sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
sayzyas 0:2cb1be240f50 736 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
sayzyas 0:2cb1be240f50 737 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 738 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 739
sayzyas 0:2cb1be240f50 740 sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
sayzyas 0:2cb1be240f50 741 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 742 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 743 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 744
sayzyas 0:2cb1be240f50 745 DEBUG_PRINT_L2("Handy> R-JS Left: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_lr_undata, send_report.data[2], js_center_value_r_ud);
sayzyas 0:2cb1be240f50 746 sprintf(msg,"OX Cr<-:%03d", jsr_lr_undata);
sayzyas 0:2cb1be240f50 747 lcdMutex.lock();
sayzyas 0:2cb1be240f50 748 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 749 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 750 /*
sayzyas 0:2cb1be240f50 751 // Receive winch position from host
sayzyas 0:2cb1be240f50 752 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 753 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 754 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 755 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 756 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 757 {
sayzyas 0:2cb1be240f50 758 DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 759 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 760 }
sayzyas 0:2cb1be240f50 761 */
sayzyas 0:2cb1be240f50 762 led4 = OFF;
sayzyas 0:2cb1be240f50 763 Thread::wait(15); // Don't delete, important!
sayzyas 0:2cb1be240f50 764 }
sayzyas 0:2cb1be240f50 765
sayzyas 0:2cb1be240f50 766 else if( // Right-upper zone
sayzyas 0:2cb1be240f50 767 ( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
sayzyas 0:2cb1be240f50 768 ( jsr_lr_undata > (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
sayzyas 0:2cb1be240f50 769 ){
sayzyas 0:2cb1be240f50 770 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
sayzyas 0:2cb1be240f50 771 if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
sayzyas 0:2cb1be240f50 772 {
sayzyas 0:2cb1be240f50 773 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 774 }
sayzyas 0:2cb1be240f50 775 else
sayzyas 0:2cb1be240f50 776 {
sayzyas 0:2cb1be240f50 777 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 778 }
sayzyas 0:2cb1be240f50 779 led4 = ON;
sayzyas 0:2cb1be240f50 780 if( jsr_ud_undata > jsr_lr_undata )
sayzyas 0:2cb1be240f50 781 {
sayzyas 0:2cb1be240f50 782 m2_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
sayzyas 0:2cb1be240f50 783 m1_speed = 100 - (( jsr_lr_undata - js_center_value_r_lr ) * 100 / js_center_value_r_lr);
sayzyas 0:2cb1be240f50 784 sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
sayzyas 0:2cb1be240f50 785 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
sayzyas 0:2cb1be240f50 786 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 787 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 788
sayzyas 0:2cb1be240f50 789 sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
sayzyas 0:2cb1be240f50 790 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 791 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 792 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 793 /*
sayzyas 0:2cb1be240f50 794 // Receive winch position from host
sayzyas 0:2cb1be240f50 795 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 796 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 797 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 798 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 799 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 800 {
sayzyas 0:2cb1be240f50 801 DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 802 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 803 }
sayzyas 0:2cb1be240f50 804 */
sayzyas 0:2cb1be240f50 805 }
sayzyas 0:2cb1be240f50 806 DEBUG_PRINT_L2("Handy> R-JS R-U zone: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
sayzyas 0:2cb1be240f50 807 sprintf(msg, "OX Cr-FRZ ");
sayzyas 0:2cb1be240f50 808 lcdMutex.lock();
sayzyas 0:2cb1be240f50 809 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 810 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 811 led4 = OFF;
sayzyas 0:2cb1be240f50 812 Thread::wait(15); // Don't delete, important!
sayzyas 0:2cb1be240f50 813 }
sayzyas 0:2cb1be240f50 814
sayzyas 0:2cb1be240f50 815 else if( // Left-upper zone
sayzyas 0:2cb1be240f50 816 ( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
sayzyas 0:2cb1be240f50 817 ( jsr_lr_undata < (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone))
sayzyas 0:2cb1be240f50 818 ){
sayzyas 0:2cb1be240f50 819 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
sayzyas 0:2cb1be240f50 820 if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
sayzyas 0:2cb1be240f50 821 {
sayzyas 0:2cb1be240f50 822 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 823 }
sayzyas 0:2cb1be240f50 824 else
sayzyas 0:2cb1be240f50 825 {
sayzyas 0:2cb1be240f50 826 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 827 }
sayzyas 0:2cb1be240f50 828 led4 = ON;
sayzyas 0:2cb1be240f50 829 if( (jsr_ud_undata - js_center_value_r_ud) > (js_center_value_r_ud-jsr_lr_undata) )
sayzyas 0:2cb1be240f50 830 {
sayzyas 0:2cb1be240f50 831 m1_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
sayzyas 0:2cb1be240f50 832 m2_speed = 100 - (( js_center_value_r_lr - jsr_lr_undata ) * 100 / js_center_value_r_lr);
sayzyas 0:2cb1be240f50 833 sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
sayzyas 0:2cb1be240f50 834 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
sayzyas 0:2cb1be240f50 835 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 836 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 837
sayzyas 0:2cb1be240f50 838 sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
sayzyas 0:2cb1be240f50 839 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 840 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 841 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 842 /*
sayzyas 0:2cb1be240f50 843 // Receive winch position from host
sayzyas 0:2cb1be240f50 844 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 845 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 846
sayzyas 0:2cb1be240f50 847 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 848 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 849 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 850 {
sayzyas 0:2cb1be240f50 851 DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 852 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 853 }
sayzyas 0:2cb1be240f50 854 */
sayzyas 0:2cb1be240f50 855 }
sayzyas 0:2cb1be240f50 856 DEBUG_PRINT_L2("Handy> R-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
sayzyas 0:2cb1be240f50 857 sprintf(msg, "OX Cr-FLZ ");
sayzyas 0:2cb1be240f50 858 lcdMutex.lock();
sayzyas 0:2cb1be240f50 859 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 860 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 861 led4 = OFF;
sayzyas 0:2cb1be240f50 862 Thread::wait(15); // Don't delete, important!
sayzyas 0:2cb1be240f50 863 }
sayzyas 0:2cb1be240f50 864
sayzyas 0:2cb1be240f50 865 else if( // Right-lower zone
sayzyas 0:2cb1be240f50 866 ( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
sayzyas 0:2cb1be240f50 867 ( jsr_lr_undata > (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
sayzyas 0:2cb1be240f50 868 ){
sayzyas 0:2cb1be240f50 869 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
sayzyas 0:2cb1be240f50 870 if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
sayzyas 0:2cb1be240f50 871 {
sayzyas 0:2cb1be240f50 872 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 873 }
sayzyas 0:2cb1be240f50 874 else
sayzyas 0:2cb1be240f50 875 {
sayzyas 0:2cb1be240f50 876 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 877 }
sayzyas 0:2cb1be240f50 878 led4 = ON;
sayzyas 0:2cb1be240f50 879 if( (js_center_value_r_ud - jsr_ud_undata) > (jsr_lr_undata - js_center_value_r_lr) )
sayzyas 0:2cb1be240f50 880 {
sayzyas 0:2cb1be240f50 881 m2_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
sayzyas 0:2cb1be240f50 882 m1_speed = 100 - (( jsr_lr_undata - js_center_value_r_lr ) * 100 / js_center_value_r_lr);
sayzyas 0:2cb1be240f50 883 sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
sayzyas 0:2cb1be240f50 884 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
sayzyas 0:2cb1be240f50 885 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 886 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 887
sayzyas 0:2cb1be240f50 888 sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
sayzyas 0:2cb1be240f50 889 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 890 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 891 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 892 /*
sayzyas 0:2cb1be240f50 893 // Receive winch position from host
sayzyas 0:2cb1be240f50 894 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 895 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 896
sayzyas 0:2cb1be240f50 897 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 898 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 899 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 900 {
sayzyas 0:2cb1be240f50 901 DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 902 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 903 }
sayzyas 0:2cb1be240f50 904 */
sayzyas 0:2cb1be240f50 905 }
sayzyas 0:2cb1be240f50 906 DEBUG_PRINT_L2("Handy> R-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
sayzyas 0:2cb1be240f50 907 sprintf(msg, "OX Cr-RRZ ");
sayzyas 0:2cb1be240f50 908 //sprintf(msg, "OX C BK RT ");
sayzyas 0:2cb1be240f50 909 lcdMutex.lock();
sayzyas 0:2cb1be240f50 910 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 911 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 912 led4 = OFF;
sayzyas 0:2cb1be240f50 913 Thread::wait(15); // Don't delete, important!
sayzyas 0:2cb1be240f50 914 }
sayzyas 0:2cb1be240f50 915
sayzyas 0:2cb1be240f50 916 else if( // Left-lower zone
sayzyas 0:2cb1be240f50 917 ( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
sayzyas 0:2cb1be240f50 918 ( jsr_lr_undata < (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone))
sayzyas 0:2cb1be240f50 919 ){
sayzyas 0:2cb1be240f50 920 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
sayzyas 0:2cb1be240f50 921 if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
sayzyas 0:2cb1be240f50 922 {
sayzyas 0:2cb1be240f50 923 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 924 }
sayzyas 0:2cb1be240f50 925 else
sayzyas 0:2cb1be240f50 926 {
sayzyas 0:2cb1be240f50 927 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 928 }
sayzyas 0:2cb1be240f50 929 led4 = ON;
sayzyas 0:2cb1be240f50 930 if( (js_center_value_r_ud - jsr_ud_undata) > ( js_center_value_r_lr - jsr_lr_undata) )
sayzyas 0:2cb1be240f50 931 {
sayzyas 0:2cb1be240f50 932 m1_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
sayzyas 0:2cb1be240f50 933 m2_speed = 100 - (( js_center_value_r_lr - jsr_lr_undata ) * 100 / js_center_value_r_lr);
sayzyas 0:2cb1be240f50 934 sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
sayzyas 0:2cb1be240f50 935 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
sayzyas 0:2cb1be240f50 936 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 937
sayzyas 0:2cb1be240f50 938 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 939 sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
sayzyas 0:2cb1be240f50 940 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 941 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 942 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 943 /*
sayzyas 0:2cb1be240f50 944 // Receive winch position from host
sayzyas 0:2cb1be240f50 945 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 946 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 947
sayzyas 0:2cb1be240f50 948 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 949 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 950 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 951 {
sayzyas 0:2cb1be240f50 952 DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 953 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 954 }
sayzyas 0:2cb1be240f50 955 */
sayzyas 0:2cb1be240f50 956 }
sayzyas 0:2cb1be240f50 957 DEBUG_PRINT_L2("Handy> R-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
sayzyas 0:2cb1be240f50 958 sprintf(msg, "OX Cr-RLZ ");
sayzyas 0:2cb1be240f50 959 lcdMutex.lock();
sayzyas 0:2cb1be240f50 960 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 961 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 962 led4 = OFF;
sayzyas 0:2cb1be240f50 963 Thread::wait(15); // Don't delete, important!
sayzyas 0:2cb1be240f50 964 }
sayzyas 0:2cb1be240f50 965 }
sayzyas 0:2cb1be240f50 966 else // ON: I-Shape
sayzyas 0:2cb1be240f50 967 {
sayzyas 0:2cb1be240f50 968 // Right Joystic < Down
sayzyas 0:2cb1be240f50 969 if( jsr_ud_undata > (js_center_value_r_ud + + setValue.jsCtrl.rjs_upper_dzone) ){
sayzyas 0:2cb1be240f50 970 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
sayzyas 0:2cb1be240f50 971 if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
sayzyas 0:2cb1be240f50 972 {
sayzyas 0:2cb1be240f50 973 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 974 }
sayzyas 0:2cb1be240f50 975 else
sayzyas 0:2cb1be240f50 976 {
sayzyas 0:2cb1be240f50 977 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 978 }
sayzyas 0:2cb1be240f50 979 led4 = ON;
sayzyas 0:2cb1be240f50 980 m1_speed = adj_crawlerSpeed(( js_center_value_r_ud+1 - (255 - jsr_ud_undata) ) * 100 / js_center_value_r_ud ); // Speed
sayzyas 0:2cb1be240f50 981 m2_speed = m1_speed; // Speed
sayzyas 0:2cb1be240f50 982
sayzyas 0:2cb1be240f50 983 sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
sayzyas 0:2cb1be240f50 984 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
sayzyas 0:2cb1be240f50 985 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 986 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 987
sayzyas 0:2cb1be240f50 988 sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
sayzyas 0:2cb1be240f50 989 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 990 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 991 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 992
sayzyas 0:2cb1be240f50 993 DEBUG_PRINT_L2("Handy> R-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
sayzyas 0:2cb1be240f50 994 sprintf(msg, "OX C FW:%03d", (255 - jsr_ud_undata));
sayzyas 0:2cb1be240f50 995 lcdMutex.lock();
sayzyas 0:2cb1be240f50 996 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 997 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 998 /*
sayzyas 0:2cb1be240f50 999 // Receive winch position from host
sayzyas 0:2cb1be240f50 1000 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1001 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1002
sayzyas 0:2cb1be240f50 1003 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1004 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1005 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 1006 {
sayzyas 0:2cb1be240f50 1007 DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 1008 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 1009 }
sayzyas 0:2cb1be240f50 1010 */
sayzyas 0:2cb1be240f50 1011 led4 = OFF;
sayzyas 0:2cb1be240f50 1012 Thread::wait(15); // Don't delete, important!
sayzyas 0:2cb1be240f50 1013 }
sayzyas 0:2cb1be240f50 1014 // Right Joystick < Up >
sayzyas 0:2cb1be240f50 1015 else if( jsr_ud_undata < (js_center_value_r_ud - + setValue.jsCtrl.rjs_upper_dzone) ){
sayzyas 0:2cb1be240f50 1016 // Crawler reverse Run
sayzyas 0:2cb1be240f50 1017 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
sayzyas 0:2cb1be240f50 1018 if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
sayzyas 0:2cb1be240f50 1019 {
sayzyas 0:2cb1be240f50 1020 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 1021 }
sayzyas 0:2cb1be240f50 1022 else
sayzyas 0:2cb1be240f50 1023 {
sayzyas 0:2cb1be240f50 1024 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 1025 }
sayzyas 0:2cb1be240f50 1026 led4 = ON;
sayzyas 0:2cb1be240f50 1027 m1_speed = adj_crawlerSpeed(( (255 - jsr_ud_undata) - js_center_value_r_ud ) * 100 / js_center_value_r_ud ); // Speed
sayzyas 0:2cb1be240f50 1028 m2_speed = m1_speed; // Speed
sayzyas 0:2cb1be240f50 1029 sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
sayzyas 0:2cb1be240f50 1030 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
sayzyas 0:2cb1be240f50 1031 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1032 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1033
sayzyas 0:2cb1be240f50 1034 sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
sayzyas 0:2cb1be240f50 1035 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1036 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1037 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1038
sayzyas 0:2cb1be240f50 1039 DEBUG_PRINT_L2("Handy> R-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", jsr_ud_undata, send_report.data[3], js_center_value_r_ud);
sayzyas 0:2cb1be240f50 1040 sprintf(msg, "OX C BK:%03d", (255 - jsr_ud_undata));
sayzyas 0:2cb1be240f50 1041
sayzyas 0:2cb1be240f50 1042 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1043 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 1044 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1045 /*
sayzyas 0:2cb1be240f50 1046 // Receive winch position from host
sayzyas 0:2cb1be240f50 1047 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1048 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1049
sayzyas 0:2cb1be240f50 1050 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1051 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1052 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 1053 {
sayzyas 0:2cb1be240f50 1054 DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 1055 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 1056 }
sayzyas 0:2cb1be240f50 1057 */
sayzyas 0:2cb1be240f50 1058 led4 = OFF;
sayzyas 0:2cb1be240f50 1059 Thread::wait(15); // Don't delete, important!
sayzyas 0:2cb1be240f50 1060 }
sayzyas 0:2cb1be240f50 1061 else{
sayzyas 0:2cb1be240f50 1062 sprintf( sbuf, "XX_CLRF_002_0000" ); // M1 Stop
sayzyas 0:2cb1be240f50 1063 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1064 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1065 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1066
sayzyas 0:2cb1be240f50 1067 sprintf( sbuf, "XX_CLLB_002_0000" ); // M2 Stop
sayzyas 0:2cb1be240f50 1068 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1069 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1070 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1071 /*
sayzyas 0:2cb1be240f50 1072 // Receive winch position from host
sayzyas 0:2cb1be240f50 1073 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1074 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1075
sayzyas 0:2cb1be240f50 1076 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1077 lcd_dsp(0,0,"OX ",11);
sayzyas 0:2cb1be240f50 1078 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1079 */
sayzyas 0:2cb1be240f50 1080 led4 = OFF;
sayzyas 0:2cb1be240f50 1081 led_ind_ptwc = OFF;
sayzyas 0:2cb1be240f50 1082 }
sayzyas 0:2cb1be240f50 1083 }
sayzyas 0:2cb1be240f50 1084 // ---------------------------------------------
sayzyas 0:2cb1be240f50 1085 // Other Switch Control: Pass
sayzyas 0:2cb1be240f50 1086 // ---------------------------------------------
sayzyas 0:2cb1be240f50 1087 if ((sw1 == 0)&&(sw2 == 1)&&(sw3==0)){ // Limit Switch valid
sayzyas 0:2cb1be240f50 1088 if( flg_lsw_valid == false )
sayzyas 0:2cb1be240f50 1089 {
sayzyas 0:2cb1be240f50 1090 flg_lsw_valid = true;
sayzyas 0:2cb1be240f50 1091 //printf("lsw valid\r\n");
sayzyas 0:2cb1be240f50 1092 if( flg_B2 == false ) //B1
sayzyas 0:2cb1be240f50 1093 {
sayzyas 0:2cb1be240f50 1094 lcd_dsp(0,1,"------ B1 -----*",16);
sayzyas 0:2cb1be240f50 1095 }
sayzyas 0:2cb1be240f50 1096 else
sayzyas 0:2cb1be240f50 1097 {
sayzyas 0:2cb1be240f50 1098 sprintf(msg,"B2Wch [%04d mm]*", tmpPosition);
sayzyas 0:2cb1be240f50 1099 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1100 lcd_dsp(0,1,msg,16);
sayzyas 0:2cb1be240f50 1101 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1102 }
sayzyas 0:2cb1be240f50 1103 Thread::wait(700);
sayzyas 0:2cb1be240f50 1104 }
sayzyas 0:2cb1be240f50 1105 else
sayzyas 0:2cb1be240f50 1106 {
sayzyas 0:2cb1be240f50 1107 flg_lsw_valid = false;
sayzyas 0:2cb1be240f50 1108 //printf("lsw invalid\r\n");
sayzyas 0:2cb1be240f50 1109 if( flg_B2 == false ) //B1
sayzyas 0:2cb1be240f50 1110 {
sayzyas 0:2cb1be240f50 1111 lcd_dsp(0,1,"------ B1 ------",16);
sayzyas 0:2cb1be240f50 1112 }
sayzyas 0:2cb1be240f50 1113 else
sayzyas 0:2cb1be240f50 1114 {
sayzyas 0:2cb1be240f50 1115 sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
sayzyas 0:2cb1be240f50 1116 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1117 lcd_dsp(0,1,msg,16);
sayzyas 0:2cb1be240f50 1118 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1119 }
sayzyas 0:2cb1be240f50 1120 Thread::wait(700);
sayzyas 0:2cb1be240f50 1121 }
sayzyas 0:2cb1be240f50 1122 }
sayzyas 0:2cb1be240f50 1123 else if ((sw1 == 0)&&(sw2 == 1)&&(sw3==1)){ // RF transform K
sayzyas 0:2cb1be240f50 1124 led_ind_run(0, led_ind_cnt_t, LED_IND_BLINK_PERIOD2);
sayzyas 0:2cb1be240f50 1125 if( led_ind_cnt_t > LED_IND_BLINK_PERIOD2*2 )
sayzyas 0:2cb1be240f50 1126 {
sayzyas 0:2cb1be240f50 1127 led_ind_cnt_t = 0;
sayzyas 0:2cb1be240f50 1128 }
sayzyas 0:2cb1be240f50 1129 else
sayzyas 0:2cb1be240f50 1130 {
sayzyas 0:2cb1be240f50 1131 led_ind_cnt_t += 1;
sayzyas 0:2cb1be240f50 1132 }
sayzyas 0:2cb1be240f50 1133 if( flg_lsw_valid == true )
sayzyas 0:2cb1be240f50 1134 {
sayzyas 0:2cb1be240f50 1135 sprintf( sbuf, "XX_TFRF_100_0100" );
sayzyas 0:2cb1be240f50 1136 }
sayzyas 0:2cb1be240f50 1137 else
sayzyas 0:2cb1be240f50 1138 {
sayzyas 0:2cb1be240f50 1139 sprintf( sbuf, "XX_TFRF_000_0100" );
sayzyas 0:2cb1be240f50 1140 }
sayzyas 0:2cb1be240f50 1141
sayzyas 0:2cb1be240f50 1142 sprintf(msg, "OX Tfm-RFK ");
sayzyas 0:2cb1be240f50 1143 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1144 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 1145 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1146
sayzyas 0:2cb1be240f50 1147 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1148 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1149 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1150 // Receive winch position from host
sayzyas 0:2cb1be240f50 1151 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1152 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1153 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1154 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1155 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 1156 {
sayzyas 0:2cb1be240f50 1157 DEBUG_PRINT_L5( "Handy(Bd2)> ### TRANSFORM LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 1158 led_tfm_lock = true;
sayzyas 0:2cb1be240f50 1159 }
sayzyas 0:2cb1be240f50 1160 if( led_tfm_lock == true )
sayzyas 0:2cb1be240f50 1161 {
sayzyas 0:2cb1be240f50 1162 led_error_blink( 0, 4 );
sayzyas 0:2cb1be240f50 1163 }
sayzyas 0:2cb1be240f50 1164 led3 = 1;
sayzyas 0:2cb1be240f50 1165 Thread::wait(15);
sayzyas 0:2cb1be240f50 1166 }
sayzyas 0:2cb1be240f50 1167 else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==1)){ // RF transform I
sayzyas 0:2cb1be240f50 1168 led_ind_run(0, led_ind_cnt_t, LED_IND_BLINK_PERIOD2);
sayzyas 0:2cb1be240f50 1169 if( led_ind_cnt_t > LED_IND_BLINK_PERIOD2*2 )
sayzyas 0:2cb1be240f50 1170 {
sayzyas 0:2cb1be240f50 1171 led_ind_cnt_t = 0;
sayzyas 0:2cb1be240f50 1172 }
sayzyas 0:2cb1be240f50 1173 else
sayzyas 0:2cb1be240f50 1174 {
sayzyas 0:2cb1be240f50 1175 led_ind_cnt_t += 1;
sayzyas 0:2cb1be240f50 1176 }
sayzyas 0:2cb1be240f50 1177 if( flg_lsw_valid == true )
sayzyas 0:2cb1be240f50 1178 {
sayzyas 0:2cb1be240f50 1179 sprintf( sbuf, "XX_TFRF_101_0100" );
sayzyas 0:2cb1be240f50 1180 }
sayzyas 0:2cb1be240f50 1181 else
sayzyas 0:2cb1be240f50 1182 {
sayzyas 0:2cb1be240f50 1183 sprintf( sbuf, "XX_TFRF_001_0100" );
sayzyas 0:2cb1be240f50 1184 }
sayzyas 0:2cb1be240f50 1185
sayzyas 0:2cb1be240f50 1186 sprintf(msg, "OX Tfm-RFI ");
sayzyas 0:2cb1be240f50 1187 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1188 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 1189 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1190
sayzyas 0:2cb1be240f50 1191 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1192 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1193 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1194 // Receive winch position from host
sayzyas 0:2cb1be240f50 1195 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1196 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1197 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1198 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1199 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 1200 {
sayzyas 0:2cb1be240f50 1201 DEBUG_PRINT_L5( "Handy(Bd2)> ### TRANSFORM LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 1202 led_tfm_lock = true;
sayzyas 0:2cb1be240f50 1203 }
sayzyas 0:2cb1be240f50 1204 if( led_tfm_lock == true )
sayzyas 0:2cb1be240f50 1205 {
sayzyas 0:2cb1be240f50 1206 led_error_blink( 0, 4 );
sayzyas 0:2cb1be240f50 1207 }
sayzyas 0:2cb1be240f50 1208 led3 = 1;
sayzyas 0:2cb1be240f50 1209 Thread::wait(15);
sayzyas 0:2cb1be240f50 1210 }
sayzyas 0:2cb1be240f50 1211 else if ((sw1 == 0)&&(sw2 == 0)&&(sw3==1)){ // LB transform K
sayzyas 0:2cb1be240f50 1212 led_ind_run(0, led_ind_cnt_t, LED_IND_BLINK_PERIOD2);
sayzyas 0:2cb1be240f50 1213 if( led_ind_cnt_t > LED_IND_BLINK_PERIOD2*2 )
sayzyas 0:2cb1be240f50 1214 {
sayzyas 0:2cb1be240f50 1215 led_ind_cnt_t = 0;
sayzyas 0:2cb1be240f50 1216 }
sayzyas 0:2cb1be240f50 1217 else
sayzyas 0:2cb1be240f50 1218 {
sayzyas 0:2cb1be240f50 1219 led_ind_cnt_t += 1;
sayzyas 0:2cb1be240f50 1220 }
sayzyas 0:2cb1be240f50 1221 if( flg_lsw_valid == true )
sayzyas 0:2cb1be240f50 1222 {
sayzyas 0:2cb1be240f50 1223 sprintf( sbuf, "XX_TFLB_100_0100" );
sayzyas 0:2cb1be240f50 1224 }
sayzyas 0:2cb1be240f50 1225 else
sayzyas 0:2cb1be240f50 1226 {
sayzyas 0:2cb1be240f50 1227 sprintf( sbuf, "XX_TFLB_000_0100" );
sayzyas 0:2cb1be240f50 1228 }
sayzyas 0:2cb1be240f50 1229
sayzyas 0:2cb1be240f50 1230 sprintf(msg, "OX Tfm-LBK ");
sayzyas 0:2cb1be240f50 1231 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1232 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 1233 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1234
sayzyas 0:2cb1be240f50 1235 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1236 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1237 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1238 // Receive winch position from host
sayzyas 0:2cb1be240f50 1239 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1240 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1241 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1242 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1243 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 1244 {
sayzyas 0:2cb1be240f50 1245 DEBUG_PRINT_L5( "Handy(Bd2)> ### TRANSFORM LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 1246 led_tfm_lock = true;
sayzyas 0:2cb1be240f50 1247 }
sayzyas 0:2cb1be240f50 1248 if( led_tfm_lock == true )
sayzyas 0:2cb1be240f50 1249 {
sayzyas 0:2cb1be240f50 1250 led_error_blink( 0, 4 );
sayzyas 0:2cb1be240f50 1251 }
sayzyas 0:2cb1be240f50 1252 led3 = 1;
sayzyas 0:2cb1be240f50 1253 Thread::wait(15);
sayzyas 0:2cb1be240f50 1254 }
sayzyas 0:2cb1be240f50 1255 else if ((sw1 == 1)&&(sw2 == 1)&&(sw3==0)){ // LB transform I
sayzyas 0:2cb1be240f50 1256 led_ind_run(0, led_ind_cnt_t, LED_IND_BLINK_PERIOD2);
sayzyas 0:2cb1be240f50 1257 if( led_ind_cnt_t > LED_IND_BLINK_PERIOD2*2 )
sayzyas 0:2cb1be240f50 1258 {
sayzyas 0:2cb1be240f50 1259 led_ind_cnt_t = 0;
sayzyas 0:2cb1be240f50 1260 }
sayzyas 0:2cb1be240f50 1261 else
sayzyas 0:2cb1be240f50 1262 {
sayzyas 0:2cb1be240f50 1263 led_ind_cnt_t += 1;
sayzyas 0:2cb1be240f50 1264 }
sayzyas 0:2cb1be240f50 1265 if( flg_lsw_valid == true )
sayzyas 0:2cb1be240f50 1266 {
sayzyas 0:2cb1be240f50 1267 sprintf( sbuf, "XX_TFLB_101_0100" );
sayzyas 0:2cb1be240f50 1268 }
sayzyas 0:2cb1be240f50 1269 else
sayzyas 0:2cb1be240f50 1270 {
sayzyas 0:2cb1be240f50 1271 sprintf( sbuf, "XX_TFLB_001_0100" );
sayzyas 0:2cb1be240f50 1272 }
sayzyas 0:2cb1be240f50 1273
sayzyas 0:2cb1be240f50 1274 sprintf(msg, "OX Tfm-LBI ");
sayzyas 0:2cb1be240f50 1275 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1276 lcd_dsp(0,0,msg,11);
sayzyas 0:2cb1be240f50 1277 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1278
sayzyas 0:2cb1be240f50 1279 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1280 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1281 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1282 // Receive winch position from host
sayzyas 0:2cb1be240f50 1283 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1284 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1285 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1286 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1287 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 1288 {
sayzyas 0:2cb1be240f50 1289 DEBUG_PRINT_L5( "Handy(Bd2)> ### TRANSFORM LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 1290 led_tfm_lock = true;
sayzyas 0:2cb1be240f50 1291 }
sayzyas 0:2cb1be240f50 1292 if( led_tfm_lock == true )
sayzyas 0:2cb1be240f50 1293 {
sayzyas 0:2cb1be240f50 1294 led_error_blink( 0, 4 );
sayzyas 0:2cb1be240f50 1295 }
sayzyas 0:2cb1be240f50 1296 led3 = 1;
sayzyas 0:2cb1be240f50 1297 Thread::wait(15);
sayzyas 0:2cb1be240f50 1298 }
sayzyas 0:2cb1be240f50 1299 else
sayzyas 0:2cb1be240f50 1300 {
sayzyas 0:2cb1be240f50 1301 sprintf( sbuf, "XX_TFRF_002_0000" );
sayzyas 0:2cb1be240f50 1302 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1303 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1304 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1305 // Receive winch position from host
sayzyas 0:2cb1be240f50 1306 // rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1307 // if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1308 sprintf( sbuf, "XX_TFLB_002_0000" );
sayzyas 0:2cb1be240f50 1309 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1310 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1311 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1312 // Receive winch position from host
sayzyas 0:2cb1be240f50 1313 // rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1314 // if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1315 led3 = OFF;
sayzyas 0:2cb1be240f50 1316 led_tfm_lock = false;
sayzyas 0:2cb1be240f50 1317 led_ind_tfm = OFF;
sayzyas 0:2cb1be240f50 1318 }
sayzyas 0:2cb1be240f50 1319 }
sayzyas 0:2cb1be240f50 1320 }
sayzyas 0:2cb1be240f50 1321 // **********************************************************
sayzyas 0:2cb1be240f50 1322 // B1: Camera pan/tile, B2: Winch control part
sayzyas 0:2cb1be240f50 1323 // **********************************************************
sayzyas 0:2cb1be240f50 1324 else{
sayzyas 0:2cb1be240f50 1325 flg_tfmcrwpart_1st = 0;
sayzyas 0:2cb1be240f50 1326 led_valid_part2 = 1;
sayzyas 0:2cb1be240f50 1327 led_valid_part1 = 0;
sayzyas 0:2cb1be240f50 1328 if( flg_wchcptpart_1st == 0 )
sayzyas 0:2cb1be240f50 1329 {
sayzyas 0:2cb1be240f50 1330 sprintf( sbuf, "XO_PART_000_0000" );
sayzyas 0:2cb1be240f50 1331 DEBUG_PRINT_L0( "Handy(Bd2)> VALID PART [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1332 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1333 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1334 Thread::wait(5);
sayzyas 0:2cb1be240f50 1335 flg_wchcptpart_1st = 1;
sayzyas 0:2cb1be240f50 1336 }
sayzyas 0:2cb1be240f50 1337 else
sayzyas 0:2cb1be240f50 1338 {
sayzyas 0:2cb1be240f50 1339 if((sw1 == 0)&&(sw2 == 1)&&(sw3==0)){ // Winch Position Clear
sayzyas 0:2cb1be240f50 1340 sprintf( sbuf, "OO_WCLR_000_0100" );
sayzyas 0:2cb1be240f50 1341 DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1342 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1343 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1344 Thread::wait(10);
sayzyas 0:2cb1be240f50 1345 }
sayzyas 0:2cb1be240f50 1346 if( // Dead zone
sayzyas 0:2cb1be240f50 1347 ( jsr_ud_undata >= (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
sayzyas 0:2cb1be240f50 1348 ( jsr_ud_undata <= (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
sayzyas 0:2cb1be240f50 1349 ( jsr_lr_undata >= (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
sayzyas 0:2cb1be240f50 1350 ( jsr_lr_undata <= (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
sayzyas 0:2cb1be240f50 1351 ){
sayzyas 0:2cb1be240f50 1352 if( flg_B2 == false ) // B1
sayzyas 0:2cb1be240f50 1353 {
sayzyas 0:2cb1be240f50 1354
sayzyas 0:2cb1be240f50 1355 sprintf( sbuf, "XX_CPAN_002_0000" ); // M2 Stop
sayzyas 0:2cb1be240f50 1356 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1357 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1358 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1359 sprintf( sbuf, "XX_CTLT_002_0000" ); // M2 Stop
sayzyas 0:2cb1be240f50 1360 DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1361 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1362 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1363 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1364 if( runcount < 15 )
sayzyas 0:2cb1be240f50 1365 {
sayzyas 0:2cb1be240f50 1366 lcd_dsp(0,0,"XO [^] ",16);
sayzyas 0:2cb1be240f50 1367 }
sayzyas 0:2cb1be240f50 1368 else if ( runcount < 30 )
sayzyas 0:2cb1be240f50 1369 {
sayzyas 0:2cb1be240f50 1370 lcd_dsp(0,0,"XO [>] ",16);
sayzyas 0:2cb1be240f50 1371 }
sayzyas 0:2cb1be240f50 1372 else if(runcount < 45 )
sayzyas 0:2cb1be240f50 1373 {
sayzyas 0:2cb1be240f50 1374 lcd_dsp(0,0,"XO [v] ",16);
sayzyas 0:2cb1be240f50 1375 }
sayzyas 0:2cb1be240f50 1376 else{
sayzyas 0:2cb1be240f50 1377 lcd_dsp(0,0,"XO [<] ",16);
sayzyas 0:2cb1be240f50 1378 }
sayzyas 0:2cb1be240f50 1379 if( runcount >= 60 )
sayzyas 0:2cb1be240f50 1380 {
sayzyas 0:2cb1be240f50 1381 runcount = 0;
sayzyas 0:2cb1be240f50 1382 }
sayzyas 0:2cb1be240f50 1383 else
sayzyas 0:2cb1be240f50 1384 {
sayzyas 0:2cb1be240f50 1385 runcount++;
sayzyas 0:2cb1be240f50 1386 };
sayzyas 0:2cb1be240f50 1387 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1388 Thread::wait(2);
sayzyas 0:2cb1be240f50 1389 led_ind_ptwc = OFF;
sayzyas 0:2cb1be240f50 1390 }
sayzyas 0:2cb1be240f50 1391 else // B2
sayzyas 0:2cb1be240f50 1392 {
sayzyas 0:2cb1be240f50 1393 sprintf( sbuf, "XX_WICH_002_0000" );
sayzyas 0:2cb1be240f50 1394 DEBUG_PRINT_L0( "Handy(Bd2)> BTN WCH STOP [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1395 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1396 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1397 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1398 //lcd_dsp(0,0,"XO ",11);
sayzyas 0:2cb1be240f50 1399
sayzyas 0:2cb1be240f50 1400 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1401 if( runcount < 15 )
sayzyas 0:2cb1be240f50 1402 {
sayzyas 0:2cb1be240f50 1403 lcd_dsp(0,0,"XO [^] ",16);
sayzyas 0:2cb1be240f50 1404 }
sayzyas 0:2cb1be240f50 1405 else if ( runcount < 30 )
sayzyas 0:2cb1be240f50 1406 {
sayzyas 0:2cb1be240f50 1407 lcd_dsp(0,0,"XO [>] ",16);
sayzyas 0:2cb1be240f50 1408 }
sayzyas 0:2cb1be240f50 1409 else if(runcount < 45 )
sayzyas 0:2cb1be240f50 1410 {
sayzyas 0:2cb1be240f50 1411 lcd_dsp(0,0,"XO [v] ",16);
sayzyas 0:2cb1be240f50 1412 }
sayzyas 0:2cb1be240f50 1413 else{
sayzyas 0:2cb1be240f50 1414 lcd_dsp(0,0,"XO [<] ",16);
sayzyas 0:2cb1be240f50 1415 }
sayzyas 0:2cb1be240f50 1416 if( runcount >= 60 )
sayzyas 0:2cb1be240f50 1417 {
sayzyas 0:2cb1be240f50 1418 runcount = 0;
sayzyas 0:2cb1be240f50 1419 }
sayzyas 0:2cb1be240f50 1420 else
sayzyas 0:2cb1be240f50 1421 {
sayzyas 0:2cb1be240f50 1422 runcount++;
sayzyas 0:2cb1be240f50 1423 };
sayzyas 0:2cb1be240f50 1424
sayzyas 0:2cb1be240f50 1425 // Receive winch position from host
sayzyas 0:2cb1be240f50 1426 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1427 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1428
sayzyas 0:2cb1be240f50 1429 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1430 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1431 DEBUG_PRINT_L0( "Handy(Bd2)> Winch Position [%04d]\r\n", tmpPosition );
sayzyas 0:2cb1be240f50 1432 if( tmpPosition != 9999 )
sayzyas 0:2cb1be240f50 1433 {
sayzyas 0:2cb1be240f50 1434 if( flg_lsw_valid == true )
sayzyas 0:2cb1be240f50 1435 {
sayzyas 0:2cb1be240f50 1436 sprintf(msg,"B2Wch [%04d mm]*", tmpPosition);
sayzyas 0:2cb1be240f50 1437 }
sayzyas 0:2cb1be240f50 1438 else
sayzyas 0:2cb1be240f50 1439 {
sayzyas 0:2cb1be240f50 1440 sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
sayzyas 0:2cb1be240f50 1441 }
sayzyas 0:2cb1be240f50 1442 }
sayzyas 0:2cb1be240f50 1443 else
sayzyas 0:2cb1be240f50 1444 {
sayzyas 0:2cb1be240f50 1445 if( flg_lsw_valid == true )
sayzyas 0:2cb1be240f50 1446 {
sayzyas 0:2cb1be240f50 1447 sprintf(msg,"B2Wch [**** mm]*");
sayzyas 0:2cb1be240f50 1448 }
sayzyas 0:2cb1be240f50 1449 else
sayzyas 0:2cb1be240f50 1450 {
sayzyas 0:2cb1be240f50 1451 sprintf(msg,"B2Wch [**** mm] ");
sayzyas 0:2cb1be240f50 1452 }
sayzyas 0:2cb1be240f50 1453 }
sayzyas 0:2cb1be240f50 1454 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1455 lcd_dsp(0,1,msg,16);
sayzyas 0:2cb1be240f50 1456 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1457 led_ind_ptwc = OFF;
sayzyas 0:2cb1be240f50 1458 }
sayzyas 0:2cb1be240f50 1459 led2 = 0;
sayzyas 0:2cb1be240f50 1460 led3 = 0;
sayzyas 0:2cb1be240f50 1461 led_ind_ptwc = 0;
sayzyas 0:2cb1be240f50 1462 led_ptwc_lock = false;
sayzyas 0:2cb1be240f50 1463 }
sayzyas 0:2cb1be240f50 1464 else if( // PAN zone
sayzyas 0:2cb1be240f50 1465 ( jsr_lr_undata >= (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone)) &&
sayzyas 0:2cb1be240f50 1466 ( jsr_ud_undata >= (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone*2)) &&
sayzyas 0:2cb1be240f50 1467 ( jsr_ud_undata <= (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone*2)) &&
sayzyas 0:2cb1be240f50 1468 ( flg_B2 == false )
sayzyas 0:2cb1be240f50 1469 ){
sayzyas 0:2cb1be240f50 1470 led2 = 1;
sayzyas 0:2cb1be240f50 1471 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
sayzyas 0:2cb1be240f50 1472 if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
sayzyas 0:2cb1be240f50 1473 {
sayzyas 0:2cb1be240f50 1474 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 1475 }
sayzyas 0:2cb1be240f50 1476 else
sayzyas 0:2cb1be240f50 1477 {
sayzyas 0:2cb1be240f50 1478 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 1479 }
sayzyas 0:2cb1be240f50 1480 m1_speed = ( jsr_lr_undata - js_center_value_r_lr ) * 100 / js_center_value_r_lr; // Speed ratio
sayzyas 0:2cb1be240f50 1481 sprintf( sbuf, "XX_CPAN_000_%04d", m1_speed );
sayzyas 0:2cb1be240f50 1482 DEBUG_PRINT_L0( "Handy(Bd2)> BTN PAN CW [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1483 // Send command to host.
sayzyas 0:2cb1be240f50 1484 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1485 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1486
sayzyas 0:2cb1be240f50 1487 // Receive winch position from host
sayzyas 0:2cb1be240f50 1488 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1489 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1490 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1491 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1492 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 1493 {
sayzyas 0:2cb1be240f50 1494 DEBUG_PRINT_L0( "Handy(Bd2)> ### CCAMERA TILT LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 1495 led_ptwc_lock = true;
sayzyas 0:2cb1be240f50 1496 }
sayzyas 0:2cb1be240f50 1497 else
sayzyas 0:2cb1be240f50 1498 {
sayzyas 0:2cb1be240f50 1499 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1500 lcd_dsp(0,0,"XO PAN CW ",11);
sayzyas 0:2cb1be240f50 1501 lcd_dsp(0,1,"------ B1 ------",16);
sayzyas 0:2cb1be240f50 1502 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1503 }
sayzyas 0:2cb1be240f50 1504 if( led_ptwc_lock == true )
sayzyas 0:2cb1be240f50 1505 {
sayzyas 0:2cb1be240f50 1506 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 1507 }
sayzyas 0:2cb1be240f50 1508 }
sayzyas 0:2cb1be240f50 1509 else if( // Left turn zone
sayzyas 0:2cb1be240f50 1510 ( jsr_lr_undata <= (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
sayzyas 0:2cb1be240f50 1511 ( jsr_ud_undata >= (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone*2)) &&
sayzyas 0:2cb1be240f50 1512 ( jsr_ud_undata <= (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone*2)) &&
sayzyas 0:2cb1be240f50 1513 ( flg_B2 == false )
sayzyas 0:2cb1be240f50 1514 ){
sayzyas 0:2cb1be240f50 1515 led2 = 1;
sayzyas 0:2cb1be240f50 1516 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
sayzyas 0:2cb1be240f50 1517 if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
sayzyas 0:2cb1be240f50 1518 {
sayzyas 0:2cb1be240f50 1519 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 1520 }
sayzyas 0:2cb1be240f50 1521 else
sayzyas 0:2cb1be240f50 1522 {
sayzyas 0:2cb1be240f50 1523 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 1524 }
sayzyas 0:2cb1be240f50 1525 m1_speed = ( js_center_value_r_lr - jsr_lr_undata ) * 100 / js_center_value_r_lr; // Speed ratio
sayzyas 0:2cb1be240f50 1526 sprintf( sbuf, "XX_CPAN_001_%04d", m1_speed );
sayzyas 0:2cb1be240f50 1527 DEBUG_PRINT_L0( "Handy(Bd2)> BTN PAN CCW[%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1528 // Send command to host.
sayzyas 0:2cb1be240f50 1529 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1530 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1531
sayzyas 0:2cb1be240f50 1532 // Receive winch position from host
sayzyas 0:2cb1be240f50 1533 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1534 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1535 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1536 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1537 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 1538 {
sayzyas 0:2cb1be240f50 1539 DEBUG_PRINT_L0( "Handy(Bd2)> ### CCAMERA TILT LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 1540 led_ptwc_lock = true;
sayzyas 0:2cb1be240f50 1541 }
sayzyas 0:2cb1be240f50 1542 else
sayzyas 0:2cb1be240f50 1543 {
sayzyas 0:2cb1be240f50 1544 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1545 lcd_dsp(0,0,"XO PAN CCW ",11);
sayzyas 0:2cb1be240f50 1546 lcd_dsp(0,1,"------ B1 ------",16);
sayzyas 0:2cb1be240f50 1547 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1548 }
sayzyas 0:2cb1be240f50 1549 if( led_ptwc_lock == true )
sayzyas 0:2cb1be240f50 1550 {
sayzyas 0:2cb1be240f50 1551 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 1552 }
sayzyas 0:2cb1be240f50 1553 }
sayzyas 0:2cb1be240f50 1554 // Down
sayzyas 0:2cb1be240f50 1555 else if( // Down Motor forward
sayzyas 0:2cb1be240f50 1556 ( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
sayzyas 0:2cb1be240f50 1557 ( jsr_lr_undata > (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone*2)) &&
sayzyas 0:2cb1be240f50 1558 ( jsr_lr_undata < (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone*2)) &&
sayzyas 0:2cb1be240f50 1559 ( flg_B2 == false)
sayzyas 0:2cb1be240f50 1560 ){
sayzyas 0:2cb1be240f50 1561 led2 = ON;
sayzyas 0:2cb1be240f50 1562 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
sayzyas 0:2cb1be240f50 1563 if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
sayzyas 0:2cb1be240f50 1564 {
sayzyas 0:2cb1be240f50 1565 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 1566 }
sayzyas 0:2cb1be240f50 1567 else
sayzyas 0:2cb1be240f50 1568 {
sayzyas 0:2cb1be240f50 1569 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 1570 }
sayzyas 0:2cb1be240f50 1571 m2_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
sayzyas 0:2cb1be240f50 1572 //m2_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
sayzyas 0:2cb1be240f50 1573 sprintf( sbuf, "XX_CTLT_000_%04d", m2_speed );
sayzyas 0:2cb1be240f50 1574 DEBUG_PRINT_L0( "Handy(Bd2)> BTN TILT CW [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1575 // Send command to host.
sayzyas 0:2cb1be240f50 1576 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1577 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1578
sayzyas 0:2cb1be240f50 1579 // Receive winch position from host
sayzyas 0:2cb1be240f50 1580 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1581 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1582 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1583 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1584 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 1585 {
sayzyas 0:2cb1be240f50 1586 DEBUG_PRINT_L0( "Handy(Bd2)> ### CCAMERA TILT LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 1587 led_ptwc_lock = true;
sayzyas 0:2cb1be240f50 1588 }
sayzyas 0:2cb1be240f50 1589 else
sayzyas 0:2cb1be240f50 1590 {
sayzyas 0:2cb1be240f50 1591 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1592 lcd_dsp(0,0,"XO TLT Down",11);
sayzyas 0:2cb1be240f50 1593 lcd_dsp(0,1,"------ B1 ------",16);
sayzyas 0:2cb1be240f50 1594 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1595 }
sayzyas 0:2cb1be240f50 1596 if( led_ptwc_lock == true )
sayzyas 0:2cb1be240f50 1597 {
sayzyas 0:2cb1be240f50 1598 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 1599 }
sayzyas 0:2cb1be240f50 1600 }
sayzyas 0:2cb1be240f50 1601 else if( // Up motor reverse
sayzyas 0:2cb1be240f50 1602 ( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
sayzyas 0:2cb1be240f50 1603 ( jsr_lr_undata > (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone*2)) &&
sayzyas 0:2cb1be240f50 1604 ( jsr_lr_undata < (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone*2)) &&
sayzyas 0:2cb1be240f50 1605 ( flg_B2 == false)
sayzyas 0:2cb1be240f50 1606 ){
sayzyas 0:2cb1be240f50 1607 led2 = 1;
sayzyas 0:2cb1be240f50 1608 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
sayzyas 0:2cb1be240f50 1609 if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
sayzyas 0:2cb1be240f50 1610 {
sayzyas 0:2cb1be240f50 1611 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 1612 }
sayzyas 0:2cb1be240f50 1613 else
sayzyas 0:2cb1be240f50 1614 {
sayzyas 0:2cb1be240f50 1615 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 1616 }
sayzyas 0:2cb1be240f50 1617 m2_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
sayzyas 0:2cb1be240f50 1618 // m2_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
sayzyas 0:2cb1be240f50 1619 sprintf( sbuf, "XX_CTLT_001_%04d", m2_speed );
sayzyas 0:2cb1be240f50 1620 DEBUG_PRINT_L0( "Handy(Bd2)> BTN TLT CCW[%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1621 // Send command to host.
sayzyas 0:2cb1be240f50 1622 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1623 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1624
sayzyas 0:2cb1be240f50 1625 // Receive winch position from host
sayzyas 0:2cb1be240f50 1626 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1627 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1628 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1629 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1630 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 1631 {
sayzyas 0:2cb1be240f50 1632 DEBUG_PRINT_L0( "Handy(Bd2)> ### CCAMERA TILT LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 1633 led_ptwc_lock = true;
sayzyas 0:2cb1be240f50 1634 }
sayzyas 0:2cb1be240f50 1635 else
sayzyas 0:2cb1be240f50 1636 {
sayzyas 0:2cb1be240f50 1637 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1638 lcd_dsp(0,0,"XO TILT Up ",11);
sayzyas 0:2cb1be240f50 1639 lcd_dsp(0,1,"------ B1 ------",16);
sayzyas 0:2cb1be240f50 1640 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1641 }
sayzyas 0:2cb1be240f50 1642 if( led_ptwc_lock == true )
sayzyas 0:2cb1be240f50 1643 {
sayzyas 0:2cb1be240f50 1644 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 1645 }
sayzyas 0:2cb1be240f50 1646 }
sayzyas 0:2cb1be240f50 1647 // Down Motor forward
sayzyas 0:2cb1be240f50 1648 else if(
sayzyas 0:2cb1be240f50 1649 ( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
sayzyas 0:2cb1be240f50 1650
sayzyas 0:2cb1be240f50 1651 ( jsr_lr_undata > (js_center_value_r_lr - 25 * setValue.jsCtrl.rjs_left_dzone)) &&
sayzyas 0:2cb1be240f50 1652 ( jsr_lr_undata < (js_center_value_r_lr + 25 * setValue.jsCtrl.rjs_right_dzone)) &&
sayzyas 0:2cb1be240f50 1653 ( flg_B2 == true )
sayzyas 0:2cb1be240f50 1654 ){
sayzyas 0:2cb1be240f50 1655 led2 = ON;
sayzyas 0:2cb1be240f50 1656 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
sayzyas 0:2cb1be240f50 1657 if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
sayzyas 0:2cb1be240f50 1658 {
sayzyas 0:2cb1be240f50 1659 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 1660 }
sayzyas 0:2cb1be240f50 1661 else
sayzyas 0:2cb1be240f50 1662 {
sayzyas 0:2cb1be240f50 1663 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 1664 }
sayzyas 0:2cb1be240f50 1665 m1_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
sayzyas 0:2cb1be240f50 1666 sprintf( sbuf, "XX_WICH_000_%04d", m1_speed );
sayzyas 0:2cb1be240f50 1667 DEBUG_PRINT_L0( "Handy(Bd2)> BTN WCH DN [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1668 // Send command to host.
sayzyas 0:2cb1be240f50 1669 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1670 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1671 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1672 lcd_dsp(0,0,"XO W Down ",11);
sayzyas 0:2cb1be240f50 1673 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1674 // Receive winch position from host.
sayzyas 0:2cb1be240f50 1675 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1676 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1677 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1678 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1679 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 1680 {
sayzyas 0:2cb1be240f50 1681 DEBUG_PRINT_L0( "Handy(Bd2)> ### WINCH LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 1682 led_ptwc_lock = true;
sayzyas 0:2cb1be240f50 1683 }
sayzyas 0:2cb1be240f50 1684 else
sayzyas 0:2cb1be240f50 1685 {
sayzyas 0:2cb1be240f50 1686 DEBUG_PRINT_L0( "Handy(Bd2)> Winch Position [%04d]\r\n", tmpPosition );
sayzyas 0:2cb1be240f50 1687 sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
sayzyas 0:2cb1be240f50 1688 if( tmpPosition != 9999 )
sayzyas 0:2cb1be240f50 1689 {
sayzyas 0:2cb1be240f50 1690 if( flg_lsw_valid == true )
sayzyas 0:2cb1be240f50 1691 {
sayzyas 0:2cb1be240f50 1692 sprintf(msg,"B2Wch [%04d mm]*", tmpPosition);
sayzyas 0:2cb1be240f50 1693 }
sayzyas 0:2cb1be240f50 1694 else
sayzyas 0:2cb1be240f50 1695 {
sayzyas 0:2cb1be240f50 1696 sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
sayzyas 0:2cb1be240f50 1697 }
sayzyas 0:2cb1be240f50 1698 }
sayzyas 0:2cb1be240f50 1699 else
sayzyas 0:2cb1be240f50 1700 {
sayzyas 0:2cb1be240f50 1701 if( flg_lsw_valid == true )
sayzyas 0:2cb1be240f50 1702 {
sayzyas 0:2cb1be240f50 1703 sprintf(msg,"B2Wch [**** mm]*");
sayzyas 0:2cb1be240f50 1704 }
sayzyas 0:2cb1be240f50 1705 else
sayzyas 0:2cb1be240f50 1706 {
sayzyas 0:2cb1be240f50 1707 sprintf(msg,"B2Wch [**** mm] ");
sayzyas 0:2cb1be240f50 1708 }
sayzyas 0:2cb1be240f50 1709 }
sayzyas 0:2cb1be240f50 1710 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1711 lcd_dsp(0,1,msg,16);
sayzyas 0:2cb1be240f50 1712 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1713 }
sayzyas 0:2cb1be240f50 1714 if( led_ptwc_lock == true )
sayzyas 0:2cb1be240f50 1715 {
sayzyas 0:2cb1be240f50 1716 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 1717 }
sayzyas 0:2cb1be240f50 1718 }
sayzyas 0:2cb1be240f50 1719 // Up motor reverse
sayzyas 0:2cb1be240f50 1720 else if(
sayzyas 0:2cb1be240f50 1721 ( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
sayzyas 0:2cb1be240f50 1722 ( jsr_lr_undata > (js_center_value_r_lr - 25 * setValue.jsCtrl.rjs_left_dzone)) &&
sayzyas 0:2cb1be240f50 1723 ( jsr_lr_undata < (js_center_value_r_lr + 25 * setValue.jsCtrl.rjs_right_dzone)) &&
sayzyas 0:2cb1be240f50 1724 ( flg_B2 == true )
sayzyas 0:2cb1be240f50 1725 ){
sayzyas 0:2cb1be240f50 1726 led2 = 1;
sayzyas 0:2cb1be240f50 1727 led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
sayzyas 0:2cb1be240f50 1728 if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
sayzyas 0:2cb1be240f50 1729 {
sayzyas 0:2cb1be240f50 1730 led_ind_cnt = 0;
sayzyas 0:2cb1be240f50 1731 }
sayzyas 0:2cb1be240f50 1732 else
sayzyas 0:2cb1be240f50 1733 {
sayzyas 0:2cb1be240f50 1734 led_ind_cnt += 1;
sayzyas 0:2cb1be240f50 1735 }
sayzyas 0:2cb1be240f50 1736 m1_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
sayzyas 0:2cb1be240f50 1737 sprintf( sbuf, "XX_WICH_001_%04d", m1_speed );
sayzyas 0:2cb1be240f50 1738 DEBUG_PRINT_L0( "Handy(Bd2)> BTN WCH UP [%s]\r\n", sbuf );
sayzyas 0:2cb1be240f50 1739 // Send command to host
sayzyas 0:2cb1be240f50 1740 rts = tsocket.send_all(sbuf, sizeof(sbuf));
sayzyas 0:2cb1be240f50 1741 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1742 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1743 lcd_dsp(0,0,"XO W Up ",11);
sayzyas 0:2cb1be240f50 1744 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1745 // Receive winch position from host
sayzyas 0:2cb1be240f50 1746 rts = tsocket.receive( rbuf, sizeof(rbuf));
sayzyas 0:2cb1be240f50 1747 if( rts < 0 ) break;
sayzyas 0:2cb1be240f50 1748 rbuf[4] = '\0';
sayzyas 0:2cb1be240f50 1749 tmpPosition = atoi( rbuf );
sayzyas 0:2cb1be240f50 1750 if( tmpPosition == 9999 )
sayzyas 0:2cb1be240f50 1751 {
sayzyas 0:2cb1be240f50 1752 DEBUG_PRINT_L0( "Handy(Bd2)> ### WWINCH LOCK DETECTION ###\r\n" );
sayzyas 0:2cb1be240f50 1753 led_ptwc_lock = true;
sayzyas 0:2cb1be240f50 1754 }
sayzyas 0:2cb1be240f50 1755 else
sayzyas 0:2cb1be240f50 1756 {
sayzyas 0:2cb1be240f50 1757 DEBUG_PRINT_L0( "Handy(Bd2)> Winch Position [%04d]\r\n", tmpPosition );
sayzyas 0:2cb1be240f50 1758 sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
sayzyas 0:2cb1be240f50 1759 if( tmpPosition != 9999 )
sayzyas 0:2cb1be240f50 1760 {
sayzyas 0:2cb1be240f50 1761 if( flg_lsw_valid == true )
sayzyas 0:2cb1be240f50 1762 {
sayzyas 0:2cb1be240f50 1763 sprintf(msg,"B2Wch [%04d mm]*", tmpPosition);
sayzyas 0:2cb1be240f50 1764 }
sayzyas 0:2cb1be240f50 1765 else
sayzyas 0:2cb1be240f50 1766 {
sayzyas 0:2cb1be240f50 1767 sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
sayzyas 0:2cb1be240f50 1768 }
sayzyas 0:2cb1be240f50 1769 }
sayzyas 0:2cb1be240f50 1770 else
sayzyas 0:2cb1be240f50 1771 {
sayzyas 0:2cb1be240f50 1772 if( flg_lsw_valid == true )
sayzyas 0:2cb1be240f50 1773 {
sayzyas 0:2cb1be240f50 1774 sprintf(msg,"B2Wch [**** mm]*");
sayzyas 0:2cb1be240f50 1775 }
sayzyas 0:2cb1be240f50 1776 else
sayzyas 0:2cb1be240f50 1777 {
sayzyas 0:2cb1be240f50 1778 sprintf(msg,"B2Wch [**** mm] ");
sayzyas 0:2cb1be240f50 1779 }
sayzyas 0:2cb1be240f50 1780 }
sayzyas 0:2cb1be240f50 1781 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1782 lcd_dsp(0,1,msg,16);
sayzyas 0:2cb1be240f50 1783 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1784 }
sayzyas 0:2cb1be240f50 1785 if( led_ptwc_lock == true )
sayzyas 0:2cb1be240f50 1786 {
sayzyas 0:2cb1be240f50 1787 led_error_blink( 1, 4 );
sayzyas 0:2cb1be240f50 1788 }
sayzyas 0:2cb1be240f50 1789 }
sayzyas 0:2cb1be240f50 1790 }
sayzyas 0:2cb1be240f50 1791 }
sayzyas 0:2cb1be240f50 1792 Thread::wait(5);
sayzyas 0:2cb1be240f50 1793 }
sayzyas 0:2cb1be240f50 1794 lcd_dsp(0,0,"!!Eth error",11);
sayzyas 0:2cb1be240f50 1795 tsocket.close();
sayzyas 0:2cb1be240f50 1796 }
sayzyas 0:2cb1be240f50 1797
sayzyas 0:2cb1be240f50 1798 }
sayzyas 0:2cb1be240f50 1799 else{
sayzyas 0:2cb1be240f50 1800 cf_led_error( &led1,&led2,&led3,&led4 );
sayzyas 0:2cb1be240f50 1801 DEBUG_PRINT_L0("Handy(Bd2)> ###ERROR### Eternet connect Fali\r\n");
sayzyas 0:2cb1be240f50 1802 DEBUG_PRINT_L0("Handy(Bd2)> This programis booting in Stand alone mode.\r\n");
sayzyas 0:2cb1be240f50 1803 flg_system_ok = false;
sayzyas 0:2cb1be240f50 1804 }
sayzyas 0:2cb1be240f50 1805 }
sayzyas 0:2cb1be240f50 1806 else{
sayzyas 0:2cb1be240f50 1807 DEBUG_PRINT_L0("Handy(Bd2)> ###ERROR### Eternet init Fail\r\n");
sayzyas 0:2cb1be240f50 1808 DEBUG_PRINT_L0("Handy(Bd2)> This programis booting in Stand alone mode.\r\n");
sayzyas 0:2cb1be240f50 1809 flg_system_ok = false;
sayzyas 0:2cb1be240f50 1810 }
sayzyas 0:2cb1be240f50 1811 }
sayzyas 0:2cb1be240f50 1812
sayzyas 0:2cb1be240f50 1813 // ======================================================================
sayzyas 0:2cb1be240f50 1814 // Read setting value from lpcal file system of mbed
sayzyas 0:2cb1be240f50 1815 // ======================================================================
sayzyas 0:2cb1be240f50 1816 bool read_LFS( setValue_t* setValue ){
sayzyas 0:2cb1be240f50 1817 FILE *fp;
sayzyas 0:2cb1be240f50 1818 char *fname = "/local/jsset.txt";
sayzyas 0:2cb1be240f50 1819 char s[150];
sayzyas 0:2cb1be240f50 1820 int c;
sayzyas 0:2cb1be240f50 1821 int data;
sayzyas 0:2cb1be240f50 1822 bool rts;
sayzyas 0:2cb1be240f50 1823
sayzyas 0:2cb1be240f50 1824 flg_mutex.lock();
sayzyas 0:2cb1be240f50 1825 fp = fopen(fname, "r");
sayzyas 0:2cb1be240f50 1826 if( fp != NULL ){ // Open "set.txt" on the local file system for writing
sayzyas 0:2cb1be240f50 1827 c = getc(fp);
sayzyas 0:2cb1be240f50 1828 if( c != '#' ){
sayzyas 0:2cb1be240f50 1829 pc.printf( "#### ERROR This is not a setting file ####\r\n");
sayzyas 0:2cb1be240f50 1830 rts = false;
sayzyas 0:2cb1be240f50 1831 }
sayzyas 0:2cb1be240f50 1832 else{
sayzyas 0:2cb1be240f50 1833 fgets( s, 100, fp );
sayzyas 0:2cb1be240f50 1834 pc.printf( "%s", s );
sayzyas 0:2cb1be240f50 1835 fscanf(fp,"%03d",&data);setValue->jsCtrl.rjs_upper_dzone=data; pc.printf("R-JS upper = %03d",setValue->jsCtrl.rjs_upper_dzone); fgets(s,100,fp); pc.printf("%s",s );
sayzyas 0:2cb1be240f50 1836 fscanf(fp,"%03d",&data);setValue->jsCtrl.rjs_lower_dzone=data; pc.printf("R-JS lower = %03d",setValue->jsCtrl.rjs_lower_dzone); fgets(s,100,fp); pc.printf("%s",s );
sayzyas 0:2cb1be240f50 1837 fscanf(fp,"%03d",&data);setValue->jsCtrl.rjs_right_dzone=data; pc.printf("R-JS upper = %03d",setValue->jsCtrl.rjs_right_dzone); fgets(s,100,fp); pc.printf("%s",s );
sayzyas 0:2cb1be240f50 1838 fscanf(fp,"%03d",&data);setValue->jsCtrl.rjs_left_dzone=data; pc.printf("R-JS lower = %03d",setValue->jsCtrl.rjs_left_dzone); fgets(s,100,fp); pc.printf("%s",s );
sayzyas 0:2cb1be240f50 1839 fscanf(fp,"%03d",&data);setValue->jsCtrl.reserved_1=data; pc.printf("%03d",setValue->jsCtrl.reserved_1); fgets(s,100,fp); pc.printf("%s",s);
sayzyas 0:2cb1be240f50 1840 fscanf(fp,"%03d",&data);setValue->jsCtrl.reserved_2=data; pc.printf("%03d",setValue->jsCtrl.reserved_2); fgets(s,100,fp); pc.printf("%s",s);
sayzyas 0:2cb1be240f50 1841 fscanf(fp,"%03d",&data);setValue->jsCtrl.reserved_3=data; pc.printf("%03d",setValue->jsCtrl.reserved_3); fgets(s,100,fp); pc.printf("%s",s);
sayzyas 0:2cb1be240f50 1842 fscanf(fp,"%03d",&data);setValue->jsCtrl.reserved_4=data; pc.printf("%03d",setValue->jsCtrl.reserved_4); fgets(s,100,fp); pc.printf("%s",s);
sayzyas 0:2cb1be240f50 1843 }
sayzyas 0:2cb1be240f50 1844 fclose(fp);
sayzyas 0:2cb1be240f50 1845 rts = true;
sayzyas 0:2cb1be240f50 1846 }
sayzyas 0:2cb1be240f50 1847 else{
sayzyas 0:2cb1be240f50 1848 pc.printf( "#### ERROR local file open error ####\r\n");
sayzyas 0:2cb1be240f50 1849 rts = false;
sayzyas 0:2cb1be240f50 1850 }
sayzyas 0:2cb1be240f50 1851 flg_mutex.unlock();
sayzyas 0:2cb1be240f50 1852 return rts;
sayzyas 0:2cb1be240f50 1853 }
sayzyas 0:2cb1be240f50 1854
sayzyas 0:2cb1be240f50 1855
sayzyas 0:2cb1be240f50 1856 extern "C" {
sayzyas 0:2cb1be240f50 1857 #include "rt_TypeDef.h"
sayzyas 0:2cb1be240f50 1858 #include "rt_System.h"
sayzyas 0:2cb1be240f50 1859 }
sayzyas 0:2cb1be240f50 1860 // **********************************************************************
sayzyas 0:2cb1be240f50 1861 //
sayzyas 0:2cb1be240f50 1862 // Main Function of this program
sayzyas 0:2cb1be240f50 1863 //
sayzyas 0:2cb1be240f50 1864 // **********************************************************************
sayzyas 0:2cb1be240f50 1865 int main()
sayzyas 0:2cb1be240f50 1866 {
sayzyas 0:2cb1be240f50 1867 Mutex file_access_mutex;
sayzyas 0:2cb1be240f50 1868 // int ret;
sayzyas 0:2cb1be240f50 1869 int try_cnt;
sayzyas 0:2cb1be240f50 1870 // bool flg_ethernet = false;
sayzyas 0:2cb1be240f50 1871 char sss[20];
sayzyas 0:2cb1be240f50 1872
sayzyas 0:2cb1be240f50 1873 // Serial baudrate
sayzyas 0:2cb1be240f50 1874 pc.baud(115200);
sayzyas 0:2cb1be240f50 1875
sayzyas 0:2cb1be240f50 1876 sw_validpart.mode( PullUp ); // use internal pullup
sayzyas 0:2cb1be240f50 1877 sw_JS_ShapeMode.mode( PullUp ); // use internal pullup
sayzyas 0:2cb1be240f50 1878
sayzyas 0:2cb1be240f50 1879 led_ind_ptwc = ON; // LED indicator pan/tilt, crawler limit
sayzyas 0:2cb1be240f50 1880 led_ind_tfm = ON; // LED indicator tfm motor current limit
sayzyas 0:2cb1be240f50 1881 led_valid_part1 = ON; // LED transform, crawler valid
sayzyas 0:2cb1be240f50 1882 led_valid_part2 = ON; // LED pan/tilt, winch valid
sayzyas 0:2cb1be240f50 1883
sayzyas 0:2cb1be240f50 1884 lcd_dsp(0,0,"B2 PMORPH Demo ",16);
sayzyas 0:2cb1be240f50 1885 lcd_dsp(0,1,"Revast Co.,Ltd. ",16);
sayzyas 0:2cb1be240f50 1886 lcd_dsp(0,0,"Rev1.00 20170403",16);
sayzyas 0:2cb1be240f50 1887 lcd_dsp(0,1,"System Booting..",16);
sayzyas 0:2cb1be240f50 1888
sayzyas 0:2cb1be240f50 1889 DEBUG_PRINT_L0("\r\n");
sayzyas 0:2cb1be240f50 1890 DEBUG_PRINT_L0("Bd2> HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH\r\n");
sayzyas 0:2cb1be240f50 1891 DEBUG_PRINT_L0("Bd2> | Project: B1/B2 Debris Prover Demonstration machine\r\n");
sayzyas 0:2cb1be240f50 1892 DEBUG_PRINT_L0("Bd2> |-------------------------------------------------------------\r\n");
sayzyas 0:2cb1be240f50 1893 DEBUG_PRINT_L0("Handy(Bd2)>> | This is: Handy Control Program of Main Controller\r\n");
sayzyas 0:2cb1be240f50 1894 DEBUG_PRINT_L0("Handy(Bd2)>> | Target MCU: mbed LPC1768\r\n");
sayzyas 0:2cb1be240f50 1895 DEBUG_PRINT_L0("Handy(Bd2)>> | Letest update: %s\r\n", LatestUpDate);
sayzyas 0:2cb1be240f50 1896 DEBUG_PRINT_L0("Handy(Bd2)>> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 0:2cb1be240f50 1897 DEBUG_PRINT_L0("Handy(Bd2)>> | Author: %s\r\n", Author);
sayzyas 0:2cb1be240f50 1898 DEBUG_PRINT_L0("Handy(Bd2)>> | Copyright(C) 2017 %s Allright Reserved\r\n", Company);
sayzyas 0:2cb1be240f50 1899 DEBUG_PRINT_L0("Handy(Bd2)>> HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH\r\n");
sayzyas 0:2cb1be240f50 1900 sprintf( sss, "%s", ProgramRevision );
sayzyas 0:2cb1be240f50 1901
sayzyas 0:2cb1be240f50 1902 // Thread::wait(50);
sayzyas 0:2cb1be240f50 1903
sayzyas 0:2cb1be240f50 1904 //---------------------------------------------------
sayzyas 0:2cb1be240f50 1905 // Read CrExp setting value from Local File System
sayzyas 0:2cb1be240f50 1906 // setting file "SET.DAT".
sayzyas 0:2cb1be240f50 1907 // When error occured, LED1 will be blinking shortly.
sayzyas 0:2cb1be240f50 1908 //---------------------------------------------------
sayzyas 0:2cb1be240f50 1909 DEBUG_PRINT_L0("Handy(Bd2)> =============================================================\r\n");
sayzyas 0:2cb1be240f50 1910 DEBUG_PRINT_L0("Handy(Bd2)> 2. Read setting value from LFS\r\n");
sayzyas 0:2cb1be240f50 1911 DEBUG_PRINT_L0("Handy(Bd2)> =============================================================\r\n");
sayzyas 0:2cb1be240f50 1912
sayzyas 0:2cb1be240f50 1913 #ifdef __CREATE_SETTING_FILE__
sayzyas 0:2cb1be240f50 1914 write_LFS(&setValue); // Create and set setting file.
sayzyas 0:2cb1be240f50 1915 #endif // __CREATE_SETTING_FILE__
sayzyas 0:2cb1be240f50 1916
sayzyas 0:2cb1be240f50 1917 // --------------------------------------------------------------------
sayzyas 0:2cb1be240f50 1918 // Read setting from local file system and set to internal structure
sayzyas 0:2cb1be240f50 1919 // --------------------------------------------------------------------
sayzyas 0:2cb1be240f50 1920 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1921 lcd_dsp(0,0,"LFS reading ... ",16);
sayzyas 0:2cb1be240f50 1922 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1923 try_cnt = LFS_READ_COUNT;
sayzyas 0:2cb1be240f50 1924 while( 1 ){
sayzyas 0:2cb1be240f50 1925 if( read_LFS(&setValue) == true ) break;
sayzyas 0:2cb1be240f50 1926 else try_cnt -= 1;
sayzyas 0:2cb1be240f50 1927 if( try_cnt == 0 ){
sayzyas 0:2cb1be240f50 1928 DEBUG_PRINT_L0("Handy(Bd2)> ***ERROR*** LFS read error\r\n");
sayzyas 0:2cb1be240f50 1929 while(1){
sayzyas 0:2cb1be240f50 1930 led1 = !led1;
sayzyas 0:2cb1be240f50 1931 Thread::wait(30);
sayzyas 0:2cb1be240f50 1932 }
sayzyas 0:2cb1be240f50 1933 }
sayzyas 0:2cb1be240f50 1934 }
sayzyas 0:2cb1be240f50 1935
sayzyas 0:2cb1be240f50 1936 DEBUG_PRINT_L0("Handy(Bd2)> LFS read OK\r\n");
sayzyas 0:2cb1be240f50 1937 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1938 lcd_dsp(0,0,"LFS read OK ",16);
sayzyas 0:2cb1be240f50 1939 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1940
sayzyas 0:2cb1be240f50 1941 led3 = ON; // Setting Data Read OK
sayzyas 0:2cb1be240f50 1942 led2 = ON; // Check target OK
sayzyas 0:2cb1be240f50 1943
sayzyas 0:2cb1be240f50 1944 /* Set basic function default setting */
sayzyas 0:2cb1be240f50 1945 baseOperation.sv_JS_OpeMode = 0;
sayzyas 0:2cb1be240f50 1946 baseOperation.sv_JS_OpeMode = 0;
sayzyas 0:2cb1be240f50 1947 baseOperation.sv_WinchValid = 0;
sayzyas 0:2cb1be240f50 1948
sayzyas 0:2cb1be240f50 1949 DEBUG_PRINT_L0( "Handy(Bd2)> ----------------------------------\r\n");
sayzyas 0:2cb1be240f50 1950 DEBUG_PRINT_L0( "Handy(Bd2)> >>>> Initializing completed ! <<<<\r\n");
sayzyas 0:2cb1be240f50 1951 DEBUG_PRINT_L0( "Handy(Bd2)> ----------------------------------\r\n");
sayzyas 0:2cb1be240f50 1952
sayzyas 0:2cb1be240f50 1953 /*
sayzyas 0:2cb1be240f50 1954 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1955 lcd_dsp(0,0,"Booted up ! ",16);
sayzyas 0:2cb1be240f50 1956 Thread::wait(500);
sayzyas 0:2cb1be240f50 1957 lcd_dsp(0,0,"LFS System OK ",16);
sayzyas 0:2cb1be240f50 1958 lcd_dsp(0,1," ",16);
sayzyas 0:2cb1be240f50 1959 Thread::wait(500);
sayzyas 0:2cb1be240f50 1960 lcd_dsp(0,0," ",16);
sayzyas 0:2cb1be240f50 1961 lcd_dsp(0,1,"B2Wch [3500 mm] ",16);
sayzyas 0:2cb1be240f50 1962 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1963 */
sayzyas 0:2cb1be240f50 1964
sayzyas 0:2cb1be240f50 1965 lcdMutex.lock();
sayzyas 0:2cb1be240f50 1966 lcd_dsp(0,0,"Start sw task ",16);
sayzyas 0:2cb1be240f50 1967 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 1968 DEBUG_PRINT_L0("Handy(Bd2)> =============================================================\r\n");
sayzyas 0:2cb1be240f50 1969 DEBUG_PRINT_L0("Handy(Bd2)>> 5. Start the task\r\n");
sayzyas 0:2cb1be240f50 1970 /* Max thread count is (may be ..) 2, How can I increase this , I don't know ?? */
sayzyas 0:2cb1be240f50 1971
sayzyas 0:2cb1be240f50 1972 // DEBUG_PRINT_L0("Handy(Bd2)> Start host gamepad task ... ");
sayzyas 0:2cb1be240f50 1973 // Thread thread_gpd(gamepad_task, NULL, osPriorityNormal, 128*4); // GamePad Task isn't necessary.
sayzyas 0:2cb1be240f50 1974 DEBUG_PRINT_L0("Handy(Bd2)> Start switch task ... ");
sayzyas 0:2cb1be240f50 1975 Thread thread_swd(sw_task, NULL, osPriorityNormal, 256 * 4);
sayzyas 0:2cb1be240f50 1976
sayzyas 0:2cb1be240f50 1977 DEBUG_PRINT_L0("\r\n");
sayzyas 0:2cb1be240f50 1978 DEBUG_PRINT_L0("Handy(Bd2)> =============================================================\r\n");
sayzyas 0:2cb1be240f50 1979
sayzyas 0:2cb1be240f50 1980 led4 = OFF;
sayzyas 0:2cb1be240f50 1981 led3 = OFF;
sayzyas 0:2cb1be240f50 1982 led2 = OFF;
sayzyas 0:2cb1be240f50 1983 led1 = ON; // Initializing is OK then Power Indicator LED ON
sayzyas 0:2cb1be240f50 1984
sayzyas 0:2cb1be240f50 1985 wait(10.0);
sayzyas 0:2cb1be240f50 1986
sayzyas 0:2cb1be240f50 1987 if( flg_system_ok == true )
sayzyas 0:2cb1be240f50 1988 {
sayzyas 0:2cb1be240f50 1989 led_ind_ptwc = OFF; // LED indicator pan/tilt, crawler limit
sayzyas 0:2cb1be240f50 1990 led_ind_tfm = OFF; // LED indicator tfm motor current limit
sayzyas 0:2cb1be240f50 1991 led_valid_part1 = OFF; // LED transform, crawler valid
sayzyas 0:2cb1be240f50 1992 led_valid_part2 = OFF; // LED pan/tilt, winch valid
sayzyas 0:2cb1be240f50 1993 }
sayzyas 0:2cb1be240f50 1994
sayzyas 0:2cb1be240f50 1995 while( 1 ) {
sayzyas 0:2cb1be240f50 1996 Thread::wait(3);
sayzyas 0:2cb1be240f50 1997 // -----------------------------------------------------------------
sayzyas 0:2cb1be240f50 1998 // Communicate with client PC program.
sayzyas 0:2cb1be240f50 1999 // TCP connection:
sayzyas 0:2cb1be240f50 2000 // -----------------------------------------------------------------
sayzyas 0:2cb1be240f50 2001 if( flg_system_ok == true )
sayzyas 0:2cb1be240f50 2002 {
sayzyas 0:2cb1be240f50 2003 lcdMutex.lock();
sayzyas 0:2cb1be240f50 2004 lcd_dsp(13,0,"[^] ",4);
sayzyas 0:2cb1be240f50 2005 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 2006 Thread::wait(300);
sayzyas 0:2cb1be240f50 2007 lcdMutex.lock();
sayzyas 0:2cb1be240f50 2008 lcd_dsp(13,0,"[>] ",4);
sayzyas 0:2cb1be240f50 2009 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 2010 Thread::wait(300);
sayzyas 0:2cb1be240f50 2011 lcdMutex.lock();
sayzyas 0:2cb1be240f50 2012 lcd_dsp(13,0,"[v] ",4);
sayzyas 0:2cb1be240f50 2013 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 2014 Thread::wait(300);
sayzyas 0:2cb1be240f50 2015 lcdMutex.lock();
sayzyas 0:2cb1be240f50 2016 lcd_dsp(13,0,"[<] ",4);
sayzyas 0:2cb1be240f50 2017 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 2018 Thread::wait(300);
sayzyas 0:2cb1be240f50 2019 }
sayzyas 0:2cb1be240f50 2020 else
sayzyas 0:2cb1be240f50 2021 {
sayzyas 0:2cb1be240f50 2022 lcdMutex.lock();
sayzyas 0:2cb1be240f50 2023 // 0123456789012345
sayzyas 0:2cb1be240f50 2024 lcd_dsp(0,0,"HandyCtrl Error!",16);
sayzyas 0:2cb1be240f50 2025 lcd_dsp(0,1,"* Check system! * ",16);
sayzyas 0:2cb1be240f50 2026 lcdMutex.unlock();
sayzyas 0:2cb1be240f50 2027 Thread::wait(750);
sayzyas 0:2cb1be240f50 2028 }
sayzyas 0:2cb1be240f50 2029 }
sayzyas 0:2cb1be240f50 2030
sayzyas 0:2cb1be240f50 2031 tsocket.close();
sayzyas 0:2cb1be240f50 2032 }