z ysaito
2018.07.26
Dependencies: EthernetInterface TextLCD USBDevice USBHost3 mbed
Fork of
USBHostHub_HelloWorld
by 
Samuel Mokrani
Diff: 0_main.cpp
- Revision:
- 11:ff06edc0219c
- Parent:
- 10:a2bd7d07c7f8
- Child:
- 12:3e6b6fcf540b
--- a/0_main.cpp Wed Feb 10 14:58:50 2016 +0000
+++ b/0_main.cpp Tue Feb 16 16:34:12 2016 +0000
@@ -1,3 +1,4 @@
+
/***************************************
* Project: B2
* Title: CrExp B2 Motor Ctrl Main
@@ -36,7 +37,6 @@
#include "USBSerial.h"
#include "rtos.h"
#include "EthernetInterface.h"
-//#include "QEI.h"
#include "common.h"
#include "stdio.h"
#include "TextLCD.h"
@@ -44,24 +44,20 @@
// USBSerial serial setting
Serial pc(USBTX, USBRX); // UART
-
// Digital I/O setting
DigitalOut led1(LED1); // 1: on, 0: off
DigitalOut led2(LED2); // 1: on, 0: off
DigitalOut led3(LED3); // 1: on, 0: off
DigitalOut led4(LED4); // 1: on, 0: off
-
// I2C setting
//i2c(p28, p27); // I2C SDA, SCL is not good ???
I2C i2c(p9, p10); // I2C SDA, SCL is good
-
// I2C address
const int32_t i2c_addr_handy = I2C_ADDRESS_HANDY << 1; // Ctrl Board0 : Handy Controller
const int32_t i2c_addr_winch = I2C_ADDRESS_WINCH << 1; // Ctrl Board1 : Winch
const int32_t i2c_addr_transform = I2C_ADDRESS_TRANSFORM << 1; // Ctrl Board2 : Transform
const int32_t i2c_addr_crawler = I2C_ADDRESS_CRAWLER << 1; // Ctrl Board3 : Crawler
const int32_t i2c_addr_resolver = I2C_ADDRESS_RESOLVER << 1; // Ctrl Board4 : Resolver
-
// Ethernet
EthernetInterface eth;
TCPSocketServer tcp_server; // TCP Server
@@ -96,7 +92,6 @@
// Mutex hwbtn_OpeMutex;
// int hwbtn_Opeflg = 0;
-char motor_current_pct;
int flg_JS_shape_mode = 0;
int flg_JS_ope_mode = 0;
uint8_t motor1_current_pct;
@@ -116,21 +111,17 @@
// ============================================================
void read_motorCurrent(
int i2c_addr,
- char* I2C_cmd,
+ char* I2C_readcmd,
char* I2C_res,
int NumberOfI2Cdata
-
){
- I2C_cmd[1] = 'C';
- i2c.write(i2c_addr, I2C_cmd, 3); // Send command to motor control board.
- //Thread::wait(500);
- i2c.read(i2c_addr, I2C_res, 3); // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+ i2c.read(i2c_addr, I2C_res, NumberOfI2Cdata); // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
/*
- DEBUG_PRINT_L4(" ++++++++++++++++++++++++++++++\n" );
- DEBUG_PRINT_L4(" Read Motor1 Current [%d]\n", I2C_res[0] );
- DEBUG_PRINT_L4(" Read Motor2 Current [%d]\n", I2C_res[1] );
- DEBUG_PRINT_L4(" Read Motor2 Current [%d]\n", I2C_res[2] );
- DEBUG_PRINT_L4(" ++++++++++++++++++++++++++++++\n" );
+ DEBUG_PRINT_L0(" ++++++++++++++++++++++++++++++\r\n" );
+ DEBUG_PRINT_L0(" Read Motor1 Current [%d]\r\n", I2C_res[0] );
+ DEBUG_PRINT_L0(" Read Motor2 Current [%d]\r\n", I2C_res[1] );
+ DEBUG_PRINT_L0(" Read [%d]\r\n", I2C_res[2] );
+ DEBUG_PRINT_L0(" ++++++++++++++++++++++++++++++\r\n" );
*/
}
@@ -163,16 +154,24 @@
int16_t ReadWinchCurrentPosition( int32_t i2c_addr )
{
char I2C_data[2];
- int16_t res_position;
+ int16_t res_position = 0;
+ int rts;
- i2c.read(i2c_addr, I2C_data, 2); // Read
+ rts = i2c.read(i2c_addr, I2C_data, 2); // Read
// flg_mutex.lock();
- res_position = (I2C_data[1] << 8) | I2C_data[0];
- // winchCurrentPosition = res_position;
+ if( rts == 0 ){
+ res_position = (I2C_data[1] << 8) | I2C_data[0];
+ }
+ else{
+ res_position = 0;
+ }
+ // winchCurrentPosition = res_position;
// flg_mutex.unlock();
return res_position;
}
+
+char I2C_res[NumberOfI2CCommand+1] = "\0";
// ============================================================
// Button control
// ============================================================
@@ -206,7 +205,7 @@
[9] : N/F
*/
char I2C_cmd[NumberOfI2CCommand+1] = "#010000";
- char I2C_res[NumberOfI2CCommand+1] = "\0";
+ char I2C_readcmd[NumberOfI2CCommand+1] = "#010000";
uint8_t btnStatus_RFK = 0;
uint8_t btnStatus_RFI = 0;
@@ -391,28 +390,36 @@
if( gamePadVID == GAMEPAD_VID_RSTHANDY ){ // NRst own original controller
flg_exp_status &= 0xFFFFFFF0;
if(!( btnStatus_Start & 0x01 )){ // I-Shape
+ flg_mutex.lock();
setValue.operation.sv_JS_ShapeMode = 0;
setValue.operation.sv_JS_OpeMode = 0;
+ flg_mutex.unlock();
flg_exp_status |= 0x00000001;
}
else{ // KO-Shape
+ flg_mutex.lock();
setValue.operation.sv_JS_ShapeMode = 1;
+ flg_mutex.unlock();
flg_exp_status |= 0x00000002;
}
if(!(btnStatus_Start & 0x02 )&&( btnStatus_Start & 0x01 )){ // Dual
+ flg_mutex.lock();
setValue.operation.sv_JS_OpeMode = 1;
+ flg_mutex.unlock();
flg_exp_status |= 0x00000004;
}
else{ // Single
+ flg_mutex.lock();
setValue.operation.sv_JS_OpeMode = 0;
+ flg_mutex.unlock();
flg_exp_status |= 0x00000008;
}
- DEBUG_PRINT_L4( "-----------------------------\n" );
- DEBUG_PRINT_L4( "%d : %d\n",btnStatus_Start, flg_exp_status );
- DEBUG_PRINT_L4( " JS shape mode change : %d\n", setValue.operation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " JS operation mode change: %d\n", setValue.operation.sv_JS_OpeMode );
- DEBUG_PRINT_L4( "-----------------------------\n" );
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ DEBUG_PRINT_L4( "%d : %d\r\n",btnStatus_Start, flg_exp_status );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", setValue.operation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " JS operation mode change: %d\r\n", setValue.operation.sv_JS_OpeMode );
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
}
else{
/*
@@ -427,41 +434,49 @@
*/
if( btnStatus_Start == btnID_Start ){
if ( btnStatus_CrossUp == btnID_CrossUp ){ // I Shape
+ flg_mutex.lock();
setValue.operation.sv_JS_ShapeMode = 0;
setValue.operation.sv_JS_OpeMode = 0;
- DEBUG_PRINT_L4( "--------------------------------\n" );
- DEBUG_PRINT_L4( " I\n" );
- DEBUG_PRINT_L4( " JS shape mode change : %d\n", setValue.operation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " JS operation mode change: %d\n", setValue.operation.sv_JS_OpeMode );
- DEBUG_PRINT_L4( "--------------------------------\n" );
+ flg_mutex.unlock();
+ DEBUG_PRINT_L4( "--------------------------------\r\n" );
+ DEBUG_PRINT_L4( " I\r\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", setValue.operation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " JS operation mode change: %d\r\n", setValue.operation.sv_JS_OpeMode );
+ DEBUG_PRINT_L4( "--------------------------------\r\n" );
flg_exp_status |= 0x00000001;
}
else if( btnStatus_CrossDn == btnID_CrossDn ){ // KO Shape
+ flg_mutex.lock();
setValue.operation.sv_JS_ShapeMode = 1;
- DEBUG_PRINT_L4( "-------------------------\n" );
- DEBUG_PRINT_L4( " KO\n" );
- DEBUG_PRINT_L4( " JS shape mode change : %d\n", setValue.operation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " JS operation mode change: %d\n", setValue.operation.sv_JS_OpeMode );
- DEBUG_PRINT_L4( "-------------------------\n" );
+ flg_mutex.unlock();
+ DEBUG_PRINT_L4( "-------------------------\r\n" );
+ DEBUG_PRINT_L4( " KO\r\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", setValue.operation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " JS operation mode change: %d\r\n", setValue.operation.sv_JS_OpeMode );
+ DEBUG_PRINT_L4( "-------------------------\r\n" );
flg_exp_status |= 0x00000002;
}
else if( btnStatus_CrossRt == btnID_CrossRt ){ // Single JS
+ flg_mutex.lock();
setValue.operation.sv_JS_OpeMode = 0;
- DEBUG_PRINT_L4( "-----------------------------\n" );
- DEBUG_PRINT_L4( " Single\n" );
- DEBUG_PRINT_L4( " JS shape mode change : %d\n", setValue.operation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " JS operation mode change: %d\n", setValue.operation.sv_JS_OpeMode );
- DEBUG_PRINT_L4( "-----------------------------\n" );
+ flg_mutex.unlock();
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ DEBUG_PRINT_L4( " Single\r\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", setValue.operation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " JS operation mode change: %d\r\n", setValue.operation.sv_JS_OpeMode );
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
flg_exp_status |= 0x00000004;
}
else if( btnStatus_CrossLt == btnID_CrossLt ){ // Dual JS
if( setValue.operation.sv_JS_ShapeMode == 1 ){ // KO Shape
+ flg_mutex.lock();
setValue.operation.sv_JS_OpeMode = 1;
- DEBUG_PRINT_L4( "-----------------------------\n" );
- DEBUG_PRINT_L4( " Dual\n" );
- DEBUG_PRINT_L4( " JS shape mode change : %d\n", setValue.operation.sv_JS_ShapeMode);
- DEBUG_PRINT_L4( " JS operation mode change: %d\n", setValue.operation.sv_JS_OpeMode );
- DEBUG_PRINT_L4( "-----------------------------\n" );
+ flg_mutex.unlock();
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
+ DEBUG_PRINT_L4( " Dual\r\n" );
+ DEBUG_PRINT_L4( " JS shape mode change : %d\r\n", setValue.operation.sv_JS_ShapeMode);
+ DEBUG_PRINT_L4( " JS operation mode change: %d\r\n", setValue.operation.sv_JS_OpeMode );
+ DEBUG_PRINT_L4( "-----------------------------\r\n" );
flg_exp_status |= 0x00000008;
}
}
@@ -492,23 +507,26 @@
* +-+-+-+-+-+-+-+-+
*/
if( setValue.operation.sv_JS_OpeMode == 1 ){
+ I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
+ I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
+ I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
+ I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
if( btnStatus_LJSFwdRvs < setValue.crawlerCtrl.sv_LBCM_dzc - setValue.crawlerCtrl.sv_LBCM_dzl ){
flg_ButtonOn = true;
led3 = 1;
I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
tmpSpeed = ( setValue.crawlerCtrl.sv_LBCM_dzc+1 - btnStatus_LJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc * setValue.crawlerCtrl.sv_LBCM_sli_F / 100; // Speed
I2C_cmd[7] = (char)tmpSpeed;
- DEBUG_PRINT_L3( "Bd0> ******************************\n");
- DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Fwd (Speed=%d)\n", tmpSpeed);
- DEBUG_PRINT_L3( "Bd0> ******************************\n");
- flg_exp_status |= 0x00400000;
+ DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Fwd (Speed=%d)\r\n", tmpSpeed);
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ DEBUG_PRINT_L2( "Bd0> 1: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 1: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+
+ flg_exp_status |= 0x00400000;
flg_ButtonOn = false;
}
else if( btnStatus_LJSFwdRvs > setValue.crawlerCtrl.sv_LBCM_dzc + setValue.crawlerCtrl.sv_LBCM_dzu ){
@@ -517,27 +535,22 @@
I2C_cmd[6] = MOTOR_RVS; // Motor2 RVS
tmpSpeed = ( btnStatus_LJSFwdRvs - setValue.crawlerCtrl.sv_LBCM_dzc ) * 100 / setValue.crawlerCtrl.sv_LBCM_dzc+1 * setValue.crawlerCtrl.sv_LBCM_sli_F / 100; // Speed I2C_cmd[5] = (char)tmpSpeed;
I2C_cmd[7] = (char)tmpSpeed;
- DEBUG_PRINT_L3( "Bd0> ******************************\n");
- DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Rvs (Speed=%d)\n", tmpSpeed);
- DEBUG_PRINT_L3( "Bd0> ******************************\n");
+ DEBUG_PRINT_L3( "Bd0> Dual Mode: L-Rvs (Speed=%d)\r\n", tmpSpeed);
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+
+ read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 2: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+
flg_exp_status |= 0x00800000;
- I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else{
I2C_cmd[6] = MOTOR_STP; // Stop
I2C_cmd[7] = 0; // Speed=0
flg_exp_status &= 0xFF3F000F;
- I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
}
@@ -547,16 +560,16 @@
I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
tmpSpeed = (( 128 - btnStatus_RJSFwdRvs ) * 100 / 127 ) * setValue.crawlerCtrl.sv_RFCM_sli_R / 100;
I2C_cmd[3] = (char)tmpSpeed;
- DEBUG_PRINT_L3( "Bd0> ******************************\n");
- DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Fwd (Speed=%d)\n", tmpSpeed);
- DEBUG_PRINT_L3( "Bd0> ******************************\n");
+ DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Fwd (Speed=%d)\r\n", tmpSpeed);
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+
+ read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+
flg_exp_status |= 0x00100000;
- I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc + setValue.crawlerCtrl.sv_RFCM_dzu ){
@@ -565,35 +578,24 @@
I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
tmpSpeed = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc+1 * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
I2C_cmd[3] = (char)tmpSpeed;
- DEBUG_PRINT_L3( "Bd0> ******************************\n");
- DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Rvs (Speed=%d)\n", tmpSpeed);
- DEBUG_PRINT_L3( "Bd0> ******************************\n");
+ DEBUG_PRINT_L3( "Bd0> Dual Mode: R-Rvs (Speed=%d)\r\n", tmpSpeed);
+ i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+
+ read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+
flg_exp_status |= 0x00200000;
- I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else{
I2C_cmd[2] = MOTOR_STP; // Stop
I2C_cmd[3] = 0; // Speed=0
flg_exp_status &= 0xFFCFFFFF;
- I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
}
- // I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- // I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- // I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- // I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- // i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
led3 = 0;
}
/* JoyStick mode 0: Single JoyStick mode
@@ -618,13 +620,16 @@
if(( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl ) && ( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu) && ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)){
flg_ButtonOn = true;
led3 = 1;
- I2C_cmd[2] = MOTOR_FWD; // Motor1 Fwd
+ if( setValue.operation.sv_JS_ShapeMode == 0 ){ // I-Shape
+ I2C_cmd[2] = MOTOR_RVS; // Motor1 Reverse
+ }
+ else{
+ I2C_cmd[2] = MOTOR_FWD; // Motor1 Fwd
+ }
I2C_cmd[3] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
I2C_cmd[7] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSFwdRvs ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
- DEBUG_PRINT_L4( "Bd0> *************************************\n");
- DEBUG_PRINT_L4( "Bd0> Single Mode: Dir1 (Speed: %d, %d)\n", I2C_cmd[5], I2C_cmd[7]);
- DEBUG_PRINT_L4( "Bd0> *************************************\n");
+ DEBUG_PRINT_L4( "Bd0> Single Mode: Dir1 (Speed: %d, %d)\r\n", I2C_cmd[5], I2C_cmd[7]);
flg_exp_status |= 0x00100000;
flg_exp_status |= 0x00400000; // 0x00500000
I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
@@ -632,19 +637,26 @@
I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- flg_ButtonOn = false;
+ read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct ); flg_ButtonOn = false;
}
else if(( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu ) && ( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu) && ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)){
flg_ButtonOn = true;
led3 = 1;
- I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
+ if( setValue.operation.sv_JS_ShapeMode == 0 ){ // I-Shape
+ I2C_cmd[2] = MOTOR_STP; // Motor1 Reverse
+ I2C_cmd[6] = MOTOR_STP; // Motor1 Reverse
+ }
+ else{
+ I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
+ I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
+ }
I2C_cmd[3] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
- I2C_cmd[6] = MOTOR_FWD; // Motor2 Fwd
I2C_cmd[7] = ( btnStatus_RJSLftRgt - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
- DEBUG_PRINT_L4( "Bd0> *************************************\n");
- DEBUG_PRINT_L4( "Bd0> Single Mode: Dir2 (Speed: %d, %d)\n", I2C_cmd[5], I2C_cmd[7]);
- DEBUG_PRINT_L4( "Bd0> *************************************\n");
+ DEBUG_PRINT_L4( "Bd0> Single Mode: Dir2 (Speed: %d, %d)\r\n", I2C_cmd[5], I2C_cmd[7]);
flg_exp_status |= 0x00200000;
flg_exp_status |= 0x00400000; // 0x00600000
I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
@@ -652,19 +664,26 @@
I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
flg_ButtonOn = false;
}
else if(( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu ) && ( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu) && ( btnStatus_RJSLftRgt > setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)){
flg_ButtonOn = true;
led3 = 1;
- I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
+ if( setValue.operation.sv_JS_ShapeMode == 0 ){ // I-Shape
+ I2C_cmd[2] = MOTOR_FWD; // Motor1 Rvs
+ }
+ else{
+ I2C_cmd[2] = MOTOR_RVS; // Motor1 Rvs
+ }
+ I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
I2C_cmd[3] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
- I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
I2C_cmd[7] = ( btnStatus_RJSFwdRvs - setValue.crawlerCtrl.sv_RFCM_dzc ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
- DEBUG_PRINT_L4( "Bd0> *************************************\n");
- DEBUG_PRINT_L4( "Bd0> Single Mode: Dir3 (Speed: %d, %d)\n", I2C_cmd[5], I2C_cmd[7]);
- DEBUG_PRINT_L4( "Bd0> *************************************\n");
+ DEBUG_PRINT_L4( "Bd0> Single Mode: Dir3 (Speed: %d, %d)\r\n", I2C_cmd[5], I2C_cmd[7]);
flg_exp_status |= 0x00200000;
flg_exp_status |= 0x00800000; // 0x00A00000
I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
@@ -672,19 +691,27 @@
I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
flg_ButtonOn = false;
}
else if(( btnStatus_RJSLftRgt < setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzu ) && ( btnStatus_RJSFwdRvs < setValue.crawlerCtrl.sv_RFCM_dzc+setValue.crawlerCtrl.sv_RFCM_dzu) && ( btnStatus_RJSFwdRvs > setValue.crawlerCtrl.sv_RFCM_dzc-setValue.crawlerCtrl.sv_RFCM_dzl)){
flg_ButtonOn = true;
led3 = 1;
- I2C_cmd[2] = MOTOR_FWD; // Motor1 Fwd
+ if( setValue.operation.sv_JS_ShapeMode == 0 ){ // I-Shape
+ I2C_cmd[2] = MOTOR_STP; // Motor1 Reverse
+ I2C_cmd[6] = MOTOR_STP; // Motor1 Reverse
+ }
+ else{
+ I2C_cmd[2] = MOTOR_FWD; // Motor1 Fwd
+ I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
+ }
I2C_cmd[3] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_F / 100; // Speed
- I2C_cmd[6] = MOTOR_RVS; // Motor2 Rvs
I2C_cmd[7] = ( setValue.crawlerCtrl.sv_RFCM_dzc+1 - btnStatus_RJSLftRgt ) * 100 / setValue.crawlerCtrl.sv_RFCM_dzc * setValue.crawlerCtrl.sv_RFCM_sli_R / 100; // Speed
- DEBUG_PRINT_L4( "Bd0> *************************************\n");
- DEBUG_PRINT_L4( "Bd0> Single Mode: Dir4 (Speed: %d, %d)\n", I2C_cmd[5], I2C_cmd[7]);
- DEBUG_PRINT_L4( "Bd0> *************************************\n");
+ DEBUG_PRINT_L4( "Bd0> Single Mode: Dir4 (Speed: %d, %d)\r\n", I2C_cmd[5], I2C_cmd[7]);
flg_exp_status |= 0x00100000;
flg_exp_status |= 0x00800000; // 0x00900000
I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
@@ -692,7 +719,11 @@
I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ read_motorCurrent( i2c_addr_crawler, I2C_readcmd, I2C_res, 3 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
flg_ButtonOn = false;
}
// ====================================
@@ -716,12 +747,6 @@
i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
}
- // I2C_cmd[4] = setValue.crawlerCtrl.sv_RFCM_ith_F;
- // I2C_cmd[5] = setValue.crawlerCtrl.sv_RFCM_ith_R;
- // I2C_cmd[8] = setValue.crawlerCtrl.sv_LBCM_ith_F;
- // I2C_cmd[9] = setValue.crawlerCtrl.sv_LBCM_ith_R;
- // i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- // i2c.write(i2c_addr_crawler, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
}
/*
* ====================================
@@ -734,14 +759,7 @@
*/
if ( btnStatus_WDN == btnID_WDN ) { // Winch Down (FWD)
flg_ButtonOn = true;
-
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 1;
- // hwbtn_OpeMutex.unlock();
-
- DEBUG_PRINT_L3( "Bd0> ****************\n" );
- DEBUG_PRINT_L3( "Bd0> BTN W-DN(Fwd)\n" );
- DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ DEBUG_PRINT_L3( "Bd0> BTN W-DN(Fwd)\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
@@ -753,13 +771,9 @@
//// = ReadWinchCurrentPosition(i2c_addr_resolver);
// ReadCurrentPositon(i2c_addr_resolver, &winchData, sizeof(winchData), 0x00 );
// winchCurrentPosition = winchData.dt_WinchCntPosition;
- ////DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\n", winchCurrentPosition );
+ ////DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
flg_exp_status |= 0x01000000;
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 0;
- // hwbtn_OpeMutex.unlock();
-
I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
@@ -767,23 +781,18 @@
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_res, NumberOfI2CCommand );
+ read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_res, 3 );
motor1_current_pct = I2C_res[0];
motor2_current_pct = I2C_res[1];
- motor_current_pct = motor1_current_pct;
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if ( btnStatus_WUP == btnID_WUP ) { // Winch Up (Rvs)
flg_ButtonOn = true;
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 1;
- // hwbtn_OpeMutex.unlock();
- DEBUG_PRINT_L3( "Bd0> ****************\n" );
- DEBUG_PRINT_L3( "Bd0> BTN W-UP(Rvs)\n" );
- DEBUG_PRINT_L3( "Bd0> ****************\n" );
+ DEBUG_PRINT_L3( "Bd0> BTN W-UP(Rvs)\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
@@ -795,24 +804,20 @@
////winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
// ReadCurrentPositon(i2c_addr_resolver, &winchData, sizeof(winchData), 0x00 );
// winchCurrentPosition = winchData.dt_WinchCntPosition;
- ////DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\n", winchCurrentPosition );
+ ////DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
flg_exp_status |= 0x02000000;
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 0;
- // hwbtn_OpeMutex.unlock();
I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_res, NumberOfI2CCommand );
+ read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_res, 3 );
motor1_current_pct = I2C_res[0];
motor2_current_pct = I2C_res[1];
- motor_current_pct = motor1_current_pct;
- motor_current_pct = I2C_res[0];
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
@@ -821,9 +826,6 @@
// ALL motor off commmand packet send
// ====================================
else {
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 1;
- // hwbtn_OpeMutex.unlock();
led3 = 0;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[2] = MOTOR_STP;
@@ -839,28 +841,12 @@
I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 0;
- // hwbtn_OpeMutex.unlock();
-
}
-// I2C_cmd[4] = setValue.winchCtrl.sv_WDM_ith_F;
-// I2C_cmd[5] = setValue.winchCtrl.sv_WDM_ith_R;
-// I2C_cmd[8] = setValue.winchCtrl.sv_WRM_ith_F;
-// I2C_cmd[9] = setValue.winchCtrl.sv_WRM_ith_R;
-// i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-// i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-
-// motor1_current_pct = I2C_res[0];
-// motor2_current_pct = I2C_res[1];
-// motor_current_pct = motor1_current_pct;
-// DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
-
//// Are these part necessary ?? ////
////winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
//ReadCurrentPositon(i2c_addr_resolver, &winchData, sizeof(winchData), 0x00 );
//winchCurrentPosition = winchData.dt_WinchCntPosition;
- ////DEBUG_PRINT_L3("Bd0> Winch Current Position (button) : %04d\n", winchCurrentPosition );
+ ////DEBUG_PRINT_L3("Bd0> Winch Current Position (button) : %04d\r\n", winchCurrentPosition );
/*
// ====================================
@@ -874,55 +860,50 @@
if ( btnStatus_RFK == btnID_RFK ) { // RF KO
flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> ****************\n" );
- DEBUG_PRINT_L3( "Bd0> BTN RF-K\n" );
- DEBUG_PRINT_L3( "Bd0> ****************\n" );
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 1;
- // hwbtn_OpeMutex.unlock();
+ DEBUG_PRINT_L3( "Bd0> BTN RF-K\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[2] = MOTOR_FWD; // Motor1 FWD
I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_F; // Speed
#endif
flg_exp_status |= 0x10000000;
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 0;
- // hwbtn_OpeMutex.unlock();
+ I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
+ I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
+ I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
+ I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
+ i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ read_motorCurrent( i2c_addr_transform, I2C_readcmd, I2C_res, 3 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
+ i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ flg_ButtonOn = false;
}
else if ( btnStatus_RFI == btnID_RFI ) { // RF I
- DEBUG_PRINT_L3( "Bd0> ****************\n" );
- DEBUG_PRINT_L3( "Bd0> BTN RF-I\n" );
- DEBUG_PRINT_L3( "Bd0> ****************\n" );
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 1;
- // hwbtn_OpeMutex.unlock();
+ DEBUG_PRINT_L3( "Bd0> BTN RF-I\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[2] = MOTOR_RVS; // Motor1 RVS
I2C_cmd[3] = setValue.tfmCtrl.sv_RFTM_sli_R; // Speed
#endif
flg_exp_status |= 0x20000000;
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 0;
- // hwbtn_OpeMutex.unlock();
-
I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ read_motorCurrent( i2c_addr_transform, I2C_readcmd, I2C_res, 3 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
flg_ButtonOn = false;
}
else if ( btnStatus_LBK == btnID_LBK ) { // LB KO
flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> ****************\n" );
- DEBUG_PRINT_L3( "Bd0> BTN LB-K\n" );
- DEBUG_PRINT_L3( "Bd0> ****************\n" );
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 1;
- // hwbtn_OpeMutex.unlock();
+ DEBUG_PRINT_L3( "Bd0> BTN LB-K\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[6] = MOTOR_FWD; // Motor2 FWD
@@ -935,21 +916,17 @@
I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ read_motorCurrent( i2c_addr_transform, I2C_readcmd, I2C_res, 3 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
-
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 0;
- // hwbtn_OpeMutex.unlock();
flg_ButtonOn = false;
}
else if ( btnStatus_LBI == btnID_LBI ) { // LB I
flg_ButtonOn = true;
- DEBUG_PRINT_L3( "Bd0> ****************\n" );
- DEBUG_PRINT_L3( "Bd0> BTN LB-I\n" );
- DEBUG_PRINT_L3( "Bd0> ****************\n" );
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 1;
- // hwbtn_OpeMutex.unlock();
+ DEBUG_PRINT_L3( "Bd0> BTN LB-I\r\n" );
led3 = 1;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[6] = MOTOR_RVS; // Motor1 RVS
@@ -962,20 +939,18 @@
I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
+ read_motorCurrent( i2c_addr_transform, I2C_readcmd, I2C_res, 3 );
+ motor1_current_pct = I2C_res[0];
+ motor2_current_pct = I2C_res[1];
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent1 (%d)%\r\n", motor1_current_pct );
+ DEBUG_PRINT_L2( "Bd0> 3: ReadMotorCurrent2 (%d)%\r\n", motor2_current_pct );
i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
-
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 0;
- // hwbtn_OpeMutex.unlock();
flg_ButtonOn = false;
}
// ====================================
// ALL motor off commmand packet send
// ====================================
else {
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 1;
- // hwbtn_OpeMutex.unlock();
led3 = 0;
#ifdef __IIC_COMAMND_SEND__
I2C_cmd[2] = MOTOR_STP;
@@ -985,9 +960,6 @@
Thread::wait(5);
#endif
flg_exp_status &= 0x0FFFFFFF;
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 0;
- // hwbtn_OpeMutex.unlock();
I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
@@ -996,19 +968,6 @@
i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
}
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 1;
- // hwbtn_OpeMutex.unlock();
-// I2C_cmd[4] = setValue.tfmCtrl.sv_RFTM_ith_F;
-// I2C_cmd[5] = setValue.tfmCtrl.sv_RFTM_ith_R;
-// I2C_cmd[8] = setValue.tfmCtrl.sv_LBTM_ith_F;
-// I2C_cmd[9] = setValue.tfmCtrl.sv_LBTM_ith_R;
- // Send Command PAcket here !
-// i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-// i2c.write(i2c_addr_transform, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board\]
- // hwbtn_OpeMutex.lock();
- // hwbtn_Opeflg = 0;
- // hwbtn_OpeMutex.unlock();
}
}
@@ -1036,38 +995,38 @@
//winchData_t winchData;
char I2C_cmd[NumberOfI2CCommand+1] = "#010000";
- char I2C_res[NumberOfI2CCommand+1] = "\0";
// winchData_t winchData;
int16_t winchCurrentPosition;
- char motor_current_pct;
+
+ int cnt = 0;
while(1){
- // DEBUG_PRINT("\nWaiting for UDP packet...\n");
+ // DEBUG_PRINT("\r\nWaiting for UDP packet...\r\n");
rcv_data_cnt = udp_server.receiveFrom(client, dbuffer, sizeof(dbuffer));
Thread::wait(10);
if( rcv_data_cnt < 0 ){
- DEBUG_PRINT_L0("Bd0> ## Receive packet fail ##\n");
+ DEBUG_PRINT_L0("Bd0> ## Receive packet fail ##\r\n");
}
else{
dbuffer[rcv_data_cnt] = '\0';
led4 = 1;
- if(!strcmp( dbuffer, "Hello Z\n" )){
+ if(!strcmp( dbuffer, "Hello Z\r\n" )){
DEBUG_PRINT_L2("Bd0> Hello Z Packet received from client by UDP\n");
char snd_data[] = "Hello I'm CrExoB2";
udp_server.sendTo(client, snd_data, sizeof(snd_data));
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
- else if(!strcmp( dbuffer, "status\n")){
- DEBUG_PRINT_L2("Bd0> Hello Status req received from client by UDP\n");
- strcpy(dbuffer,"XXXX\n");
+ else if(!strcmp( dbuffer, "status\r\n")){
+ DEBUG_PRINT_L2("Bd0> Hello Status req received from client by UDP\r\n");
+ strcpy(dbuffer,"XXXX\r\n");
udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if(!strcmp( dbuffer, "Hello")){
- DEBUG_PRINT_L2( "Bd0> Hello Packet received from [ 0x%02x ]\n", flg_exp_status );
+ DEBUG_PRINT_L2( "Bd0> Hello Packet received from [ 0x%02x ]\r\n", flg_exp_status );
/* ***************************************** */
/* Read Winch Current Position from Resolver */
@@ -1075,6 +1034,7 @@
winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
Thread::wait(10);
+ sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
// -------------------------------------
// Crawler Moving
// -------------------------------------
@@ -1083,41 +1043,41 @@
// Reverce move a
// Right rotation 6
// Left rotation 9
+
if((flg_exp_status & 0x00F00000) == 0x00500000 ){
- sprintf( dbuffer, "WCFW %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ // 01234 5678 9012 34569
+ sprintf( dbuffer, "WCFW %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
// Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00a00000 ){
- sprintf( dbuffer, "WCRV %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ sprintf( dbuffer, "WCRV %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
// Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00600000 ){
- sprintf( dbuffer, "WCRR %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ sprintf( dbuffer, "WCRR %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
// Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00900000 ){
- sprintf( dbuffer, "WCLR %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ sprintf( dbuffer, "WCLR %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
// Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00800000 ){
- sprintf( dbuffer, "LCRV %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ sprintf( dbuffer, "LCRV %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
// Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00400000 ){
- sprintf( dbuffer, "LCFW %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ sprintf( dbuffer, "LCFW %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
// Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00200000 ){
- sprintf( dbuffer, "RCRV %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ sprintf( dbuffer, "RCRV %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
// Thread::wait(10);
}
else if((flg_exp_status & 0x00F00000) == 0x00100000 ){
- sprintf( dbuffer, "RCFW %03d %04d\0\0" , motor_speed, winchCurrentPosition);
+ sprintf( dbuffer, "RCFW %03d %03d %04d\0\0" , motor1_current_pct, motor2_current_pct, winchCurrentPosition);
// Thread::wait(10);
}
- DEBUG_PRINT_L2("Bd0> >>>> Crawler >>>>>>>>>>>>>>>>>>>>\n");
- DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
- DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
// -------------------------------------
@@ -1125,137 +1085,55 @@
// Transform
// -------------------------------------
else if( flg_exp_status & 0x20000000 ){
-#ifdef __READ_CURRENT_AT_CIF_TASK__
- read_motorCurrent( i2c_addr_transform, I2C_cmd, I2C_res, NumberOfI2CCommand );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
- motor_current_pct = motor1_current_pct;
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
-#endif // __READ_CURRENT_AT_CIF_TASK__
- sprintf(dbuffer,"RF2I %03d %04d\0", motor_current_pct, winchCurrentPosition ); // RF Crawler Tfm I
- DEBUG_PRINT_L2("Bd0> >>>> Transform >>>>>>>>>>>>>>>>>>\n");
- DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
- DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ sprintf(dbuffer,"RF2I %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // RF Crawler Tfm I
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x10000000 ){
-#ifdef __READ_CURRENT_AT_CIF_TASK__
- read_motorCurrent( i2c_addr_transform, I2C_cmd, I2C_res, NumberOfI2CCommand );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
- motor_current_pct = motor1_current_pct;
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
-#endif // __READ_CURRENT_AT_CIF_TASK__
- sprintf(dbuffer,"RF2K %03d %04d\0", motor_current_pct, winchCurrentPosition ); // RF Crawler Tfm K
- DEBUG_PRINT_L2("Bd0> >>>> Transform >>>>>>>>>>>>>>>>>>\n");
- DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
- DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ sprintf(dbuffer,"RF2K %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // RF Crawler Tfm K
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x80000000 ){
-#ifdef __READ_CURRENT_AT_CIF_TASK__
- read_motorCurrent( i2c_addr_transform, I2C_cmd, I2C_res, NumberOfI2CCommand );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
- motor_current_pct = motor1_current_pct;
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
-#endif // __READ_CURRENT_AT_CIF_TASK__
- sprintf(dbuffer,"LB2I %03d %04d\0", motor_current_pct, winchCurrentPosition ); // LB Crawler Tfm I
- DEBUG_PRINT_L2("Bd0> >>>> Transform >>>>>>>>>>>>>>>>>>\n");
- DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
- DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ sprintf(dbuffer,"LB2I %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // LB Crawler Tfm I
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x40000000 ){
-#ifdef __READ_CURRENT_AT_CIF_TASK__
- read_motorCurrent( i2c_addr_transform, I2C_cmd, I2C_res, NumberOfI2CCommand );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
- motor_current_pct = motor1_current_pct;
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
-#endif // __READ_CURRENT_AT_CIF_TASK__
- sprintf(dbuffer,"LB2K %03d %04d\0", motor_current_pct, winchCurrentPosition ); // LB Crawler Tfm K
- DEBUG_PRINT_L2("Bd0> >>>> Transform >>>>>>>>>>>>>>>>>>\n");
- DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
- DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ sprintf(dbuffer,"LB2K %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // LB Crawler Tfm K
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
// -------------------------------------
// Winch Moving
// -------------------------------------
else if( flg_exp_status & 0x01000000 ){ // Wincd down (FWD)
-#ifdef __READ_CURRENT_AT_CIF_TASK__
- read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_res, NumberOfI2CCommand );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
- motor_current_pct = motor1_current_pct;
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
-#endif // __READ_CURRENT_AT_CIF_TASK__
- sprintf(dbuffer,"WCDN %03d %04d\0", motor_current_pct, winchCurrentPosition ); // Winch down
- DEBUG_PRINT_L2("Bd0> >>>> Winch >>>>>>>>>>>>>>>>>>>>>>\n");
- DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
- DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ sprintf(dbuffer,"WCDN %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x02000000 ){ // Winch up (RVS)
-#ifdef __READ_CURRENT_AT_CIF_TASK__
- read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_res, NumberOfI2CCommand );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
- motor_current_pct = motor1_current_pct;
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
-#endif // __READ_CURRENT_AT_CIF_TASK__
- sprintf(dbuffer,"WCUP %03d %04d\0", motor_current_pct, winchCurrentPosition ); // Winch down
- DEBUG_PRINT_L2("Bd0> >>>> Winch >>>>>>>>>>>>>>>>>>>>>>\n");
- DEBUG_PRINT_L2("Bd0> Send2Client: %s\n", dbuffer);
- DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ sprintf(dbuffer,"WCUP %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else if( flg_exp_status & 0x0000000f ){
-#ifdef __READ_CURRENT_AT_CIF_TASK__
- read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_res, NumberOfI2CCommand );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
- motor_current_pct = motor1_current_pct;
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
-#endif // __READ_CURRENT_AT_CIF_TASK__
- sprintf( dbuffer,"JSMD %03d %04d\0", setValue.operation.sv_JS_ShapeMode, setValue.operation.sv_JS_OpeMode );
- DEBUG_PRINT_L2("Bd0> >>>> Winch >>>>>>>>>>>>>>>>>>>>>>\n");
- DEBUG_PRINT_L2("Bd0> Send JSMode: %s\n", dbuffer);
- DEBUG_PRINT_L2("Bd0> >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+ if( cnt == 5 ){
+ sprintf( dbuffer,"JSMD %03d %03d %04d\0", setValue.operation.sv_JS_ShapeMode, setValue.operation.sv_JS_OpeMode );
+ cnt = 0;
+ }
+ else{
+ sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
+ }
+ cnt++;
+ DEBUG_PRINT_L2("Bd0> S2C: %s\r\n", dbuffer);
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
}
else{
-#ifdef __READ_CURRENT_AT_CIF_TASK__
- read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_res, NumberOfI2CCommand );
- motor1_current_pct = I2C_res[0];
- motor2_current_pct = I2C_res[1];
- motor_current_pct = motor1_current_pct;
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
- DEBUG_PRINT_L2( "Bd0> ReadMotorCurrent (%d)%\n", motor_current_pct );
- DEBUG_PRINT_L2( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
-#endif // __READ_CURRENT_AT_CIF_TASK__
+ sprintf( dbuffer,"OFF_ %03d %03d %04d\0", motor1_current_pct, motor2_current_pct, winchCurrentPosition ); // Winch down
Thread::wait(10); // When this period seto to short time, gamepad command can not get. 50msec OK
- // sprintf( dbuffer,"OFF_ %03d %04d\0", motor_current_pct, winchCurrentPosition ); // Winch down
}
/* Send data to client PC */
- DEBUG_PRINT_L2(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
- DEBUG_PRINT_L2(">>>> Send data 2 client PC >>>>\n");
- DEBUG_PRINT_L2(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
udp_server.sendTo(client, dbuffer, sizeof(dbuffer));
Thread::wait(10);
}
@@ -1301,10 +1179,11 @@
led2 = ON;
// led1 = 1;
+ // Send status to Handy Ctrl controller, but currently this is only for Main Controller.
I2C_cmd[4] = 0x00;
I2C_cmd[5] = 0x01;
- i2c.write(i2c_addr_handy, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- DEBUG_PRINT_L2("Bd0>Send Handy Controller(%02x) >>>>>>>>>>\n",i2c_addr_handy );
+ i2c.write(i2c_addr_handy, I2C_cmd, NumberOfI2CCommand);
+ // DEBUG_PRINT_L2("Bd0> Send Handy Controller(%02x) >>>>>>>>>>\r\n",i2c_addr_handy );
Thread::wait(500);
}
@@ -1318,10 +1197,11 @@
{
FILE *wfp;
- DEBUG_PRINT_L3("\nBd0> Local file system write ... ");
+ flg_mutex.lock();
+ DEBUG_PRINT_L3("\r\nBd0> Local file system write ... ");
wfp = fopen("/local/set.dat", "wb"); // Open local file "set.txt" for writing
if(!wfp){
- DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\n");
+ DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
}
else{
Thread::wait(200);
@@ -1329,8 +1209,9 @@
Thread::wait(200);
fclose(wfp);
- DEBUG_PRINT_L3("file writen\n");
+ DEBUG_PRINT_L3("file writen\r\n");
}
+ flg_mutex.unlock();
}
// ======================================================================
@@ -1339,26 +1220,72 @@
int read_Setting_lfs( void )
{
FILE *rfp;
+ int rts;
- DEBUG_PRINT_L3("\nBd0> Read Setting data from local file system \n");
- DEBUG_PRINT_L3( "Bd0> ---------------------------------------- \n");
+ //setValue_t lsetValue; // Setting Data
+
+ DEBUG_PRINT_L3("\r\nBd0> Read Setting data from local file system \r\n");
+ DEBUG_PRINT_L3( "Bd0> ---------------------------------------- \r\n");
rfp = fopen("/local/set.dat", "rb"); // Open local file "set.txt" for writing
if(!rfp){
- DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\n");
- return _FAIL_;
+ DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
+ rts = _FAIL_;
}
else{
- Thread::wait(500);
-
+ Thread::wait(100);
+ DEBUG_PRINT_L3("fread\r\n");
+ flg_mutex.lock();
fread( (int8_t*)&setValue, sizeof(int8_t), sizeof(setValue), rfp );
+ DEBUG_PRINT_L3("fread done\r\n");
dspSetValue2Console(&pc, &setValue);
+ flg_mutex.unlock();
- Thread::wait(500);
+ // Thread::wait(500);
+ DEBUG_PRINT_L3("fclose\r\n");
fclose(rfp);
- DEBUG_PRINT_L3("\n");
-
- return _OK_;
- }
+ DEBUG_PRINT_L3("\r\n");
+
+ rts = _OK_;
+ }
+ flg_mutex.unlock();
+ return rts;
+}
+
+// ======================================================================
+// Read setting value from lpcal file system of mbed
+// ======================================================================
+int read_Setting2_lfs( void )
+{
+ FILE *rfp;
+ int rts;
+
+// setValue_t lsetValue; // Setting Data
+
+ DEBUG_PRINT_L3("\r\nBd0> Read Setting data from local file system \r\n");
+ DEBUG_PRINT_L3( "Bd0> ---------------------------------------- \r\n");
+ rfp = fopen("/local/set.dat", "rb"); // Open local file "set.txt" for writing
+ if(!rfp){
+ DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
+ rts = _FAIL_;
+ }
+ else{
+ Thread::wait(100);
+ DEBUG_PRINT_L3("fread\r\n");
+ flg_mutex.lock();
+ fread( (int8_t*)&setValue, sizeof(int8_t), sizeof(setValue), rfp );
+ DEBUG_PRINT_L3("fread done\r\n");
+ dspSetValue2Console(&pc, &setValue);
+ flg_mutex.unlock();
+
+ // Thread::wait(500);
+ DEBUG_PRINT_L3("fclose\r\n");
+ fclose(rfp);
+ DEBUG_PRINT_L3("\r\n");
+
+ rts = _OK_;
+ }
+ flg_mutex.unlock();
+ return rts;
}
// ======================================================================
@@ -1371,11 +1298,10 @@
// char subnet_mask[20];
// char gateway[20];
- DEBUG_PRINT_L3("\nBd0> Read Network Setting data from local file system \n");
- DEBUG_PRINT_L3( "Bd0> nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn \n");
+ DEBUG_PRINT_L3("\r\nBd0> Read Network Setting data from local file system \r\n");
rfp = fopen("/local/net.dat", "r"); // Open local file "set.txt" for writing
if(!rfp){
- DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\n");
+ DEBUG_PRINT_L3("Bd0> **!!local file open error!!**\r\n");
return _FAIL_;
}
else{
@@ -1385,15 +1311,15 @@
fscanf(rfp, "%s", subnet_mask);
fscanf(rfp, "%s", gateway);
- DEBUG_PRINT_L3("Bd0> nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn\n");
- DEBUG_PRINT_L3("Bd0> ip address : %s\n", ip_address);
- DEBUG_PRINT_L3("Bd0> subnet mask : %s\n", subnet_mask);
- DEBUG_PRINT_L3("Bd0> default gateway: %s\n", gateway);
- DEBUG_PRINT_L3("Bd0> nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn\n");
+ DEBUG_PRINT_L3("Bd0> ---------------------------------------\r\n");
+ DEBUG_PRINT_L3("Bd0> ip address : %s\r\n", ip_address);
+ DEBUG_PRINT_L3("Bd0> subnet mask : %s\r\n", subnet_mask);
+ DEBUG_PRINT_L3("Bd0> default gateway: %s\r\n", gateway);
+ DEBUG_PRINT_L3("Bd0> ---------------------------------------\r\n");
Thread::wait(500);
fclose(rfp);
- DEBUG_PRINT_L3("\n");
+ DEBUG_PRINT_L3("\r\n");
return _OK_;
}
@@ -1418,7 +1344,7 @@
uint16_t winchCurrentPosition;
char I2C_read[NumberOfI2CCommand+1];
- char I2C_resdata[2]; // Resolver read data
+ char I2C_readcmd[NumberOfI2CCommand+1];
// if (hwbtn_Opeflg == 1){
// Thread::wait(1);
@@ -1435,16 +1361,16 @@
if( mode == WINCH_POSITION_CLEAR ){
led3 = ON;
- DEBUG_PRINT_L3("Bd0> === WINCH_POSITION_CLEAR ===\n");
+ DEBUG_PRINT_L3("Bd0> === WINCH_POSITION_CLEAR ===\r\n");
rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
- DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\n", rcv_data_cnt );
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
if( rcv_data_cnt < 0 ){
- DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\n" );
+ DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
// tcp_client.send( (char*)winchDataP, winchData_s);
// break;
}
else{
- DEBUG_PRINT_L3("Bd0> %s\n", dbuffer );
+ DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
// if( !strcmp( dbuffer, "WinchStepDnOf" ) ){
// break;
// }
@@ -1456,22 +1382,19 @@
led3 = OFF;
}
else if (( mode == WINCH_MMODE_RELATIVE ) || ( mode == WINCH_MMODE_ABSOLUTE )){
- if ( mode == WINCH_MMODE_RELATIVE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_RELATIVE === \n");
- if ( mode == WINCH_MMODE_ABSOLUTE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_ABSOLUTE === \n");
+ if ( mode == WINCH_MMODE_RELATIVE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_RELATIVE === \r\n");
+ if ( mode == WINCH_MMODE_ABSOLUTE ) DEBUG_PRINT_L3("Bd0> === WINCH_MMODE_ABSOLUTE === \r\n");
rcv_data_cnt = tcp_client.receive( dbuffer, dbuffer_s);
*(dbufferP+rcv_data_cnt) = '\0';
- winchDataP->operation = '\n';
+ winchDataP->operation = '\r\n';
- DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\n", rcv_data_cnt );
+ DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
// Copy received data to Winch data structure.
memcpy( winchDataP, (winchData_t *)dbuffer, winchData_s);
- //// ##########################
- ////zzz winchDataP->dt_WinchCntPosition = res_position; // Current Position is winch data. But currently this value will be clear after reset MC.
- //// ##########################
// winchDataP->dt_WinchDstPosition += winchDataP->dt_WinchCntPosition;
- DEBUG_PRINT_L3("Bd0> Winch Rtv Move [ From %04d >>> To %04d ]\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
+ DEBUG_PRINT_L3("Bd0> Winch Rtv Move [ From %04d >>> To %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
swbtn_OpeMutex.lock();
swbtn_Opeflg = 1;
@@ -1482,24 +1405,24 @@
led3 = ON;
////winchDataP->dt_WinchCntPosition = res_position; // Current position.
- DEBUG_PRINT_L3("Bd0> == Winch Position ==============\n");
- DEBUG_PRINT_L3("Bd0> CURRENT : %d\n", winchDataP->dt_WinchCntPosition );
- DEBUG_PRINT_L3("Bd0> DESTINATION: %d\n", winchDataP->dt_WinchDstPosition );
- DEBUG_PRINT_L3("Bd0> ================================\n");
+ DEBUG_PRINT_L3("Bd0> == Winch Position ==============\r\n");
+ DEBUG_PRINT_L3("Bd0> CURRENT : %d\r\n", winchDataP->dt_WinchCntPosition );
+ DEBUG_PRINT_L3("Bd0> DESTINATION: %d\r\n", winchDataP->dt_WinchDstPosition );
+ DEBUG_PRINT_L3("Bd0> ================================\r\n");
tcp_client.send( (char*)winchDataP, winchData_s);
- DEBUG_PRINT_L3("Bd0> Send Winch Rtv data [ %04d >>>> %04d ]\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
+ DEBUG_PRINT_L3("Bd0> Send Winch Rtv data [ %04d >>>> %04d ]\r\n", winchDataP->dt_WinchCntPosition, winchDataP->dt_WinchDstPosition );
rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
- DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\n", rcv_data_cnt );
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
if( rcv_data_cnt < 0 ){
- DEBUG_PRINT_L3("##ERROR## Data receive\n" );
+ DEBUG_PRINT_L3("##ERROR## Data receive\r\n" );
// tcp_client.send( (char*)winchDataP, winchData_s);
break;
}
else{
- DEBUG_PRINT_L3("Bd0> %s\n", dbuffer );
+ DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
if( !strcmp( dbuffer, "WinchRtvStop" ) ){
flg_stop_operation = true;
break;
@@ -1525,30 +1448,27 @@
}
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
// Read winch current position from Resolver.
winchCurrentPosition = ReadWinchCurrentPosition( i2c_addr_resolver );
winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
winchDataP->operation = 0x00;
//i2c.read(i2c_addr_resolver, I2C_resdata, 2); // Read
//res_position = (I2C_resdata[1] << 8) | I2C_resdata[0];
- DEBUG_PRINT_L3("Bd0> DISTANCE from [ 0x%2x : %04x (%02x %02x) ]\n", i2c_addr_resolver, winchCurrentPosition, I2C_resdata[1], I2C_resdata[0]);
// --------------------------------------
// Read motor current
// --------------------------------------
- read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_read, NumberOfI2CCommand );
+ read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_read, 3 );
winchDataP->dt_WinchMotor1Current = I2C_read[0]; // Motor current set
winchDataP->dt_WinchMotor2Current = I2C_read[1]; // Motor current set
winchDataP->operation = I2C_read[2];
- DEBUG_PRINT_L3( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
- DEBUG_PRINT_L3( "Bd0> ReadMotorCurrent (%d)%\n", winchDataP->dt_WinchMotor1Current );
- DEBUG_PRINT_L3( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
- DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\n", i2c_addr_winch, I2C_read[0]);
+ DEBUG_PRINT_L3( "Bd0> 15: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
+ DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", i2c_addr_winch, I2C_read[0]);
led3 = OFF;
if( abs ( winchDataP->dt_WinchCntPosition - winchDataP->dt_WinchDstPosition) <= 0 ){
break;
}
- Thread::wait(10); // Time interval for program debugging
+ Thread::wait(2); // Time interval for program debugging
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
}
I2C_cmd[2] = MOTOR_STP; // Motor1 STOP
I2C_cmd[3] = 0; // Speed
@@ -1564,13 +1484,13 @@
winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
winchDataP->operation = 0x00;
//winchCurrentPosition = res_position; // Set curretn winch position that send for Host PC here when auto move mode.
- DEBUG_PRINT_L3("Bd0> Check destination is same to setting point or not: %d\n", winchCurrentPosition);
+ DEBUG_PRINT_L3("Bd0> Check destination is same to setting point or not: %d\r\n", winchCurrentPosition);
if( winchDataP->dt_WinchDstPosition == winchDataP->dt_WinchCntPosition ){
- DEBUG_PRINT_L3("Bd0> Destination is same to setting point, then stop operation\n" );
+ DEBUG_PRINT_L3("Bd0> Destination is same to setting point, then stop operation\r\n" );
break; // When final destination == set point , then break. else adjust position again.
}
if( flg_stop_operation == true ){
- DEBUG_PRINT_L3("Bd0> Winch auto operation force stop\n" );
+ DEBUG_PRINT_L3("Bd0> Winch auto operation force stop\r\n" );
flg_stop_operation = false;
break;
}
@@ -1581,7 +1501,7 @@
winchCurrentPosition = ReadWinchCurrentPosition( i2c_addr_resolver );
winchDataP->dt_WinchCntPosition = winchCurrentPosition; // Current position.
winchDataP->operation = 0x77;
- DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\n", winchCurrentPosition);
+ DEBUG_PRINT_L3("Bd0> Send final winch position to PC: %d\r\n", winchCurrentPosition);
tcp_client.send( (char*)winchDataP, winchData_s);
@@ -1598,25 +1518,24 @@
// ----------------------------------------------------------
else if (( mode == WINCH_STEPDOWN_BTN_ON )||( mode == WINCH_U_STEPDOWN_BTN_ON )) {
- if ( mode == WINCH_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPDOWN_BTN_ON ===\n" );
- if ( mode == WINCH_U_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPDOWN_BTN_ON ===\n" );
+ if ( mode == WINCH_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPDOWN_BTN_ON ===\r\n" );
+ if ( mode == WINCH_U_STEPDOWN_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPDOWN_BTN_ON ===\r\n" );
swbtn_OpeMutex.lock();
swbtn_Opeflg = 1;
swbtn_OpeMutex.unlock();
- man_speed = 50;
while( 1 ){
led3 = ON;
- DEBUG_PRINT_L3("Bd0> WINCH_STEPDOWN_BTN_ON\n");
+ DEBUG_PRINT_L3("Bd0> WINCH_STEPDOWN_BTN_ON\r\n");
rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
- DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\n", rcv_data_cnt );
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
if( rcv_data_cnt < 0 ){
- DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\n" );
+ DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
// tcp_client.send( (char*)winchDataP, winchData_s);
break;
}
else{
- DEBUG_PRINT_L3("Bd0> %s\n", dbuffer );
+ DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
if(( !strcmp( dbuffer, "WinchStepDnOf" ))||( !strcmp( dbuffer, "WinchuStepDnOf" )) ){
break;
}
@@ -1629,46 +1548,44 @@
I2C_cmd[3] = man_speed; // Speed
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
led3 = OFF;
- read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_read, NumberOfI2CCommand );
+ read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_read, 3 );
winchDataP->dt_WinchMotor1Current = I2C_read[0];
winchDataP->dt_WinchMotor2Current = I2C_read[1];
winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
- DEBUG_PRINT_L3( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
- DEBUG_PRINT_L3( "Bd0> ReadMotorCurrent (%d)%\n", winchDataP->dt_WinchMotor1Current );
- DEBUG_PRINT_L3( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L3( "Bd0> 16: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
if( winchDataP->operation == 0x88 ){
winchDataP->dt_WinchMotor1Current = 0xFF;
}
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\n", i2c_addr_winch, I2C_read[0]);
+ DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", i2c_addr_winch, I2C_read[0]);
winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
winchDataP->dt_WinchCntPosition = winchCurrentPosition;
winchDataP->operation = 0x00;
// ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x00 );
tcp_client.send( (char*)winchDataP, winchData_s);
- DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\n", winchCurrentPosition );
-
- Thread::wait(10); // Time interval for program debugging
+ DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
+ // Thread::wait(2); // Time interval for program debugging
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
}
- DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\n" );
+ DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
I2C_cmd[2] = MOTOR_STP; // Motor1 FWD
I2C_cmd[3] = 0; // Speed
I2C_cmd[6] = MOTOR_STP; // Motor2 FWD
I2C_cmd[7] = 0; // Speed
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\n" );
+ DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
winchDataP->dt_WinchCntPosition = winchCurrentPosition;
winchDataP->operation = 0x77;
//ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x77 );
tcp_client.send( (char*)winchDataP, winchData_s);
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
swbtn_OpeMutex.lock();
swbtn_Opeflg = 0;
@@ -1681,30 +1598,26 @@
// ----------------------------------------------------------
else if (( mode == WINCH_STEPUP_BTN_ON )||( mode == WINCH_U_STEPUP_BTN_ON )) {
- if ( mode == WINCH_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPUP_BTN_ON ===\n" );
- if ( mode == WINCH_U_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPUP_BTN_ON ===\n" );
+ if ( mode == WINCH_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_STEPUP_BTN_ON ===\r\n" );
+ if ( mode == WINCH_U_STEPUP_BTN_ON ) DEBUG_PRINT_L3( "Bd0> === WINCH_U_STEPUP_BTN_ON ===\r\n" );
swbtn_OpeMutex.lock();
swbtn_Opeflg = 1;
swbtn_OpeMutex.unlock();
- man_speed = 1;
while( 1 ){
led3 = ON;
- DEBUG_PRINT_L3("Bd0> WINCH_STEPUP_BTN_ON\n");
+ DEBUG_PRINT_L3("Bd0> WINCH_STEPUP_BTN_ON\r\n");
rcv_data_cnt = tcp_client.receive(dbuffer, dbuffer_s);
- DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\n", rcv_data_cnt );
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
if( rcv_data_cnt < 0 ){
- DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\n" );
+ DEBUG_PRINT_L3("Bd0> ##ERROR## Data receive\r\n" );
// tcp_client.send( (char*)winchDataP, winchData_s);
break;
}
else{
- DEBUG_PRINT_L3("Bd0> %s\n", dbuffer );
+ DEBUG_PRINT_L3("Bd0> %s\r\n", dbuffer );
if(
- ( !strcmp( dbuffer, "WinchStepUpOf" ))
- ||
- (!strcmp( dbuffer, "WinchuStepUpOf" ))
- ){
+ ( !strcmp( dbuffer, "WinchStepUpOf" ))||(!strcmp( dbuffer, "WinchuStepUpOf" )) ){
break;
}
}
@@ -1716,47 +1629,44 @@
I2C_cmd[3] = man_speed; // Speed
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
led3 = OFF;
- read_motorCurrent( i2c_addr_winch, I2C_cmd, I2C_read, NumberOfI2CCommand );
+ read_motorCurrent( i2c_addr_winch, I2C_readcmd, I2C_read, 3 );
winchDataP->dt_WinchMotor1Current = I2C_read[0];
winchDataP->dt_WinchMotor2Current = I2C_read[1];
winchDataP->operation = I2C_read[2]; // This is motor lock status inform to PC.
- DEBUG_PRINT_L3( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
- DEBUG_PRINT_L3( "Bd0> ReadMotorCurrent (%d)%\n", winchDataP->dt_WinchMotor1Current );
- DEBUG_PRINT_L3( "Bd0> RRRRRRRRRRRRRRRRRRRRRRRR\n" );
+ DEBUG_PRINT_L3( "Bd0> 17: ReadMotorCurrent (%d)%\r\n", winchDataP->dt_WinchMotor1Current );
if( winchDataP->operation == 0x88 ){
winchDataP->dt_WinchMotor1Current = 0xFF;
}
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\n", i2c_addr_winch, I2C_read[0]);
+ DEBUG_PRINT_L3("Bd0> Return from [ 0x%2x : %d ]\r\n", i2c_addr_winch, I2C_read[0]);
winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
winchDataP->dt_WinchCntPosition = winchCurrentPosition;
winchDataP->operation = 0x00;
// ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x00 );
tcp_client.send( (char*)winchDataP, winchData_s);
- DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\n", winchCurrentPosition );
-
- Thread::wait(10); // Time interval for program debugging
+ DEBUG_PRINT_L3( "Bd0> Send Winch Position: %d\r\n", winchCurrentPosition );
+ // Thread::wait(2); // Time interval for program debugging
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
}
- DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\n" );
+ DEBUG_PRINT_L3( "Bd0> Send Motor Stop by IIC\r\n" );
I2C_cmd[2] = MOTOR_STP; // Motor1 FWD
I2C_cmd[3] = 0; // Speed
I2C_cmd[6] = MOTOR_STP; // Motor2 FWD
I2C_cmd[7] = 0; // Speed
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
-
-
- DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\n" );
+
+ DEBUG_PRINT_L3( "Bd0> Send 0x77 2 PC\r\n" );
winchCurrentPosition = ReadWinchCurrentPosition(i2c_addr_resolver);
winchDataP->dt_WinchCntPosition = winchCurrentPosition;
winchDataP->operation = 0x77;
// ReadCurrentPositon(i2c_addr_resolver, winchDataP, winchData_s, 0x77 );
tcp_client.send( (char*)winchDataP, winchData_s);
+ i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
swbtn_OpeMutex.lock();
swbtn_Opeflg = 0;
@@ -1766,7 +1676,7 @@
}
else {
- DEBUG_PRINT_L3("STEPSTEPSTEPSTEPSTEP\n");
+ // DEBUG_PRINT_L3("STEPSTEPSTEPSTEPSTEP\r\n");
}
// }
}
@@ -1781,7 +1691,7 @@
winchData_t winchData;
char I2C_cmd[NumberOfI2CCommand+1] = "#010000000";
-// char I2C_res[NumberOfI2CCommand+1] = "\0";
+ char I2C_res[NumberOfI2CCommand+1] = "\0";
char ip_address[20];;
char subnet_mask[20];
@@ -1801,30 +1711,30 @@
cf_led_demo( &led1, &led2, &led3, &led4, 15, 35 );
DEBUG_PRINT_L0("\r\n");
- DEBUG_PRINT_L0("Bd0> +--------------------------------------------------------------\n");
- DEBUG_PRINT_L0("Bd0> | Project: B2 Crawler Explorer for 1F-1 PCV internal inspection\n");
- DEBUG_PRINT_L0("Bd0> |---------\n");
- DEBUG_PRINT_L0("Bd0> | This is: Main Control Program of Main Controller\n");
+ DEBUG_PRINT_L0("Bd0> +--------------------------------------------------------------\r\n");
+ DEBUG_PRINT_L0("Bd0> | Project: B2 Crawler Explorer for 1F-1 PCV internal inspection\r\n");
+ DEBUG_PRINT_L0("Bd0> |---------\r\n");
+ DEBUG_PRINT_L0("Bd0> | This is: Main Control Program of Main Controller\r\n");
DEBUG_PRINT_L0("Bd0> | Target MCU: mbed LPC1768\r\n");
DEBUG_PRINT_L0("Bd0> | Letest update: %s\r\n", LatestUpDate);
DEBUG_PRINT_L0("Bd0> | Program Revision: %s\r\n", ProgramRevision);
DEBUG_PRINT_L0("Bd0> | Author: %s\r\n", Author);
DEBUG_PRINT_L0("Bd0> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
- DEBUG_PRINT_L0("Bd0> ---------------------------------------------------------------\n");
+ DEBUG_PRINT_L0("Bd0> ---------------------------------------------------------------\r\n");
sprintf( ttt, "%s", ProgramRevision );
- DEBUG_PRINT_L0("Bd0> Start ststem initializing ...\n");
- DEBUG_PRINT_L0("Bd0> ------------------------\n");
- DEBUG_PRINT_L0("Bd0> Initalizing Ethernet ...\n");
- DEBUG_PRINT_L0("Bd0> ------------------------\n");
+ DEBUG_PRINT_L0("Bd0> Start ststem initializing ...\r\n");
+ DEBUG_PRINT_L0("Bd0> ------------------------\r\n");
+ DEBUG_PRINT_L0("Bd0> Initalizing Ethernet ...\r\n");
+ DEBUG_PRINT_L0("Bd0> ------------------------\r\n");
read_NetSetting_lfs( ip_address, subnet_mask, gateway );
- DEBUG_PRINT_L0("Bd0> nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn\n");
- DEBUG_PRINT_L0("Bd0> ip address : %s\n", ip_address);
- DEBUG_PRINT_L0("Bd0> subnet mask : %s\n", subnet_mask);
- DEBUG_PRINT_L0("Bd0> default gateway: %s\n", gateway);
- DEBUG_PRINT_L0("Bd0> nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn\n");
+ DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
+ DEBUG_PRINT_L0("Bd0> ip address : %s\r\n", ip_address);
+ DEBUG_PRINT_L0("Bd0> subnet mask : %s\r\n", subnet_mask);
+ DEBUG_PRINT_L0("Bd0> default gateway: %s\r\n", gateway);
+ DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
#ifdef __ETERNET_DHCP__
ret = eth.init(); // Use DHCP
@@ -1836,12 +1746,12 @@
);
#endif // __ETERNET_DHCP__
if( ret == 0 ){
- DEBUG_PRINT_L0("Bd0> Eternet init ... OK\n");
+ DEBUG_PRINT_L0("Bd0> Eternet init ... OK\r\n");
ret = eth.connect();
if( ret == 0 ){
cf_led_onoff( &led1,&led2,&led3,&led4, false, false, false, true );
- DEBUG_PRINT_L0("Bd0> Eternat connect ... OK\n");
- DEBUG_PRINT_L0("Bd0> [ IP Address : %s ]\n", eth.getIPAddress());
+ DEBUG_PRINT_L0("Bd0> Eternat connect ... OK\r\n");
+ DEBUG_PRINT_L0("Bd0> [ IP Address : %s ]\r\n", eth.getIPAddress());
udp_server.bind(UDP_SERVER_PORT);
tcp_server.bind(TCP_SERVER_PORT);
tcp_server.listen();
@@ -1849,11 +1759,11 @@
}
else{
cf_led_error( &led1,&led2,&led3,&led4 );
- DEBUG_PRINT_L0("Bd0> ***ERROR*** Eternat connect Fali\n");
+ DEBUG_PRINT_L0("Bd0> ***ERROR*** Eternat connect Fali\r\n");
}
}
else{
- DEBUG_PRINT_L0("Bd0> *** ERROR*** Eternat init Fail\n");
+ DEBUG_PRINT_L0("Bd0> *** ERROR*** Eternat init Fail\r\n");
}
Thread::wait(500);
@@ -1863,22 +1773,22 @@
// setting file "SET.DAT".
// When error occured, LED1 will be blinking shortly.
//---------------------------------------------------
- DEBUG_PRINT_L0("Bd0> ---------------------------\n");
- DEBUG_PRINT_L0("Bd0> Read setting value from LFS\n");
- DEBUG_PRINT_L0("Bd0> ---------------------------\n");
+ DEBUG_PRINT_L0("Bd0> ---------------------------\r\n");
+ DEBUG_PRINT_L0("Bd0> Read setting value from LFS\r\n");
+ DEBUG_PRINT_L0("Bd0> ---------------------------\r\n");
try_cnt = LFS_READ_COUNT;
while( 1 ){
if( read_Setting_lfs() == _OK_ ) break;
else try_cnt -= 1;
if( try_cnt == 0 ){
- DEBUG_PRINT_L0("Bd0> ***ERROR*** LFS read error\n");
+ DEBUG_PRINT_L0("Bd0> ***ERROR*** LFS read error\r\n");
while(1){
led1 = !led1;
Thread::wait(30);
}
}
}
- DEBUG_PRINT_L0("Bd0> LFS read OK\n");
+ DEBUG_PRINT_L0("Bd0> LFS read OK\r\n");
//---------------------------------------------------
// Checking Targer LCXpresso824-MAX board here.
@@ -1886,10 +1796,10 @@
// target.
// When error occured, LED1 will blinking slowly.
//---------------------------------------------------
- DEBUG_PRINT_L0("--------------------------\n");
- DEBUG_PRINT_L0("Check the target controler\n");
- DEBUG_PRINT_L0("--------------------------\n");
-/*
+ DEBUG_PRINT_L0("--------------------------\r\n");
+ DEBUG_PRINT_L0("Check the target controler\r\n");
+ DEBUG_PRINT_L0("--------------------------\r\n");
+
try_cnt = TARGET_CHECK_COUNT;
while(1){
// **********************************************************************
@@ -1902,15 +1812,15 @@
i2c.write(i2c_addr_winch, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board.
Thread::wait(100);
i2c.read(i2c_addr_winch, I2C_res, NumberOfI2CCommand);
- DEBUG_PRINT_L0("Bd0> Try count down: %d/%d\n", try_cnt, TARGET_CHECK_COUNT );
- DEBUG_PRINT_L0("Bd0> Return from [ i2c address = 0x%2x : %s ]\n", i2c_addr_winch>>1, I2C_res);
- DEBUG_PRINT_L0("Bd0> MCTR1[Winch motor controller] found\n");
+ DEBUG_PRINT_L0("Bd0> Try count down: %d/%d\r\n", try_cnt, TARGET_CHECK_COUNT );
+ DEBUG_PRINT_L0("Bd0> Return from [ i2c address = 0x%2x : %s ]\r\n", i2c_addr_winch>>1, I2C_res);
+ DEBUG_PRINT_L0("Bd0> MCTR1[Winch motor controller] found\r\n");
if( I2C_res[0] == 'S' ){
break;
}
else try_cnt -= 1;
if( try_cnt == 0 ){
- DEBUG_PRINT_L0("Bd0> ##ERROR##\n");
+ DEBUG_PRINT_L0("Bd0> ##ERROR##\r\n");
led1 = OFF;
while(1){
led1 = !led1; // ON
@@ -1932,8 +1842,8 @@
}
}
}
- DEBUG_PRINT_L0("Bd0> Target system found\n");
-*/
+ DEBUG_PRINT_L0("Bd0> Target system found\r\n");
+
/*
**************************************************
Send Calculation Data to Resolver Controller
@@ -1946,19 +1856,19 @@
I2C_cmd[5] = (uint8_t)((setValue.winchCtrl.sv_WRS_CCableDmrx100)&0xFF); // Cable diameter lower
I2C_cmd[6] = setValue.winchCtrl.sv_WRS_RResolution; // Resolver resolution
i2c.write(i2c_addr_resolver, I2C_cmd, NumberOfI2CCommand); // Send command to motor control board
- DEBUG_PRINT_L0(" ... done\n");
+ DEBUG_PRINT_L0(" ... done\r\n");
- DEBUG_PRINT_L0("Bd0> ----------------------------------------\n");
+ DEBUG_PRINT_L0("Bd0> ----------------------------------------\r\n");
/* Max thread count is (may be ..) 2, How can I increase this , I don't know ?? */
DEBUG_PRINT_L0("Bd0> Start host interface task ... ");
Thread thread_hif(clientPC_interface_task, NULL, osPriorityHigh, 256 * 2);
- DEBUG_PRINT_L0("\nBd0> Start host gamepad task ... ");
+ DEBUG_PRINT_L0("\r\nBd0> Start host gamepad task ... ");
Thread thread_gpd(gamepad_task, NULL, osPriorityHigh, 256 * 2);
- DEBUG_PRINT_L0("\nBd0> ----------------------------------------\n");
+ DEBUG_PRINT_L0("\r\nBd0> ----------------------------------------\r\n");
- DEBUG_PRINT_L0( "Bd0> ------------------------\n");
- DEBUG_PRINT_L0( "Bd0> Initializing completed !\n");
- DEBUG_PRINT_L0( "Bd0> ------------------------\n");
+ DEBUG_PRINT_L0( "Bd0> ------------------------\r\n");
+ DEBUG_PRINT_L0( "Bd0> Initializing completed !\r\n");
+ DEBUG_PRINT_L0( "Bd0> ------------------------\r\n");
led1 = ON; // Initializing is OK then Power Indicator LED ON
@@ -1977,16 +1887,16 @@
{
tcp_server.accept(tcp_client);
tcp_client.set_blocking(false, 3500); // Timeout after (300) msec
- DEBUG_PRINT_L3("Bd0> ----------------------------\n");
- DEBUG_PRINT_L3("Bd0> TCP Connection from: %s\n", tcp_client.get_address());
- DEBUG_PRINT_L3("Bd0> ----------------------------\n");
+ DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
+ DEBUG_PRINT_L3("Bd0> TCP Connection from: %s\r\n", tcp_client.get_address());
+ DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
while(true){
rcv_data_cnt = tcp_client.receive(dbuffer, sizeof(dbuffer));
- DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\n", rcv_data_cnt );
+ DEBUG_PRINT_L3("Bd0> rcv_data_cnt = %d\r\n", rcv_data_cnt );
if( rcv_data_cnt < 0 ){
- // DEBUG_PRINT("## TCP Receive packet fail ##\n");
+ // DEBUG_PRINT("## TCP Receive packet fail ##\r\n");
break;
}
else{
@@ -2104,9 +2014,9 @@
}
else if( !strcmp( dbuffer, "SetValue" ) ){
- DEBUG_PRINT_L3("Bd0> SetValue Request from client\n");
+ DEBUG_PRINT_L3("Bd0> SetValue Request from client\r\n");
rcv_data_cnt = tcp_client.receive( dbuffer, sizeof(dbuffer));
- DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\n", rcv_data_cnt );
+ DEBUG_PRINT_L3("Bd0> TCP Rcv data count is [%d]\r\n", rcv_data_cnt );
memcpy( &setValue, (setValue_t *)dbuffer, sizeof( setValue)) ;
// Display setting value list to consol
dspSetValue2Console( &pc, &setValue );
@@ -2115,17 +2025,17 @@
write_Setting_lfs();
// Thread::wait(500);
// Read setting data from local file system
- if ( read_Setting_lfs() == _FAIL_ ){
- DEBUG_PRINT_L0("Bd0> ### ERROR### Can't read out setting data from lfs\n");
+ if ( read_Setting2_lfs() == _FAIL_ ){
+ DEBUG_PRINT_L0("Bd0> ### ERROR### Can't read out setting data from lfs\r\n");
}
}
else if(!strcmp( dbuffer, "GetValue" )){
DEBUG_PRINT_L3("Bd0> GetValue request from TCP client <-- send");
- read_Setting_lfs();
+ read_Setting2_lfs();
// Display setting value list to consol
dspSetValue2Console( &pc, &setValue );
tcp_client.send_all( (char*)&setValue, sizeof(setValue));
- DEBUG_PRINT_L2("(%d)\n", sizeof(setValue));
+ DEBUG_PRINT_L2("(%d)\r\n", sizeof(setValue));
}
}
if( rcv_data_cnt <= 0 ) break;