z ysaito
2018.07.26
Dependencies: EthernetInterface TextLCD USBDevice USBHost2 mbed
P1_2Y_handy.cpp
- Committer:
- sayzyas
- Date:
- 2018-07-26
- Revision:
- 1:392f3708c3e2
- Parent:
- 0:2cb1be240f50
File content as of revision 1:392f3708c3e2:
/*
////////////////////////////////////////////////////////////////////////////
Project: B1/B2 DebrisProbe Demonstration model
Title: Debris Prober demo HandyCtrl Main
Target: mbed LPC1768
Data: 2017.06.30
Author: sayzyas as ZNR
------------------------------------------------
The Final Project of this company ! .... may be.
------------------------------------------------
## ##### ## #####
## ## ## ## ## ##
## ## ## ##### #####
## ## ## ## ## ##
## ## ##### #####
mbed LPC1768
+-------------USB-----------+
| GND VOUT(3.3V) |
| VIN VU(5.0V OUT)|
| VB IF- |
| mR IF+ |
| p5 mosi Ether RD- |
| p6 miso Ether RD+ |
| p7 sck Ether TD- |
| p8 Ether TD+ |
| p9 tx sdi USB D- |
| p10 rx scl USB D+ |
| p11 mosi CAN rd p30 |
| p12 miso CAN td p29 |
| p13 tx sck sda tx p28 |
| p14 rx scl rx P27 |
| p15 AIn PWM P26 |
| p16 AIn PWM P25 |
| p17 AIn PWM p24 |
| p18 AIn AOut PWM p23 |
| p19 AIn PWM p22 |
| p20 AIn PWM p21 |
+---------------------------+
////////////////////////////////////////////////////////////////////////////
*/
#include "mbed.h"
#include "USBHostGamepad.h"
#include "USBSerial.h"
#include "rtos.h"
#include "EthernetInterface.h"
#include "common.h"
#include "stdio.h"
//#include "TextLCD.h"
#include "com_func.h"
#include "USBHID.h"
#include "TextLCD.h"
// USBSerial serial setting
Serial pc(USBTX, USBRX); // UART
// Digital I/O setting
DigitalOut led1(LED1); // 1:on,0:off System is OK then ON.
DigitalOut led2(LED2); // 1:on,0:off GamePad is connected.
DigitalOut led3(LED3); // 1:on,0:off When got the GamePas switch input then ON
DigitalOut led4(LED4); // 1:on,0:off Access indicator with PC
// LED indicator
DigitalOut led_ind_ptwc(p14); // LED indicator pan/tilt, winch, crawler limit
DigitalOut led_ind_tfm(p15); // LED indicator tfm motor current limit
DigitalOut led_valid_part1(p25); // LED transform, crawler valid
DigitalOut led_valid_part2(p26); // LED pan/tilt, winch valid
int16_t led_ind_cnt = 0;
int16_t led_ind_cnt_t = 0;
// Switch Matrix 3bit SW ON = 0 input
// bit 2 1 0 Send data
// 0 1 1 1 :
// 1 1 1 0 : 0x01 RF Tfm K
// 2 1 0 1 : 0x02 RF Tfm I
// 3 1 0 0 : 0x04 LB Tfm K
// 4 0 1 1 : 0x08 LB Tfm I
// 5 0 1 0 : 0x10 Winch Down
// 6 0 0 1 : 0x20 Winch Up
// 7 0 0 0 :
DigitalIn sw1(p5); // Bit 0
DigitalIn sw2(p6); // Bit 1
DigitalIn sw3(p7); // Bit 2
// Digital Input 1:OFF, 0:ON
DigitalIn sw_JS_ShapeMode(p29); // SW: i \ k ,pde ( 1: K, 0: I )
DigitalIn sw_validpart(p30); // SW: CrExp Shape ( 1: Left-Winch. 0: Right-tfmcrwler )
// Analig Joystick input for crawler control
AnalogIn Rjsin_UD(p16); // Crawler R-UD
AnalogIn Rjsin_LR(p17); // Crawler R-LR
//AnalogIn Ljsin_UD(p19); // Crawler L-UD
//AnalogIn Ljsin_LR(p20); // Crawler L-LR
const char* TCP_SERVER_ADDRESS = "192.168.0.24";
// Ethernet
EthernetInterface eth;
// TCP
TCPSocketConnection tsocket; // TCP client
// UDP
UDPSocket usocket; // UDP client
Endpoint client;
// Local File System
LocalFileSystem local("local"); // Create the local filesystem under the name "local"
TextLCD lcd(p11, p12, p24, p23, p22, p21); // rs, e, d4-d7
uint8_t js_center_value_r_ud = 0x80;
uint8_t js_center_value_r_lr = 0x80;
uint8_t js_center_value_l_ud = 0x80;
uint8_t js_center_value_l_lr = 0x80;
// Global
uint32_t flg_gamePad_Connected = 0;
char PC_cmd[11+1] = "&0100000000";
basic_operation_t baseOperation;
char dbuffer[128];
// Global Parameter of setting
setValue_t setValue; // Setting Data
/* Status flag */
/*
0000 0000 : button LI LK RI RK PCW PCCW TU TD
0000 0000 : limit switch
0000 0000 : res
0000 0000 : res
*/
uint32_t flg_exp_status = 0;
Mutex lcdMutex;
Mutex flg_mutex;
int swbtn_Opeflg = 0;
int16_t winchCurrentPosition;
int16_t winchOffsetValue = 0; // 2016.10.07 added
int16_t winchDramDiameter = 5985; // 2017.01.06 added
Mutex mtx_wcp;
bool flg_ButtonOn = false;
bool flg_lsw_valid = false;
int flg_JS_shape_mode = 0;
int flg_JS_ope_mode = 0;
int motor1_current_pct;
int motor2_current_pct;
uint8_t limitSw_Sts = 0;
char motorLock_sts = '\0';
void led_winch_valid( int counter )
{
if( counter < 25)
{
led_valid_part2 = 0;
}
else
{
led_valid_part2 = 1;
}
}
void led_crawler_valid( int counter )
{
if( counter < 25)
{
led_valid_part2 = 1;
}
else
{
led_valid_part2 = 0;
}
}
void lcd_dsp( int column, int row, char* msg, int cnt)
{
for( int i = 0; i < cnt; i++ )
{
lcd.locate(column+i,row);
lcd.putc(*msg++);
}
Thread::wait(10);
}
// ============================================================
// Send Status to PC
// ============================================================
void sendStatus2PC( char *cmd, int32_t numberOfCmd ){
int i;
led4 = 1;
for ( i = 0; i < numberOfCmd; i++ ) {
pc.putc(*cmd++);
}
led4 = 0;
}
uint8_t adj_crawlerSpeed( uint8_t in )
{
uint8_t out;
/*
pc.printf( "#### %d ####\r\n", in );
if( ( in > 0 ) && ( in < 98 ))
{
out = (uint8_t)((float)in * 0.8);
}
else
{
out = 100;
}
*/
out = in;
return out;
}
bool cmd_snd2serverZ(
char* cmd, // command send to server
int cmdSize
){
tsocket.send(cmd, cmdSize);
DEBUG_PRINT_L0("Snd cmd 2 server [ %s (%d) ]\r\n", cmd, cmdSize );
// Clean up
return true;
}
bool flg_mc_tfmcrw = false;
bool flg_mc_winch = false;
char I2C_res[NumberOfI2CCommand+1] = "\0";
// *****************************************************************
// calibration joystick data
// *****************************************************************
bool calibrate_joystick( int js, uint8_t data_ud, uint8_t data_lr ){
uint16_t tmp;
if( js == 0 ) // R-JS
{
tmp = (uint16_t)js_center_value_r_ud;
tmp += (uint16_t)data_ud;
tmp /= 2;
js_center_value_r_ud = (uint8_t)tmp;
tmp = (uint16_t)js_center_value_r_lr;
tmp += (uint16_t)data_lr;
tmp /= 2;
js_center_value_r_lr = (uint8_t)tmp;
if
(
(((127-10) < js_center_value_r_ud )&&(js_center_value_r_ud < (127+10))) &&
(((127-10) < js_center_value_r_lr )&&(js_center_value_r_lr < (127+10)))
)
{
return true;
}
else
{
return false;
}
}
else // Other
{
tmp = (uint16_t)js_center_value_l_ud;
tmp += (uint16_t)data_ud;
tmp /= 2;
js_center_value_l_ud = (uint8_t)tmp;
tmp = (uint16_t)js_center_value_l_lr;
tmp += (uint16_t)data_lr;
tmp /= 2;
js_center_value_l_lr = (uint8_t)tmp;
if
(
(((127-10) < js_center_value_l_ud )&&(js_center_value_l_ud < (127+10))) &&
(((127-10) < js_center_value_l_lr )&&(js_center_value_l_lr < (127+10)))
) {
return true;
}
else
{
return false;
}
}
}
// LED current indicator blink control when
// each motor is moving.
void led_ind_run
(
int led_number,
int count,
int period
){
if(( count >= 0 )&&( count < period ))
{
if( led_number == 1 )
{
led_ind_ptwc = ON;
}
else
{
led_ind_tfm = ON;
}
}
else if(( count >= period )&&( count < period*2 ))
{
if( led_number == 1 )
{
led_ind_ptwc = OFF;
}
else
{
led_ind_tfm = OFF;
}
}
else
{
led_ind_ptwc = OFF;
led_ind_tfm = OFF;
}
}
void led_error_blink( int no, int times )
{
for( int i = 0; i < times; i++ ) {
if( no == 1 )
{
led_ind_ptwc = 1;
}
else
{
led_ind_tfm = 1;
}
Thread::wait(50);
if( no == 1 )
{
led_ind_ptwc = 0;
}
else
{
led_ind_tfm = 0;
}
Thread::wait(50);
}
}
bool flg_B2 = false;
bool flg_system_ok = true;
// *****************************************************************
// switch control task
// ----------------------
//
// Up: center -> 255
// Down: 002 -> center
// Right: center -> 255
// Left : 0 -> center
//
// *****************************************************************
int16_t tmpPosition;
void sw_task( void const *){
char msg[16] = "";
uint16_t jsr_ud_data;
uint16_t jsr_lr_data;
uint8_t jsr_ud_undata;
uint8_t jsr_lr_undata;
uint8_t m1_speed;
uint8_t m2_speed;
char sbuf[128] = ""; // TCP send buffer
char rbuf[128]; // TCP receive buffer
int flg_tfmcrwpart_1st = 0;
int flg_wchcptpart_1st = 0;
int rts;
bool jsjg = true;
bool led_tfm_lock = false;
bool led_ptwc_lock = false;
int runcount = 0;
DEBUG_PRINT_L0("Handy(Bd2)>> Start ststem initializing ...\r\n");
DEBUG_PRINT_L0("Handy(Bd2)>> =============================================================\r\n");
DEBUG_PRINT_L0("Handy(Bd2)>> Initalizing Ethernet ...\r\n");
DEBUG_PRINT_L0("Handy(Bd2)>> =============================================================\r\n");
const char* ip_address = "192.168.0.31";
const char* subnet_mask = "255.255.255.0";
const char* gateway = "192.168.0.0";
DEBUG_PRINT_L0("Handy(Bd2)> --------------------------------------\r\n");
DEBUG_PRINT_L0("Handy(Bd2)> ip address : %s\r\n", ip_address);
DEBUG_PRINT_L0("Handy(Bd2)> subnet mask : %s\r\n", subnet_mask);
DEBUG_PRINT_L0("Handy(Bd2)> default gateway: %s\r\n", gateway);
DEBUG_PRINT_L0("Handy(Bd2)> --------------------------------------\r\n");
char sss[24];
int ret = eth.init(
ip_address, // const char* ip,
subnet_mask, // const char* mask,
gateway // const char* gateway
);
if( ret == 0 ){
DEBUG_PRINT_L0("Handy(Bd2)> Eternet init ... OK\r\n");
ret = eth.connect(7000); // Connection time out: 7000msec
if( ret == 0 ){
cf_led_onoff( &led1,&led2,&led3,&led4, false, false, false, true );
DEBUG_PRINT_L0("Handy(Bd2)> [ IP Address : %s ]\r\n", eth.getIPAddress());
while( true )
{
if (tsocket.connect(TCP_SERVER_ADDRESS, TCP_SERVER_PORT) < 0) {
DEBUG_PRINT_L0("Handy(Bd2)> ###ERROR### : Unable to connect to (%s) on port (%d)\r\n", TCP_SERVER_ADDRESS, TCP_SERVER_PORT);
flg_system_ok = false;
wait(0.5);
break;
}
flg_system_ok = true;
sprintf(sss, "IP:%s", ip_address );
lcdMutex.lock();
lcd_dsp(0,0,"Network .. OK ",16);
sss[15] = ' ';
lcd_dsp(0,1,sss,16);
lcdMutex.unlock();
sprintf( sbuf, "Hello" );
DEBUG_PRINT_L0( "Handy(Bd2)> Send [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
wait_ms(5);
// Receive winch position from host
tsocket.receive( rbuf, sizeof(rbuf));
//
// Check target (B1 or B2) here !
//
if( !strncmp( rbuf, "B2Demo", 6 ) )
{
DEBUG_PRINT_L0( "Handy(Bd2)> Echo back [%s]\r\n", rbuf );
flg_B2 = true;
}
wait_ms(300);
DEBUG_PRINT_L1("Calibrating joystick ... ");
for( int i = 0; i < CALIBRATION_COUNT; i++){
jsr_ud_data = Rjsin_UD.read_u16();
jsr_lr_data = Rjsin_LR.read_u16();
jsr_ud_undata = (uint8_t)(jsr_ud_data >> 8);
jsr_lr_undata = (uint8_t)(jsr_lr_data >> 8);
jsjg = calibrate_joystick( 0, jsr_ud_undata, jsr_lr_undata); // Under constraction
}
if( jsjg == false ){
flg_system_ok = false;
wait(0.5);
break;
}
DEBUG_PRINT_L1("done\r\n");
lcdMutex.lock();
wait_ms(500);
lcd_dsp(0,0,"System Boot up ",16);
if( flg_B2 == true )
{
sprintf( msg, "B2Wch [**** mm] "); // B2
}
else
{
sprintf( msg, "------ B1 ------"); // B1
}
lcdMutex.lock();
lcd_dsp(0,0," ",16);
lcd_dsp(0,1,msg,16);
lcdMutex.unlock();
while(1){
jsr_ud_data = Rjsin_UD.read_u16();
jsr_lr_data = Rjsin_LR.read_u16();
jsr_ud_undata = (uint8_t)(jsr_ud_data >> 8);
jsr_lr_undata = (uint8_t)(jsr_lr_data >> 8);
DEBUG_PRINT_SW("Handy SW> ");
DEBUG_PRINT_SW("R:%03d/%3d,%03d/%3d ", jsr_ud_undata, js_center_value_r_ud, jsr_lr_undata, js_center_value_r_ud );
DEBUG_PRINT_SW("SW: %d %d %d ", sw1.read(), sw2.read(), sw3.read() );
DEBUG_PRINT_SW("JS: %d %d\r\n", sw_JS_ShapeMode.read(), sw_validpart.read() );
// **********************************************************
// Transform and crawler control part
// **********************************************************
if( sw_validpart == 0 ){ // sw on
flg_wchcptpart_1st = 0;
led_valid_part2 = 0;
led_valid_part1 = 1;
if( flg_tfmcrwpart_1st == 0 )
{
sprintf( sbuf, "OX_PART_000_0000" );
DEBUG_PRINT_L0( "Handy(Bd2)> VALID PART [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
Thread::wait(5);
flg_tfmcrwpart_1st = 1;
}
else
{
// ---------------------------------------------
// Right JoyStick Control
// ---------------------------------------------
if( sw_JS_ShapeMode == 1 ){ // OFF: KO-Shape
if( // Dead zone
( jsr_ud_undata >= (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
( jsr_ud_undata <= (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
( jsr_lr_undata >= (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
( jsr_lr_undata <= (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
){
sprintf( sbuf, "XX_CLRF_002_0000" ); // M1 Stop
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CLLB_002_0000" ); // M2 Stop
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
lcdMutex.lock();
if( runcount < 15 )
{
lcd_dsp(0,0,"OX [^] ",16);
}
else if ( runcount < 30 )
{
lcd_dsp(0,0,"OX [>] ",16);
}
else if(runcount < 45 )
{
lcd_dsp(0,0,"OX [v] ",16);
}
else{
lcd_dsp(0,0,"OX [<] ",16);
}
if( runcount >= 60 )
{
runcount = 0;
}
else
{
runcount++;
}
lcdMutex.unlock();
// Receive winch position from host
// rts = tsocket.receive( rbuf, sizeof(rbuf));
// if( rts < 0 ) break;
led4 = 0;
led_ind_ptwc = 0;
led_ind_cnt = 0;
Thread::wait(15); // Don't delete, important!
}
else if( // Forward zone
( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
( jsr_lr_undata > (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
( jsr_lr_undata < (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
){
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
led4 = ON;
m1_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
m2_speed = m1_speed;
sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
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);
sprintf(msg, "OX Cr-F:%03d", (255 - jsr_ud_undata));
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
/*
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
led_error_blink( 1, 4 );
}
*/
led4 = OFF;
Thread::wait(15); // Don't delete, important!
}
else if( // Reverse zone
( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
( jsr_lr_undata > (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
( jsr_lr_undata < (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
){
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
led4 = ON;
m1_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
m2_speed = m1_speed;
sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
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);
sprintf(msg, "OX Cr-R:%03d", (255 - jsr_ud_undata));
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
/*
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
led_error_blink( 1, 4 );
}
*/
led4 = OFF;
Thread::wait(15); // Don't delete, important!
}
else if( // Right turn zone
( jsr_lr_undata > (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone)) &&
( jsr_ud_undata > (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
( jsr_ud_undata < (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone))
){
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
led4 = ON;
m1_speed = ( jsr_lr_undata - js_center_value_r_lr ) * 100 / js_center_value_r_lr; // Speed ratio
m2_speed = m1_speed; // Speed
sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
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);
sprintf(msg,"OX Cr->:%03d", jsr_lr_undata);
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
/*
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
led_error_blink( 1, 4 );
}
*/
led4 = OFF;
Thread::wait(15); // Don't delete, important!
}
else if( // Left turn zone
( jsr_lr_undata < (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
( jsr_ud_undata > (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
( jsr_ud_undata < (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone))
){
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
led4 = ON;
m1_speed = ( js_center_value_r_lr - jsr_lr_undata ) * 100 / js_center_value_r_lr; // Speed ratio
m2_speed = m1_speed;
sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
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);
sprintf(msg,"OX Cr<-:%03d", jsr_lr_undata);
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
/*
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
led_error_blink( 1, 4 );
}
*/
led4 = OFF;
Thread::wait(15); // Don't delete, important!
}
else if( // Right-upper zone
( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
( jsr_lr_undata > (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
){
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
led4 = ON;
if( jsr_ud_undata > jsr_lr_undata )
{
m2_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
m1_speed = 100 - (( jsr_lr_undata - js_center_value_r_lr ) * 100 / js_center_value_r_lr);
sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
/*
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
led_error_blink( 1, 4 );
}
*/
}
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);
sprintf(msg, "OX Cr-FRZ ");
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
led4 = OFF;
Thread::wait(15); // Don't delete, important!
}
else if( // Left-upper zone
( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
( jsr_lr_undata < (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone))
){
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
led4 = ON;
if( (jsr_ud_undata - js_center_value_r_ud) > (js_center_value_r_ud-jsr_lr_undata) )
{
m1_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
m2_speed = 100 - (( js_center_value_r_lr - jsr_lr_undata ) * 100 / js_center_value_r_lr);
sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
/*
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
led_error_blink( 1, 4 );
}
*/
}
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);
sprintf(msg, "OX Cr-FLZ ");
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
led4 = OFF;
Thread::wait(15); // Don't delete, important!
}
else if( // Right-lower zone
( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
( jsr_lr_undata > (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
){
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
led4 = ON;
if( (js_center_value_r_ud - jsr_ud_undata) > (jsr_lr_undata - js_center_value_r_lr) )
{
m2_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
m1_speed = 100 - (( jsr_lr_undata - js_center_value_r_lr ) * 100 / js_center_value_r_lr);
sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
/*
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
led_error_blink( 1, 4 );
}
*/
}
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);
sprintf(msg, "OX Cr-RRZ ");
//sprintf(msg, "OX C BK RT ");
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
led4 = OFF;
Thread::wait(15); // Don't delete, important!
}
else if( // Left-lower zone
( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
( jsr_lr_undata < (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone))
){
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
led4 = ON;
if( (js_center_value_r_ud - jsr_ud_undata) > ( js_center_value_r_lr - jsr_lr_undata) )
{
m1_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
m2_speed = 100 - (( js_center_value_r_lr - jsr_lr_undata ) * 100 / js_center_value_r_lr);
sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
/*
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
led_error_blink( 1, 4 );
}
*/
}
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);
sprintf(msg, "OX Cr-RLZ ");
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
led4 = OFF;
Thread::wait(15); // Don't delete, important!
}
}
else // ON: I-Shape
{
// Right Joystic < Down
if( jsr_ud_undata > (js_center_value_r_ud + + setValue.jsCtrl.rjs_upper_dzone) ){
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
led4 = ON;
m1_speed = adj_crawlerSpeed(( js_center_value_r_ud+1 - (255 - jsr_ud_undata) ) * 100 / js_center_value_r_ud ); // Speed
m2_speed = m1_speed; // Speed
sprintf( sbuf, "XX_CLRF_001_%04d", m1_speed ); // M1 Rvs
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CLLB_000_%04d", m2_speed ); // M2 Fwd
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
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);
sprintf(msg, "OX C FW:%03d", (255 - jsr_ud_undata));
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
/*
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
led_error_blink( 1, 4 );
}
*/
led4 = OFF;
Thread::wait(15); // Don't delete, important!
}
// Right Joystick < Up >
else if( jsr_ud_undata < (js_center_value_r_ud - + setValue.jsCtrl.rjs_upper_dzone) ){
// Crawler reverse Run
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD8);
if( led_ind_cnt > LED_IND_BLINK_PERIOD8*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
led4 = ON;
m1_speed = adj_crawlerSpeed(( (255 - jsr_ud_undata) - js_center_value_r_ud ) * 100 / js_center_value_r_ud ); // Speed
m2_speed = m1_speed; // Speed
sprintf( sbuf, "XX_CLRF_000_%04d", m1_speed ); // M1 Fwd
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CLLB_001_%04d", m2_speed ); // M2 Rvs
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
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);
sprintf(msg, "OX C BK:%03d", (255 - jsr_ud_undata));
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
/*
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CRAWLER LOCK DETECTION ###\r\n" );
led_error_blink( 1, 4 );
}
*/
led4 = OFF;
Thread::wait(15); // Don't delete, important!
}
else{
sprintf( sbuf, "XX_CLRF_002_0000" ); // M1 Stop
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CLLB_002_0000" ); // M2 Stop
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
/*
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
lcdMutex.lock();
lcd_dsp(0,0,"OX ",11);
lcdMutex.unlock();
*/
led4 = OFF;
led_ind_ptwc = OFF;
}
}
// ---------------------------------------------
// Other Switch Control: Pass
// ---------------------------------------------
if ((sw1 == 0)&&(sw2 == 1)&&(sw3==0)){ // Limit Switch valid
if( flg_lsw_valid == false )
{
flg_lsw_valid = true;
//printf("lsw valid\r\n");
if( flg_B2 == false ) //B1
{
lcd_dsp(0,1,"------ B1 -----*",16);
}
else
{
sprintf(msg,"B2Wch [%04d mm]*", tmpPosition);
lcdMutex.lock();
lcd_dsp(0,1,msg,16);
lcdMutex.unlock();
}
Thread::wait(700);
}
else
{
flg_lsw_valid = false;
//printf("lsw invalid\r\n");
if( flg_B2 == false ) //B1
{
lcd_dsp(0,1,"------ B1 ------",16);
}
else
{
sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
lcdMutex.lock();
lcd_dsp(0,1,msg,16);
lcdMutex.unlock();
}
Thread::wait(700);
}
}
else if ((sw1 == 0)&&(sw2 == 1)&&(sw3==1)){ // RF transform K
led_ind_run(0, led_ind_cnt_t, LED_IND_BLINK_PERIOD2);
if( led_ind_cnt_t > LED_IND_BLINK_PERIOD2*2 )
{
led_ind_cnt_t = 0;
}
else
{
led_ind_cnt_t += 1;
}
if( flg_lsw_valid == true )
{
sprintf( sbuf, "XX_TFRF_100_0100" );
}
else
{
sprintf( sbuf, "XX_TFRF_000_0100" );
}
sprintf(msg, "OX Tfm-RFK ");
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L5( "Handy(Bd2)> ### TRANSFORM LOCK DETECTION ###\r\n" );
led_tfm_lock = true;
}
if( led_tfm_lock == true )
{
led_error_blink( 0, 4 );
}
led3 = 1;
Thread::wait(15);
}
else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==1)){ // RF transform I
led_ind_run(0, led_ind_cnt_t, LED_IND_BLINK_PERIOD2);
if( led_ind_cnt_t > LED_IND_BLINK_PERIOD2*2 )
{
led_ind_cnt_t = 0;
}
else
{
led_ind_cnt_t += 1;
}
if( flg_lsw_valid == true )
{
sprintf( sbuf, "XX_TFRF_101_0100" );
}
else
{
sprintf( sbuf, "XX_TFRF_001_0100" );
}
sprintf(msg, "OX Tfm-RFI ");
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L5( "Handy(Bd2)> ### TRANSFORM LOCK DETECTION ###\r\n" );
led_tfm_lock = true;
}
if( led_tfm_lock == true )
{
led_error_blink( 0, 4 );
}
led3 = 1;
Thread::wait(15);
}
else if ((sw1 == 0)&&(sw2 == 0)&&(sw3==1)){ // LB transform K
led_ind_run(0, led_ind_cnt_t, LED_IND_BLINK_PERIOD2);
if( led_ind_cnt_t > LED_IND_BLINK_PERIOD2*2 )
{
led_ind_cnt_t = 0;
}
else
{
led_ind_cnt_t += 1;
}
if( flg_lsw_valid == true )
{
sprintf( sbuf, "XX_TFLB_100_0100" );
}
else
{
sprintf( sbuf, "XX_TFLB_000_0100" );
}
sprintf(msg, "OX Tfm-LBK ");
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L5( "Handy(Bd2)> ### TRANSFORM LOCK DETECTION ###\r\n" );
led_tfm_lock = true;
}
if( led_tfm_lock == true )
{
led_error_blink( 0, 4 );
}
led3 = 1;
Thread::wait(15);
}
else if ((sw1 == 1)&&(sw2 == 1)&&(sw3==0)){ // LB transform I
led_ind_run(0, led_ind_cnt_t, LED_IND_BLINK_PERIOD2);
if( led_ind_cnt_t > LED_IND_BLINK_PERIOD2*2 )
{
led_ind_cnt_t = 0;
}
else
{
led_ind_cnt_t += 1;
}
if( flg_lsw_valid == true )
{
sprintf( sbuf, "XX_TFLB_101_0100" );
}
else
{
sprintf( sbuf, "XX_TFLB_001_0100" );
}
sprintf(msg, "OX Tfm-LBI ");
lcdMutex.lock();
lcd_dsp(0,0,msg,11);
lcdMutex.unlock();
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L5( "Handy(Bd2)> ### TRANSFORM LOCK DETECTION ###\r\n" );
led_tfm_lock = true;
}
if( led_tfm_lock == true )
{
led_error_blink( 0, 4 );
}
led3 = 1;
Thread::wait(15);
}
else
{
sprintf( sbuf, "XX_TFRF_002_0000" );
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
// Receive winch position from host
// rts = tsocket.receive( rbuf, sizeof(rbuf));
// if( rts < 0 ) break;
sprintf( sbuf, "XX_TFLB_002_0000" );
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
// Receive winch position from host
// rts = tsocket.receive( rbuf, sizeof(rbuf));
// if( rts < 0 ) break;
led3 = OFF;
led_tfm_lock = false;
led_ind_tfm = OFF;
}
}
}
// **********************************************************
// B1: Camera pan/tile, B2: Winch control part
// **********************************************************
else{
flg_tfmcrwpart_1st = 0;
led_valid_part2 = 1;
led_valid_part1 = 0;
if( flg_wchcptpart_1st == 0 )
{
sprintf( sbuf, "XO_PART_000_0000" );
DEBUG_PRINT_L0( "Handy(Bd2)> VALID PART [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
Thread::wait(5);
flg_wchcptpart_1st = 1;
}
else
{
if((sw1 == 0)&&(sw2 == 1)&&(sw3==0)){ // Winch Position Clear
sprintf( sbuf, "OO_WCLR_000_0100" );
DEBUG_PRINT_L0( "Handy(Bd2)>> Send [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
Thread::wait(10);
}
if( // Dead zone
( jsr_ud_undata >= (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
( jsr_ud_undata <= (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
( jsr_lr_undata >= (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
( jsr_lr_undata <= (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone))
){
if( flg_B2 == false ) // B1
{
sprintf( sbuf, "XX_CPAN_002_0000" ); // M2 Stop
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
sprintf( sbuf, "XX_CTLT_002_0000" ); // M2 Stop
DEBUG_PRINT_L0( " [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
lcdMutex.lock();
if( runcount < 15 )
{
lcd_dsp(0,0,"XO [^] ",16);
}
else if ( runcount < 30 )
{
lcd_dsp(0,0,"XO [>] ",16);
}
else if(runcount < 45 )
{
lcd_dsp(0,0,"XO [v] ",16);
}
else{
lcd_dsp(0,0,"XO [<] ",16);
}
if( runcount >= 60 )
{
runcount = 0;
}
else
{
runcount++;
};
lcdMutex.unlock();
Thread::wait(2);
led_ind_ptwc = OFF;
}
else // B2
{
sprintf( sbuf, "XX_WICH_002_0000" );
DEBUG_PRINT_L0( "Handy(Bd2)> BTN WCH STOP [%s]\r\n", sbuf );
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
lcdMutex.lock();
//lcd_dsp(0,0,"XO ",11);
lcdMutex.lock();
if( runcount < 15 )
{
lcd_dsp(0,0,"XO [^] ",16);
}
else if ( runcount < 30 )
{
lcd_dsp(0,0,"XO [>] ",16);
}
else if(runcount < 45 )
{
lcd_dsp(0,0,"XO [v] ",16);
}
else{
lcd_dsp(0,0,"XO [<] ",16);
}
if( runcount >= 60 )
{
runcount = 0;
}
else
{
runcount++;
};
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
DEBUG_PRINT_L0( "Handy(Bd2)> Winch Position [%04d]\r\n", tmpPosition );
if( tmpPosition != 9999 )
{
if( flg_lsw_valid == true )
{
sprintf(msg,"B2Wch [%04d mm]*", tmpPosition);
}
else
{
sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
}
}
else
{
if( flg_lsw_valid == true )
{
sprintf(msg,"B2Wch [**** mm]*");
}
else
{
sprintf(msg,"B2Wch [**** mm] ");
}
}
lcdMutex.lock();
lcd_dsp(0,1,msg,16);
lcdMutex.unlock();
led_ind_ptwc = OFF;
}
led2 = 0;
led3 = 0;
led_ind_ptwc = 0;
led_ptwc_lock = false;
}
else if( // PAN zone
( jsr_lr_undata >= (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone)) &&
( jsr_ud_undata >= (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone*2)) &&
( jsr_ud_undata <= (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone*2)) &&
( flg_B2 == false )
){
led2 = 1;
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
m1_speed = ( jsr_lr_undata - js_center_value_r_lr ) * 100 / js_center_value_r_lr; // Speed ratio
sprintf( sbuf, "XX_CPAN_000_%04d", m1_speed );
DEBUG_PRINT_L0( "Handy(Bd2)> BTN PAN CW [%s]\r\n", sbuf );
// Send command to host.
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CCAMERA TILT LOCK DETECTION ###\r\n" );
led_ptwc_lock = true;
}
else
{
lcdMutex.lock();
lcd_dsp(0,0,"XO PAN CW ",11);
lcd_dsp(0,1,"------ B1 ------",16);
lcdMutex.unlock();
}
if( led_ptwc_lock == true )
{
led_error_blink( 1, 4 );
}
}
else if( // Left turn zone
( jsr_lr_undata <= (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone)) &&
( jsr_ud_undata >= (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone*2)) &&
( jsr_ud_undata <= (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone*2)) &&
( flg_B2 == false )
){
led2 = 1;
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
m1_speed = ( js_center_value_r_lr - jsr_lr_undata ) * 100 / js_center_value_r_lr; // Speed ratio
sprintf( sbuf, "XX_CPAN_001_%04d", m1_speed );
DEBUG_PRINT_L0( "Handy(Bd2)> BTN PAN CCW[%s]\r\n", sbuf );
// Send command to host.
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CCAMERA TILT LOCK DETECTION ###\r\n" );
led_ptwc_lock = true;
}
else
{
lcdMutex.lock();
lcd_dsp(0,0,"XO PAN CCW ",11);
lcd_dsp(0,1,"------ B1 ------",16);
lcdMutex.unlock();
}
if( led_ptwc_lock == true )
{
led_error_blink( 1, 4 );
}
}
// Down
else if( // Down Motor forward
( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
( jsr_lr_undata > (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone*2)) &&
( jsr_lr_undata < (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone*2)) &&
( flg_B2 == false)
){
led2 = ON;
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
m2_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
//m2_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
sprintf( sbuf, "XX_CTLT_000_%04d", m2_speed );
DEBUG_PRINT_L0( "Handy(Bd2)> BTN TILT CW [%s]\r\n", sbuf );
// Send command to host.
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CCAMERA TILT LOCK DETECTION ###\r\n" );
led_ptwc_lock = true;
}
else
{
lcdMutex.lock();
lcd_dsp(0,0,"XO TLT Down",11);
lcd_dsp(0,1,"------ B1 ------",16);
lcdMutex.unlock();
}
if( led_ptwc_lock == true )
{
led_error_blink( 1, 4 );
}
}
else if( // Up motor reverse
( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
( jsr_lr_undata > (js_center_value_r_lr - setValue.jsCtrl.rjs_left_dzone*2)) &&
( jsr_lr_undata < (js_center_value_r_lr + setValue.jsCtrl.rjs_right_dzone*2)) &&
( flg_B2 == false)
){
led2 = 1;
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
m2_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
// m2_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
sprintf( sbuf, "XX_CTLT_001_%04d", m2_speed );
DEBUG_PRINT_L0( "Handy(Bd2)> BTN TLT CCW[%s]\r\n", sbuf );
// Send command to host.
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### CCAMERA TILT LOCK DETECTION ###\r\n" );
led_ptwc_lock = true;
}
else
{
lcdMutex.lock();
lcd_dsp(0,0,"XO TILT Up ",11);
lcd_dsp(0,1,"------ B1 ------",16);
lcdMutex.unlock();
}
if( led_ptwc_lock == true )
{
led_error_blink( 1, 4 );
}
}
// Down Motor forward
else if(
( jsr_ud_undata < (js_center_value_r_ud - setValue.jsCtrl.rjs_lower_dzone)) &&
( jsr_lr_undata > (js_center_value_r_lr - 25 * setValue.jsCtrl.rjs_left_dzone)) &&
( jsr_lr_undata < (js_center_value_r_lr + 25 * setValue.jsCtrl.rjs_right_dzone)) &&
( flg_B2 == true )
){
led2 = ON;
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
m1_speed = ( js_center_value_r_ud - jsr_ud_undata ) * 100 / js_center_value_r_ud; // Speed ratio
sprintf( sbuf, "XX_WICH_000_%04d", m1_speed );
DEBUG_PRINT_L0( "Handy(Bd2)> BTN WCH DN [%s]\r\n", sbuf );
// Send command to host.
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
lcdMutex.lock();
lcd_dsp(0,0,"XO W Down ",11);
lcdMutex.unlock();
// Receive winch position from host.
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### WINCH LOCK DETECTION ###\r\n" );
led_ptwc_lock = true;
}
else
{
DEBUG_PRINT_L0( "Handy(Bd2)> Winch Position [%04d]\r\n", tmpPosition );
sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
if( tmpPosition != 9999 )
{
if( flg_lsw_valid == true )
{
sprintf(msg,"B2Wch [%04d mm]*", tmpPosition);
}
else
{
sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
}
}
else
{
if( flg_lsw_valid == true )
{
sprintf(msg,"B2Wch [**** mm]*");
}
else
{
sprintf(msg,"B2Wch [**** mm] ");
}
}
lcdMutex.lock();
lcd_dsp(0,1,msg,16);
lcdMutex.unlock();
}
if( led_ptwc_lock == true )
{
led_error_blink( 1, 4 );
}
}
// Up motor reverse
else if(
( jsr_ud_undata > (js_center_value_r_ud + setValue.jsCtrl.rjs_upper_dzone)) &&
( jsr_lr_undata > (js_center_value_r_lr - 25 * setValue.jsCtrl.rjs_left_dzone)) &&
( jsr_lr_undata < (js_center_value_r_lr + 25 * setValue.jsCtrl.rjs_right_dzone)) &&
( flg_B2 == true )
){
led2 = 1;
led_ind_run(1, led_ind_cnt, LED_IND_BLINK_PERIOD);
if( led_ind_cnt > LED_IND_BLINK_PERIOD*2 )
{
led_ind_cnt = 0;
}
else
{
led_ind_cnt += 1;
}
m1_speed = ( jsr_ud_undata - js_center_value_r_ud ) * 100 / js_center_value_r_ud; // Speed ratio
sprintf( sbuf, "XX_WICH_001_%04d", m1_speed );
DEBUG_PRINT_L0( "Handy(Bd2)> BTN WCH UP [%s]\r\n", sbuf );
// Send command to host
rts = tsocket.send_all(sbuf, sizeof(sbuf));
if( rts < 0 ) break;
lcdMutex.lock();
lcd_dsp(0,0,"XO W Up ",11);
lcdMutex.unlock();
// Receive winch position from host
rts = tsocket.receive( rbuf, sizeof(rbuf));
if( rts < 0 ) break;
rbuf[4] = '\0';
tmpPosition = atoi( rbuf );
if( tmpPosition == 9999 )
{
DEBUG_PRINT_L0( "Handy(Bd2)> ### WWINCH LOCK DETECTION ###\r\n" );
led_ptwc_lock = true;
}
else
{
DEBUG_PRINT_L0( "Handy(Bd2)> Winch Position [%04d]\r\n", tmpPosition );
sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
if( tmpPosition != 9999 )
{
if( flg_lsw_valid == true )
{
sprintf(msg,"B2Wch [%04d mm]*", tmpPosition);
}
else
{
sprintf(msg,"B2Wch [%04d mm] ", tmpPosition);
}
}
else
{
if( flg_lsw_valid == true )
{
sprintf(msg,"B2Wch [**** mm]*");
}
else
{
sprintf(msg,"B2Wch [**** mm] ");
}
}
lcdMutex.lock();
lcd_dsp(0,1,msg,16);
lcdMutex.unlock();
}
if( led_ptwc_lock == true )
{
led_error_blink( 1, 4 );
}
}
}
}
Thread::wait(5);
}
lcd_dsp(0,0,"!!Eth error",11);
tsocket.close();
}
}
else{
cf_led_error( &led1,&led2,&led3,&led4 );
DEBUG_PRINT_L0("Handy(Bd2)> ###ERROR### Eternet connect Fali\r\n");
DEBUG_PRINT_L0("Handy(Bd2)> This programis booting in Stand alone mode.\r\n");
flg_system_ok = false;
}
}
else{
DEBUG_PRINT_L0("Handy(Bd2)> ###ERROR### Eternet init Fail\r\n");
DEBUG_PRINT_L0("Handy(Bd2)> This programis booting in Stand alone mode.\r\n");
flg_system_ok = false;
}
}
// ======================================================================
// Read setting value from lpcal file system of mbed
// ======================================================================
bool read_LFS( setValue_t* setValue ){
FILE *fp;
char *fname = "/local/jsset.txt";
char s[150];
int c;
int data;
bool rts;
flg_mutex.lock();
fp = fopen(fname, "r");
if( fp != NULL ){ // Open "set.txt" on the local file system for writing
c = getc(fp);
if( c != '#' ){
pc.printf( "#### ERROR This is not a setting file ####\r\n");
rts = false;
}
else{
fgets( s, 100, fp );
pc.printf( "%s", s );
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 );
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 );
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 );
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 );
fscanf(fp,"%03d",&data);setValue->jsCtrl.reserved_1=data; pc.printf("%03d",setValue->jsCtrl.reserved_1); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->jsCtrl.reserved_2=data; pc.printf("%03d",setValue->jsCtrl.reserved_2); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->jsCtrl.reserved_3=data; pc.printf("%03d",setValue->jsCtrl.reserved_3); fgets(s,100,fp); pc.printf("%s",s);
fscanf(fp,"%03d",&data);setValue->jsCtrl.reserved_4=data; pc.printf("%03d",setValue->jsCtrl.reserved_4); fgets(s,100,fp); pc.printf("%s",s);
}
fclose(fp);
rts = true;
}
else{
pc.printf( "#### ERROR local file open error ####\r\n");
rts = false;
}
flg_mutex.unlock();
return rts;
}
extern "C" {
#include "rt_TypeDef.h"
#include "rt_System.h"
}
// **********************************************************************
//
// Main Function of this program
//
// **********************************************************************
int main()
{
Mutex file_access_mutex;
// int ret;
int try_cnt;
// bool flg_ethernet = false;
char sss[20];
// Serial baudrate
pc.baud(115200);
sw_validpart.mode( PullUp ); // use internal pullup
sw_JS_ShapeMode.mode( PullUp ); // use internal pullup
led_ind_ptwc = ON; // LED indicator pan/tilt, crawler limit
led_ind_tfm = ON; // LED indicator tfm motor current limit
led_valid_part1 = ON; // LED transform, crawler valid
led_valid_part2 = ON; // LED pan/tilt, winch valid
lcd_dsp(0,0,"B2 PMORPH Demo ",16);
lcd_dsp(0,1,"Revast Co.,Ltd. ",16);
lcd_dsp(0,0,"Rev1.00 20170403",16);
lcd_dsp(0,1,"System Booting..",16);
DEBUG_PRINT_L0("\r\n");
DEBUG_PRINT_L0("Bd2> HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH\r\n");
DEBUG_PRINT_L0("Bd2> | Project: B1/B2 Debris Prover Demonstration machine\r\n");
DEBUG_PRINT_L0("Bd2> |-------------------------------------------------------------\r\n");
DEBUG_PRINT_L0("Handy(Bd2)>> | This is: Handy Control Program of Main Controller\r\n");
DEBUG_PRINT_L0("Handy(Bd2)>> | Target MCU: mbed LPC1768\r\n");
DEBUG_PRINT_L0("Handy(Bd2)>> | Letest update: %s\r\n", LatestUpDate);
DEBUG_PRINT_L0("Handy(Bd2)>> | Program Revision: %s\r\n", ProgramRevision);
DEBUG_PRINT_L0("Handy(Bd2)>> | Author: %s\r\n", Author);
DEBUG_PRINT_L0("Handy(Bd2)>> | Copyright(C) 2017 %s Allright Reserved\r\n", Company);
DEBUG_PRINT_L0("Handy(Bd2)>> HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH\r\n");
sprintf( sss, "%s", ProgramRevision );
// Thread::wait(50);
//---------------------------------------------------
// Read CrExp setting value from Local File System
// setting file "SET.DAT".
// When error occured, LED1 will be blinking shortly.
//---------------------------------------------------
DEBUG_PRINT_L0("Handy(Bd2)> =============================================================\r\n");
DEBUG_PRINT_L0("Handy(Bd2)> 2. Read setting value from LFS\r\n");
DEBUG_PRINT_L0("Handy(Bd2)> =============================================================\r\n");
#ifdef __CREATE_SETTING_FILE__
write_LFS(&setValue); // Create and set setting file.
#endif // __CREATE_SETTING_FILE__
// --------------------------------------------------------------------
// Read setting from local file system and set to internal structure
// --------------------------------------------------------------------
lcdMutex.lock();
lcd_dsp(0,0,"LFS reading ... ",16);
lcdMutex.unlock();
try_cnt = LFS_READ_COUNT;
while( 1 ){
if( read_LFS(&setValue) == true ) break;
else try_cnt -= 1;
if( try_cnt == 0 ){
DEBUG_PRINT_L0("Handy(Bd2)> ***ERROR*** LFS read error\r\n");
while(1){
led1 = !led1;
Thread::wait(30);
}
}
}
DEBUG_PRINT_L0("Handy(Bd2)> LFS read OK\r\n");
lcdMutex.lock();
lcd_dsp(0,0,"LFS read OK ",16);
lcdMutex.unlock();
led3 = ON; // Setting Data Read OK
led2 = ON; // Check target OK
/* Set basic function default setting */
baseOperation.sv_JS_OpeMode = 0;
baseOperation.sv_JS_OpeMode = 0;
baseOperation.sv_WinchValid = 0;
DEBUG_PRINT_L0( "Handy(Bd2)> ----------------------------------\r\n");
DEBUG_PRINT_L0( "Handy(Bd2)> >>>> Initializing completed ! <<<<\r\n");
DEBUG_PRINT_L0( "Handy(Bd2)> ----------------------------------\r\n");
/*
lcdMutex.lock();
lcd_dsp(0,0,"Booted up ! ",16);
Thread::wait(500);
lcd_dsp(0,0,"LFS System OK ",16);
lcd_dsp(0,1," ",16);
Thread::wait(500);
lcd_dsp(0,0," ",16);
lcd_dsp(0,1,"B2Wch [3500 mm] ",16);
lcdMutex.unlock();
*/
lcdMutex.lock();
lcd_dsp(0,0,"Start sw task ",16);
lcdMutex.unlock();
DEBUG_PRINT_L0("Handy(Bd2)> =============================================================\r\n");
DEBUG_PRINT_L0("Handy(Bd2)>> 5. Start the task\r\n");
/* Max thread count is (may be ..) 2, How can I increase this , I don't know ?? */
// DEBUG_PRINT_L0("Handy(Bd2)> Start host gamepad task ... ");
// Thread thread_gpd(gamepad_task, NULL, osPriorityNormal, 128*4); // GamePad Task isn't necessary.
DEBUG_PRINT_L0("Handy(Bd2)> Start switch task ... ");
Thread thread_swd(sw_task, NULL, osPriorityNormal, 256 * 4);
DEBUG_PRINT_L0("\r\n");
DEBUG_PRINT_L0("Handy(Bd2)> =============================================================\r\n");
led4 = OFF;
led3 = OFF;
led2 = OFF;
led1 = ON; // Initializing is OK then Power Indicator LED ON
wait(10.0);
if( flg_system_ok == true )
{
led_ind_ptwc = OFF; // LED indicator pan/tilt, crawler limit
led_ind_tfm = OFF; // LED indicator tfm motor current limit
led_valid_part1 = OFF; // LED transform, crawler valid
led_valid_part2 = OFF; // LED pan/tilt, winch valid
}
while( 1 ) {
Thread::wait(3);
// -----------------------------------------------------------------
// Communicate with client PC program.
// TCP connection:
// -----------------------------------------------------------------
if( flg_system_ok == true )
{
lcdMutex.lock();
lcd_dsp(13,0,"[^] ",4);
lcdMutex.unlock();
Thread::wait(300);
lcdMutex.lock();
lcd_dsp(13,0,"[>] ",4);
lcdMutex.unlock();
Thread::wait(300);
lcdMutex.lock();
lcd_dsp(13,0,"[v] ",4);
lcdMutex.unlock();
Thread::wait(300);
lcdMutex.lock();
lcd_dsp(13,0,"[<] ",4);
lcdMutex.unlock();
Thread::wait(300);
}
else
{
lcdMutex.lock();
// 0123456789012345
lcd_dsp(0,0,"HandyCtrl Error!",16);
lcd_dsp(0,1,"* Check system! * ",16);
lcdMutex.unlock();
Thread::wait(750);
}
}
tsocket.close();
}