z ysaito
2018.07.26
P0_main.cpp
- Committer:
- sayzyas
- Date:
- 2018-07-26
- Revision:
- 0:b3376afd10d8
File content as of revision 0:b3376afd10d8:
/*
//////////////////////////////////////////////////////////
Project: B2Dash Fix falling winch test model
Data: 2017.July to September
Title: 1F-1 Explorer Project
Target: mbed LPC1768
Author: zStranger
-----------------------------------------------
##### ##### ##### ##### ##### #####
# # # # # #
# # # # # #
# # # # # #
# # # # # #
# # # # # #
##### ##### ##### ##### ##### #####
-----------------------------------------------
X X XXX XXX X XXXX XX XX X
XX X X X X X X X X X X x XX
XXX X X X X X X XXX X X XXX
XXXX X XXX X X X X X XX XXXX
XXX X X X X X X X X x XXX
XX X X X X X X X X X X X XX
X XXXX X XXX X X XXX XX X
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+ |
IIC SDA | p9 tx sdi USB D- |
IIC SCL | p10 rx scl USB D+ |
| p11 mosi CAN rd p30 |
| p12 miso CAN td p29 |
| p13 tx sck sda tx p28 | Tx(SDC2130)
| p14 rx scl rx P27 | Rx(SDC2130)
Int | p15 AIn PWM P26 |
RJS U/D | p16 AIn PWM P25 |
LJS U/D | p17 AIn PWM p24 | SW(Auto/Manuu)
| p18 AIn AOut PWM p23 |
| p19 AIn PWM p22 |
| p20 AIn PWM p21 |
+---------------------------+
//////////////////////////////////////////////////////////
*/
#include "mbed.h"
#include "TCPSocket.h"
#include <EthernetInterface.h>
#include "TCPServer.h"
#include "TCPSocket.h"
#include "common.h"
#include "debugprint.h"
#include "mtrAccess.h"
#include "i2cAccess.h"
#include "lfsAccess.h"
#include "music.h"
#include "Websocket.h"
/* ====================================================================== */
/* System configuration */
/* ====================================================================== */
Thread thread;
// One time timer
Timeout onetimer;
/* Ethernet */
EthernetInterface eth;
TCPSocket socket;
TCPServer tcp_server; // TCP Server
TCPSocket tcp_client;
SocketAddress tcp_client_addr;
/* Serial */
Serial pc(USBTX, USBRX); // UART
//Serial sdc2130(p28, p27); // Communicate with RpboteQ Driver by tx, rx
/* IIC */
//I2C i2c(p9, p10); // I2C SDA, SCL is good
/* Digital IO */
DigitalOut onbdled_1(LED1);
DigitalOut onbdled_2(LED2);
DigitalOut onbdled_3(LED3);
DigitalOut onbdled_4(LED4);
DigitalIn wch_opmode(p24); // SW: Winch operation auto = 1, Manual = 0
/* Analig I/O */
AnalogIn Rjsin_UD(p16); // R-JotStick Up/Down
AnalogIn Ljsin_UD(p17); // L-JoyStick Up/Down : Manual setting only
InterruptIn intfrom824(p15); // Interrupt pin from 824MAX
bool flg_JSon = false;
mtrAccess mtrAcs;
bool flgMtrStp = false;
waveMusic wMusic;
Mutex music_mutex;
// Local File System
LocalFileSystem local("local"); // Create the local filesystem under the name "local"
/* ====================================================================== */
/* Interrupt handler */
/* ====================================================================== */
void intRise(void) { // Port Rising
// Process
//mtrAcs.cmdControl( "XX_CABL", 7, 0 ); // Motor on
onbdled_3 = 1;
flgMtrStp = true;
}
void intFall(void) { // Port Falling
//onbdled_3 = 0;
//flgMtrStp = false;
}
int16_t adjust_jsvalue( int16_t data )
{
float speed;
speed = (float)data * (float)data * (float)100 / 16384;
return (int16_t)speed;
}
uint8_t rjs_center_value_ud = 0x80;
uint8_t ljs_center_value_ud = 0x80;
/* ====================================================================== */
/*
Joystick calibration joystick
255 Upper
+---+
|127|
+---+
0 Lower
*/
/* ====================================================================== */
bool calibrate_joystick( uint8_t rdata, uint8_t ldata ){
uint16_t tmp;
tmp = (uint16_t)rjs_center_value_ud;
tmp += (uint16_t)rdata;
tmp /= 2;
rjs_center_value_ud = (uint8_t)tmp;
tmp = (uint16_t)ljs_center_value_ud;
tmp += (uint16_t)ldata;
tmp /= 2;
ljs_center_value_ud = (uint8_t)tmp;
if
(
(((127-10) < rjs_center_value_ud )&&(rjs_center_value_ud < (127+10))) &&
(((127-10) < ljs_center_value_ud )&&(ljs_center_value_ud < (127+10)))
)
{
return true;
}
else
{
return false;
}
}
lfsAccess lfsAcs;
void set_motor_interval_flag()
{
mtrAcs.flg_timerint_motor = true;
}
/* ====================================================================== */
/* Task(Thread) : JoyStick and Switch check and operation */
/* ====================================================================== */
//void sw_task( void const *){ // for OS2
void sw_task(){ // for OS5
uint16_t rjs_ud_data;
uint8_t rjs_ud_undata;
uint16_t ljs_ud_data;
uint8_t ljs_ud_undata;
// bool jsjg = true;
int16_t rjs_speed;
int16_t ljs_speed;
int16_t sp_wdram;
int16_t sp_wcabl;
char I2C_cmd[12];
i2cAccess i2cAcs;
char msg[128];
//int dropAmount = 0;
mtrAcs.setBaudRate(115200);
DEBUG_PRINT_L0("LPC1768> Start ststem initializing ...\r\n");
DEBUG_PRINT_L0("LPC1768> -------------------------------------\r\n");
DEBUG_PRINT_L0("LPC1768> Initalizing Ethernet ...\r\n");
DEBUG_PRINT_L0("LPC1768> -------------------------------------\r\n");
// Send drop amount calculation base data to main
I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Winch is stopping (send calculate base data)
I2C_cmd[I2C_CP_WDRAM_DIA_X100_UPPER] = ( lfsAcs.setValue.wchCtrl.dram_dmtr_x100 >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_WDRAM_DIA_X100_LOWER] = lfsAcs.setValue.wchCtrl.dram_dmtr_x100 & 0xFF;
I2C_cmd[I2C_CP_ADJUST_VALUE_X10000_UPPER] = ( lfsAcs.setValue.wchCtrl.adj_val_x10000 >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_ADJUST_VALUE_X10000_LOWER] = lfsAcs.setValue.wchCtrl.adj_val_x10000 & 0xFF;
I2C_cmd[I2C_CP_RESOLVER_RESO] = lfsAcs.setValue.wchCtrl.res_resolution & 0xFF;
if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
{
DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
}
#ifdef __NETWORK_ACCESS__
const char* ip_address = "192.168.11.31";
const char* subnet_mask = "255.255.255.0";
const char* gateway = "192.168.0.0";
DEBUG_PRINT_L0("LPC1768> -- set -------------------------------\r\n");
DEBUG_PRINT_L0("LPC1768> ip address : %s\r\n", ip_address);
DEBUG_PRINT_L0("LPC1768> subnet mask : %s\r\n", subnet_mask);
DEBUG_PRINT_L0("LPC1768> default gateway: %s\r\n", gateway);
DEBUG_PRINT_L0("LPC1768> --------------------------------------\r\n");
//// eth.init(ip_address, subnet_mask, gateway); // For mbed OS 2.X ??
//eth.set_network( ip_address, subnet_mask, gateway ); // For mbed OS5 or later ??
eth.connect();
DEBUG_PRINT_L0("LPC1768> -- got -------------------------------\r\n");
DEBUG_PRINT_L0("LPC1768> The target ip address : '%s'\r\n", eth.get_ip_address());
DEBUG_PRINT_L0("LPC1768> The target netmask : '%s'\r\n", eth.get_netmask());
DEBUG_PRINT_L0("LPC1768> The target gateway : '%s'\r\n", eth.get_gateway());
DEBUG_PRINT_L0("LPC1768> The target mac address: '%s'\r\n", eth.get_mac_address());
DEBUG_PRINT_L0("LPC1768> --------------------------------------\r\n");
#endif // __NETWORK_ACCESS__
wMusic.FFPre();
while(1)
{
#ifdef __NETWORK_ACCESS__
// Create a websocket instance
Websocket ws("ws://192.168.11.10:9999/", ð);
bool connect_error = ws.connect();
// tcp_server.open(ð);
// tcp_server.bind(TCP_CMDSERVER_PORT);
// tcp_server.listen(5);
#endif // __NETWORK_ACCESS__
if( connect_error != false )
{
// Send drop amount calculation base data to main
I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Winch is stopping (send calculate base data)
I2C_cmd[I2C_CP_WDRAM_DIA_X100_UPPER] = ( lfsAcs.setValue.wchCtrl.dram_dmtr_x100 >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_WDRAM_DIA_X100_LOWER] = lfsAcs.setValue.wchCtrl.dram_dmtr_x100 & 0xFF;
I2C_cmd[I2C_CP_ADJUST_VALUE_X10000_UPPER] = ( lfsAcs.setValue.wchCtrl.adj_val_x10000 >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_ADJUST_VALUE_X10000_LOWER] = lfsAcs.setValue.wchCtrl.adj_val_x10000 & 0xFF;
I2C_cmd[I2C_CP_RESOLVER_RESO] = lfsAcs.setValue.wchCtrl.res_resolution & 0xFF;
if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
{
DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
}
if(lfsAcs.setValue.wchCtrl.move_interval_cw == 0)
{
mtrAcs.move_interval_cw = 1; // set 1us for timer because timer does not work when time is set to 0 (temporary setting)
}
else
{
mtrAcs.move_interval_cw = lfsAcs.setValue.wchCtrl.move_interval_cw;
}
if(lfsAcs.setValue.wchCtrl.move_interval_ccw == 0)
{
mtrAcs.move_interval_ccw = 1; // set 1us for timer because timer does not work when time is set to 0 (temporary setting)
}
else
{
mtrAcs.move_interval_ccw = lfsAcs.setValue.wchCtrl.move_interval_ccw;
}
#ifdef __NETWORK_ACCESS__
int error_c = ws.read(msg);
DEBUG_PRINT_L0("Received %s", msg);
#endif // __NETWORK_ACCESS__
Thread::wait(10);
#ifdef __NETWORK_ACCESS__
// tcp_server.accept(&tcp_client, &tcp_client_addr);
#endif // __NETWORK_ACCESS__
DEBUG_PRINT_L1("Calibrating joystick ... ");
for( int i = 0; i < CALIBRATION_COUNT; i++){
rjs_ud_data = Rjsin_UD.read_u16();
ljs_ud_data = Ljsin_UD.read_u16();
rjs_ud_undata = (uint8_t)(rjs_ud_data >> 8);
ljs_ud_undata = (uint8_t)(ljs_ud_data >> 8);
calibrate_joystick( rjs_ud_undata, ljs_ud_undata );
}
while(true)
{
i2cAcs.i2cReadParameters( I2C_ADDRESS_RESOLVER );
DEBUG_PRINT_SW("LPC1768> ");
DEBUG_PRINT_SW("Motor Speed: %03d,%03d,%03d,%03d\t | ", i2cAcs.sp_wdram_f, i2cAcs.sp_wdram_r, i2cAcs.sp_wcabl_f, i2cAcs.sp_wcabl_r );
DEBUG_PRINT_SW("DropAmt: %04d\t | ", i2cAcs.drop_ammount );
#ifdef __NETWORK_ACCESS__
sprintf(msg, "%d %d %d %d %d %d", i2cAcs.drop_ammount, 3500-i2cAcs.drop_ammount, i2cAcs.sp_wdram_f, i2cAcs.sp_wdram_r, i2cAcs.sp_wcabl_f, i2cAcs.sp_wcabl_r );
error_c = ws.send(msg);
DEBUG_PRINT_SW("Send(%d): %s, ", error_c, msg);
if( error_c == -1 )
{
DEBUG_PRINT_SW("** send error **\r\n");
break;
}
error_c = ws.read(msg);
DEBUG_PRINT_SW("Recv(%d): %s\t", error_c, msg);
#endif // __NETWORK_ACCESS__
rjs_ud_data = Rjsin_UD.read_u16();
ljs_ud_data = Ljsin_UD.read_u16();
rjs_ud_undata = (uint8_t)(rjs_ud_data >> 8);
ljs_ud_undata = (uint8_t)(ljs_ud_data >> 8);
if(wch_opmode==1) // SW off is auto mode (RJS only mode)
{
if( // Dead zone motor stop
( rjs_ud_undata >= (rjs_center_value_ud - DEAD_BAND)) &&
( rjs_ud_undata <= (rjs_center_value_ud + DEAD_BAND))
){
if( flg_JSon == true )
{
onetimer.detach(); // stop onetimer to call flip
mtrAcs.flg_timerint_motor = false;
}
rjs_speed = 0;
sp_wdram = rjs_speed;
sp_wcabl = rjs_speed;
// DEBUG_PRINT_SW("LPC1768> ");
DEBUG_PRINT_SW("RJSonly: %03d,%03d [%3d] : ", sp_wdram, sp_wcabl, rjs_ud_undata );
DEBUG_PRINT_SW("Dead\r\n");
mtrAcs.cmdControl( "XX_BOTH", 7, 0, 0 ); // Motor on
flg_JSon = false;
flgMtrStp = false;
mtrAcs.flg_ival_motor_cw = false;
mtrAcs.flg_ival_motor_ccw = false;
onbdled_3 = 0;
}
// Up motor reverse
else if( // data: 127 to 255
( rjs_ud_undata > (rjs_center_value_ud))
){
if( flg_JSon == false )
{
// Attch one time timer interrupt
// m1 --> timer(+/-) us --> M2
onetimer.attach_us(&set_motor_interval_flag, abs(mtrAcs.move_interval_ccw*1000)); // setup onetimer to call flip after 1 seconds
}
rjs_speed = adjust_jsvalue((int16_t)(rjs_ud_undata - rjs_center_value_ud));
sp_wdram = rjs_speed * i2cAcs.sp_wdram_r / 100;
sp_wcabl = rjs_speed * i2cAcs.sp_wcabl_r / 100;
sp_wdram *= -1;
sp_wcabl *= -1;
// DEBUG_PRINT_SW("LPC1768> ");
DEBUG_PRINT_SW("RJSonly: %03d,%03d [%3d] : ", sp_wdram, sp_wcabl, rjs_ud_undata );
DEBUG_PRINT_SW("Up(CCW)\r\n");
if ( flgMtrStp == true ) // true is interrupted from motor control board
{
mtrAcs.cmdControl( "XX_BOTH", 7, 0, 0); // Motor off
wMusic.pi(2);
mtrAcs.flg_ival_motor_cw = false;
mtrAcs.flg_ival_motor_ccw = false;
}
else
{
mtrAcs.cmdControl( "XX_BOTH", 7, sp_wdram*10, sp_wcabl*5); // Motor on
mtrAcs.flg_ival_motor_ccw = true;
}
flg_JSon = true;
}
// Down motor forward
else if( // data : 127 to 0
( rjs_ud_undata < (rjs_center_value_ud))
){
if( flg_JSon == false )
{
// Attch one time timer interrupt
// m1 --> timer(+/-) us --> M2
onetimer.attach_us(&set_motor_interval_flag, abs(mtrAcs.move_interval_cw*1000)); // setup onetimer to call flip after 1 seconds
}
rjs_speed = adjust_jsvalue((int16_t)(rjs_center_value_ud - rjs_ud_undata));
sp_wdram = rjs_speed * i2cAcs.sp_wdram_f / 100;
sp_wcabl = rjs_speed * i2cAcs.sp_wcabl_f / 100;
// DEBUG_PRINT_SW("LPC1768> ");
DEBUG_PRINT_SW("RJSonly: %03d,%03d [%3d] : ", sp_wdram, sp_wcabl, rjs_ud_undata );
DEBUG_PRINT_SW("Down(CW)\r\n");
if ( flgMtrStp == true ) // true is interrupted from motor control board
{
mtrAcs.cmdControl( "XX_BOTH", 7, 0, 0); // Motor off
wMusic.pi(4);
mtrAcs.flg_ival_motor_cw = false;
mtrAcs.flg_ival_motor_ccw = false;
}
else
{
mtrAcs.cmdControl( "XX_BOTH", 7, sp_wdram*10, sp_wcabl*5); // Motor on
mtrAcs.flg_ival_motor_cw = true;
}
flg_JSon = true;
}
}
else // SW on is manual mode (RJS and LJS independent mode)
{
/* R JS : Dram motor control */
if( // Dead zone
( rjs_ud_undata >= (rjs_center_value_ud - DEAD_BAND)) &&
( rjs_ud_undata <= (rjs_center_value_ud + DEAD_BAND))
){
rjs_speed = 0;
sp_wdram = rjs_speed;
// DEBUG_PRINT_SW("LPC1768> ");
DEBUG_PRINT_SW("R: %03d [%3d] : ", rjs_speed, rjs_ud_undata );
DEBUG_PRINT_SW("Dead\t");
mtrAcs.cmdControl( "XX_DRAM", 7, 0, 0 ); // Motor on
}
// Up motor reverse
else if( // data: 127 to 255
( rjs_ud_undata > (rjs_center_value_ud))
){
rjs_speed = adjust_jsvalue((int16_t)(rjs_ud_undata - rjs_center_value_ud));
sp_wdram = rjs_speed * i2cAcs.sp_wdram_r / 100;
sp_wdram *= -1;
// DEBUG_PRINT_SW("LPC1768> ");
DEBUG_PRINT_SW("R: %03d [%3d] : ", sp_wdram, rjs_ud_undata );
DEBUG_PRINT_SW("Up(CCW)\t");
mtrAcs.cmdControl( "XX_DRAM", 7, sp_wdram*10, 0); // Motor on
}
// Down motor forward
else if( // data : 127 to 0
( rjs_ud_undata < (rjs_center_value_ud))
){
rjs_speed = adjust_jsvalue((int16_t)(rjs_center_value_ud - rjs_ud_undata));
sp_wdram = rjs_speed * i2cAcs.sp_wdram_f / 100;
// DEBUG_PRINT_SW("LPC1768> ");
DEBUG_PRINT_SW("R:%03d [%3d] : ", sp_wdram, rjs_ud_undata );
DEBUG_PRINT_SW("Down(CW)\t");
mtrAcs.cmdControl( "XX_DRAM", 7, sp_wdram*10, 0); // Motor on
}
/* L JS : Cable motor control */
if( // Dead zone
( ljs_ud_undata >= (ljs_center_value_ud - DEAD_BAND)) &&
( ljs_ud_undata <= (ljs_center_value_ud + DEAD_BAND))
){
ljs_speed = 0;
sp_wcabl = ljs_speed;
// DEBUG_PRINT_SW("LPC1768> ");
DEBUG_PRINT_SW("L: %03d [%3d] : ", sp_wcabl, ljs_ud_undata );
DEBUG_PRINT_SW("Dead\r\n");
mtrAcs.cmdControl( "XX_CABL", 7, 0, 0 ); // Motor on
}
// Up motor reverse
else if( // data: 127 to 255
( ljs_ud_undata > (ljs_center_value_ud))
){
ljs_speed = adjust_jsvalue((int16_t)(ljs_ud_undata - ljs_center_value_ud));
sp_wcabl = ljs_speed * i2cAcs.sp_wcabl_r / 100;
sp_wcabl *= -1;
// DEBUG_PRINT_SW("LPC1768> ");
DEBUG_PRINT_SW("L:%03d [%3d] : ", sp_wcabl, ljs_ud_undata );
DEBUG_PRINT_SW("Up(CCW)\r\n");
mtrAcs.cmdControl( "XX_CABL", 7, 0, sp_wcabl*5); // Motor on
}
// Down motor forward
else if( // data : 127 to 0
( ljs_ud_undata < (ljs_center_value_ud))
){
ljs_speed = adjust_jsvalue((int16_t)(ljs_center_value_ud - ljs_ud_undata));
sp_wcabl = ljs_speed * i2cAcs.sp_wcabl_f / 100;
// DEBUG_PRINT_SW("LPC1768> ");
DEBUG_PRINT_SW("L: %d [%d] : ", sp_wcabl, ljs_ud_undata );
DEBUG_PRINT_SW("Down(CW)\r\n");
mtrAcs.cmdControl( "XX_CABL", 7, 0, sp_wcabl*5); // Motor on
}
}
if(( flg_JSon == false )&&( flgMtrStp == false ))
{
I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Winch is stopping (send calculate base data)
I2C_cmd[I2C_CP_WDRAM_DIA_X100_UPPER] = ( lfsAcs.setValue.wchCtrl.dram_dmtr_x100 >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_WDRAM_DIA_X100_LOWER] = lfsAcs.setValue.wchCtrl.dram_dmtr_x100 & 0xFF;
I2C_cmd[I2C_CP_ADJUST_VALUE_X10000_UPPER] = ( lfsAcs.setValue.wchCtrl.adj_val_x10000 >> 8 ) & 0xFF;
I2C_cmd[I2C_CP_ADJUST_VALUE_X10000_LOWER] = lfsAcs.setValue.wchCtrl.adj_val_x10000 & 0xFF;
I2C_cmd[I2C_CP_RESOLVER_RESO] = lfsAcs.setValue.wchCtrl.res_resolution & 0xFF;
if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
{
DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
}
}
else // Winch is operating
{
I2C_cmd[I2C_CP_COMMAND] = 'N';
// DEBUG_PRINT_L0("N\r\n");
if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
{
DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
}
}
Thread::wait(20);
}
}
}
}
/* ====================================================================== */
/* Main routine */
/* ====================================================================== */
int main() {
intfrom824.rise(&intRise); // Interrupt handler registration
intfrom824.fall(&intFall); // Interrupt handler registration
wch_opmode.mode(PullUp);
intfrom824.mode(PullUp);
// Serial baudrate
pc.baud(115200);
mtrAccess mtrAcs;
mtrAcs.setBaudRate(115200);
DEBUG_PRINT_L0("\r\n");
DEBUG_PRINT_L0("LPC1768> +-------------------------------------------------------------\r\n");
DEBUG_PRINT_L0("LPC1768> | Project: B2Dash Debris Explorer Winch test machine\r\n");
DEBUG_PRINT_L0("LPC1768> |-------------------------------------------------------------\r\n");
DEBUG_PRINT_L0("LPC1768> | This is: Main Control Program\r\n");
DEBUG_PRINT_L0("LPC1768> | Target MCU: mbed LPC1768\r\n");
DEBUG_PRINT_L0("LPC1768> | Letest update: %s\r\n", LatestUpDate);
DEBUG_PRINT_L0("LPC1768> | Program Revision: %s\r\n", ProgramRevision);
DEBUG_PRINT_L0("LPC1768> | Author: %s\r\n", Author);
DEBUG_PRINT_L0("LPC1768> | Copyright(C) 2017 %s Allright Reserved\r\n", Company);
DEBUG_PRINT_L0("LPC1768> +-------------------------------------------------------------\r\n");
lfsAcs.readSetting();
DEBUG_PRINT_L0("\r\n");
DEBUG_PRINT_L0("LPC1768> <Setting from LFS>\r\n");
DEBUG_PRINT_L0("LPC1768> %04d # 01 [2byte] Cnt th; winch dram Motor ( forward )\r\n", lfsAcs.setValue.wchCtrl.cth_dram_mtr_f);
DEBUG_PRINT_L0("LPC1768> %04d # 02 [2byte] Cnt th; winch dram Motor ( reverse )\r\n", lfsAcs.setValue.wchCtrl.cth_dram_mtr_r);
DEBUG_PRINT_L0("LPC1768> %04d # 03 [2byte] Cnt th; winch cabl Motor ( forward )\r\n", lfsAcs.setValue.wchCtrl.cth_cabl_mtr_f);
DEBUG_PRINT_L0("LPC1768> %04d # 04 [2byte] Cnt th; winch cabl Motor ( reverse )\r\n", lfsAcs.setValue.wchCtrl.cth_cabl_mtr_r);
DEBUG_PRINT_L0("LPC1768> %05d # 04 [2byte] Winch dram diameter x 100\r\n", lfsAcs.setValue.wchCtrl.dram_dmtr_x100);
DEBUG_PRINT_L0("LPC1768> %05d # 04 [2byte] Winch adjust value x 10000\r\n", lfsAcs.setValue.wchCtrl.adj_val_x10000);
DEBUG_PRINT_L0("LPC1768> %03d # 04 [2byte] Winch resolver resolution (bit)\r\n", lfsAcs.setValue.wchCtrl.res_resolution);
wMusic.m_poteto(1);
DEBUG_PRINT_L0("LPC1768> ------------------\r\n");
DEBUG_PRINT_L0("LPC1768> Start the task\r\n");
thread.start(sw_task);
//Thread thread_swd(sw_task, NULL, osPriorityNormal, 256 * 4); // for OS2
DEBUG_PRINT_L0("LPC1768> ------------------\r\n"); // for OS5
while(1) {
onbdled_1 = 1;
Thread::wait(30);
onbdled_1 = 0;
onbdled_2 = 1;
Thread::wait(30);
onbdled_2 = 0;
// onbdled_3 = 1;
// Thread::wait(30);
// onbdled_3 = 0;
// onbdled_4 = 1;
// Thread::wait(30);
// onbdled_4 = 0;
Thread::wait(200);
}
}