z ysaito
2018.07.26
Dependencies: EthernetInterface TextLCD USBDevice USBHost mbed
P1_0Y_main.cpp
- Committer:
- sayzyas
- Date:
- 2018-07-26
- Revision:
- 1:fdf87a1a724b
- Parent:
- 0:19075177391c
File content as of revision 1:fdf87a1a724b:
/*
////////////////////////////////////////////////////////////////////////////
Project: B1/B2 DebrisProbe Demonstration model
Title: Debris Prober Ctrl Main
Target: mbed LPC1768
Author: sayzyas as ZNR (Z is not a staff of R)
-----------------------------------------------
The Final Project of this company ! ... may be.
-----------------------------------------------
## ##### ## #####
## ## ## ## ## ##
## ## ## ##### #####
## ## ## ## ## ##
## ## ##### #####
mbed LPC1768
+-------------USB-----------+
| GND VOUT(3.3V) |
| VIN VU(5.0V OUT)|
| VB IF- |
| mR IF+ |
| p5 mosi Ether RD- |
| p6 miso Ether RD+ |
| p7 sck Ether TD- |
| p8 Ether TD+ |
| p9 tx sdi USB D- |
| p10 rx scl USB D+ |
| p11 mosi CAN rd p30 |
| p12 miso CAN td p29 |
| p13 tx sck sda tx p28 |
| p14 rx scl rx P27 |
| p15 AIn PWM P26 |
| p16 AIn PWM P25 |
| p17 AIn PWM p24 |
| p18 AIn AOut PWM p23 |
| p19 AIn PWM p22 |
| p20 AIn PWM p21 |
+---------------------------+
////////////////////////////////////////////////////////////////////////////
*/
#include "mbed.h"
#include "USBHostGamepad.h"
#include "USBSerial.h"
#include "rtos.h"
#include "EthernetInterface.h"
#include "common.h"
#include "stdio.h"
#include "com_func.h"
#include "lfsAccess.h"
#include "i2cAccess.h"
#include "mtrAccess.h"
#include "ledCtrl.h"
#include "music.h"
// USBSerial serial setting
Serial pc(USBTX, USBRX); // UART
//Serial sdc2130(p28, p27); // Communicate with RpboteQ Driver by tx, rx
// Digital I/O setting
DigitalIn limit_sw_rf(p15); // Limit switch (RF-h) for Transform controller only
DigitalIn limit_sw_lb(p16); // Limit switch (LB-I) for Transform controller only
// Ethernet
EthernetInterface eth;
TCPSocketServer tcp_server; // TCP Server
TCPSocketConnection tcp_client;
UDPSocket udp_server; // UDP Server
Endpoint client;
// Local File System
LocalFileSystem local("local"); // Create the local filesystem under the name "local"
// JKSoft Blue mbed Board Specific Seting
//AnalogOut DACout(p18);
//DigitalOut AMPEnable(p12);
DigitalIn obd_sw1(p25); // On board physical SW
DigitalIn obd_sw2(p26); // On board physical SW
// **********************************************************************
//
// Main Function of this program
//
// **********************************************************************
int main()
{
Mutex file_access_mutex;
char I2C_cmd[NumberOfI2CCommand];
int try_cnt;
int rcv_data_cnt;
bool flg_ethernet = false;
char ttt[20];
char tmp_b[6];
int direction;
int speed;
int lswstop;
char hbuf[128] = ""; // TCP send buffer
char sbuf[128] = ""; // TCP send buffer
char rbuf[128]; // TCP receive buffer
int winchPosition = 0;
bool flg_CLRF_motor_stop = false;
bool flg_CLLB_motor_stop = false;
bool flg_TFRF_motor_stop = false;
bool flg_TFLB_motor_stop = false;
bool flg_WICH_motor_stop = false;
bool flg_CPAN_motor_stop = false;
bool flg_CTLT_motor_stop = false;
bool flg_mlock_check = false;
i2cAccess i2cAcs;
lfsAccess lfsAcs;
mtrAccess mtrAcs;
ledCtrl led;
waveMusic wMusic;
obd_sw1.mode(PullUp);
obd_sw2.mode(PullUp);
// Serial baudrate
pc.baud(115200);
mtrAcs.setBaudRate(115200);
led.led_demo( 10, 15 );
/*
LED1: System boot OK
LED2
LED3: Ethernet access
LED4: Ethernet OK
*/
wMusic.PC9801();
led.led_off(1);
led.led_off(2);
led.led_off(3);
led.led_off(4);
led.led_main_off();
DEBUG_PRINT_L0("\r\n");
DEBUG_PRINT_L0("Bd0> /_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_\r\n");
DEBUG_PRINT_L0("Bd0> | Project: B1/B2 Debris Prover Demonstration machine\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> /_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_\r\n");
sprintf( ttt, "%s", ProgramRevision );
limit_sw_rf.mode( PullUp ); // use internal pullup
limit_sw_lb.mode( PullUp ); // use internal pullup
DEBUG_PRINT_L0("Bd0> Start ststem initializing ...\r\n");
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
DEBUG_PRINT_L0("Bd0> 1. Initalizing Ethernet ...\r\n");
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
const char* ip_address = "192.168.0.24";
const char* subnet_mask = "255.255.255.0";
const char* gateway = "192.168.0.0";
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");
if( eth.init( ip_address, subnet_mask, gateway ) == 0 ){
DEBUG_PRINT_L0("Bd0> Eternet init ... OK\r\n");
if( eth.connect(7000) == 0 ){
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_CMDSERVER_PORT);
tcp_server.listen();
flg_ethernet = true;
led.led_main_blink(1);
wMusic.pi(1);
//---------------------------------------------------
// Read CrExp setting value from Local File System
// setting file "SET.DAT".
// When error occured, LED1 will be blinking shortly.
//---------------------------------------------------
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
DEBUG_PRINT_L0("Bd0> 2. Read setting value from LFS\r\n");
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
// --------------------------------------------------------------------
// Read setting from local file system and set to internal structure
// --------------------------------------------------------------------
try_cnt = LFS_READ_COUNT;
while( 1 ){
if( lfsAcs.readSetting(1) == true )
{
break;
}
else
{
try_cnt -= 1;
}
if( try_cnt == 0 ){
DEBUG_PRINT_L0("Bd0> ***ERROR*** LFS read error\r\n");
while(1){
led.led_error();
led.led_main_error();
wMusic.wave( 0.6, 1500.0, 0.3 );
wMusic.wave( 0.0, 1500.0, 0.1 );
}
}
}
DEBUG_PRINT_L2( "%04d # 01-01 [2byte] Crw RF mtr fwd cnt thd\r\n", lfsAcs.setValue.crwCtrl.rf_mtr_ithd_f);
DEBUG_PRINT_L2( "%04d # 01-02 [2byte] Crw RF mtr rvs cnt thd\r\n", lfsAcs.setValue.crwCtrl.rf_mtr_ithd_r);
DEBUG_PRINT_L2( "%04d # 01-03 [2byte] Crw LB mtr fwd cnt thd\r\n", lfsAcs.setValue.crwCtrl.lb_mtr_ithd_f);
DEBUG_PRINT_L2( "%04d # 01-04 [2byte] Crw LB mtr rvs cnt thd\r\n", lfsAcs.setValue.crwCtrl.lb_mtr_ithd_r);
DEBUG_PRINT_L2( "%03d # 01-05 [1byte] Crw RF mtr fwd speed\r\n", lfsAcs.setValue.crwCtrl.rf_mtr_hspd_f);
DEBUG_PRINT_L2( "%03d # 01-06 [1byte] Crw RF mtr rvs speed\r\n", lfsAcs.setValue.crwCtrl.rf_mtr_hspd_r);
DEBUG_PRINT_L2( "%03d # 01-07 [1byte] Crw LB mtr fwd speed\r\n", lfsAcs.setValue.crwCtrl.lb_mtr_hspd_f);
DEBUG_PRINT_L2( "%03d # 01-08 [1byte] Crw LB mtr rvs speed\r\n", lfsAcs.setValue.crwCtrl.lb_mtr_hspd_r);
DEBUG_PRINT_L2( "%03d # 01-09 [1byte] reserved 1\r\n", lfsAcs.setValue.crwCtrl.reserved_1);
DEBUG_PRINT_L2( "%03d # 01-10 [1byte] reserved 2\r\n", lfsAcs.setValue.crwCtrl.reserved_2);
DEBUG_PRINT_L2( "%03d # 01-11 [1byte] reserved 3\r\n", lfsAcs.setValue.crwCtrl.reserved_3);
DEBUG_PRINT_L2( "%03d # 01-12 [1byte] reserved 4\r\n", lfsAcs.setValue.crwCtrl.reserved_4);
DEBUG_PRINT_L2( "%04d # 02-01 [2byte] Tfm RF mtr fwd cnt thd\r\n", lfsAcs.setValue.tfmCtrl.rf_mtr_ithd_f);
DEBUG_PRINT_L2( "%04d # 02-02 [2byte] Tfm RF mtr rvs cnt thd\r\n", lfsAcs.setValue.tfmCtrl.rf_mtr_ithd_r);
DEBUG_PRINT_L2( "%04d # 02-03 [2byte] Tfm LB mtr fwd cnt thd\r\n", lfsAcs.setValue.tfmCtrl.lb_mtr_ithd_f);
DEBUG_PRINT_L2( "%04d # 02-04 [2byte] Tfm LB mtr rvs cnt thd\r\n", lfsAcs.setValue.tfmCtrl.lb_mtr_ithd_r);
DEBUG_PRINT_L2( "%03d # 02-05 [1byte] Tfm RF mtr fwd speed\r\n", lfsAcs.setValue.tfmCtrl.rf_mtr_hspd_f);
DEBUG_PRINT_L2( "%03d # 02-06 [1byte] Tfm RF mtr rvs speed\r\n", lfsAcs.setValue.tfmCtrl.rf_mtr_hspd_r);
DEBUG_PRINT_L2( "%03d # 02-07 [1byte] Tfm LB mtr fwd speed\r\n", lfsAcs.setValue.tfmCtrl.lb_mtr_hspd_f);
DEBUG_PRINT_L2( "%03d # 02-08 [1byte] Tfm LB mtr rvs speed\r\n", lfsAcs.setValue.tfmCtrl.lb_mtr_hspd_r);
DEBUG_PRINT_L2( "%03d # 02-09 [1byte] reserved 1\r\n", lfsAcs.setValue.tfmCtrl.reserved_1);
DEBUG_PRINT_L2( "%03d # 02-10 [1byte] reserved 2\r\n", lfsAcs.setValue.tfmCtrl.reserved_2);
DEBUG_PRINT_L2( "%03d # 02-11 [1byte] reserved 3\r\n", lfsAcs.setValue.tfmCtrl.reserved_3);
DEBUG_PRINT_L2( "%03d # 02-12 [1byte] reserved 4\r\n", lfsAcs.setValue.tfmCtrl.reserved_4);
DEBUG_PRINT_L2( "%04d # 03-01 [2byte] Pan mtr fwd cnt thd\r\n", lfsAcs.setValue.ptlCtrl.pan_mtr_ithd_f);
DEBUG_PRINT_L2( "%04d # 03-02 [2byte] Pan mtr rvs cnt thd\r\n", lfsAcs.setValue.ptlCtrl.pan_mtr_ithd_r);
DEBUG_PRINT_L2( "%04d # 03-03 [2byte] Tlt mtr fwd cnt thd\r\n", lfsAcs.setValue.ptlCtrl.tlt_mtr_ithd_f);
DEBUG_PRINT_L2( "%04d # 03-04 [2byte] Tlt mtr rvs cnt thd\r\n", lfsAcs.setValue.ptlCtrl.tlt_mtr_ithd_r);
DEBUG_PRINT_L2( "%03d # 03-05 [1byte] Pan mtr fwd h-speed\r\n", lfsAcs.setValue.ptlCtrl.pan_mtr_hspd_f);
DEBUG_PRINT_L2( "%03d # 03-06 [1byte] Pan mtr rvs h-speed\r\n", lfsAcs.setValue.ptlCtrl.pan_mtr_hspd_r);
DEBUG_PRINT_L2( "%03d # 03-07 [1byte] Tlt mtr fwd h-speed\r\n", lfsAcs.setValue.ptlCtrl.tlt_mtr_hspd_f);
DEBUG_PRINT_L2( "%03d # 03-08 [1byte] Tlt mtr rvs h-speed\r\n", lfsAcs.setValue.ptlCtrl.tlt_mtr_hspd_r);
DEBUG_PRINT_L2( "%03d # 03-09 [1byte] reserved 1\r\n", lfsAcs.setValue.ptlCtrl.reserved_1);
DEBUG_PRINT_L2( "%03d # 03-10 [1byte] reserved 2\r\n", lfsAcs.setValue.ptlCtrl.reserved_2);
DEBUG_PRINT_L2( "%03d # 03-11 [1byte] reserved 3\r\n", lfsAcs.setValue.ptlCtrl.reserved_3);
DEBUG_PRINT_L2( "%03d # 03-12 [1byte] reserved 3\r\n", lfsAcs.setValue.ptlCtrl.reserved_4);
DEBUG_PRINT_L2( "%04d # 01 [2byte] Wch drm mtr fwd cnt thd\r\n", lfsAcs.setValue.wchCtrl.drm_mtr_ithd_f);
DEBUG_PRINT_L2( "%04d # 02 [2byte] Wch drm mtr rvs cnt thd\r\n", lfsAcs.setValue.wchCtrl.drm_mtr_ithd_r);
DEBUG_PRINT_L2( "%04d # 03 [2byte] Wch No2 mtr fwd cnt thd\r\n", lfsAcs.setValue.wchCtrl.no2_mtr_ithd_f);
DEBUG_PRINT_L2( "%04d # 04 [2byte] Wch No2 mtr rvs cnt thd\r\n", lfsAcs.setValue.wchCtrl.no2_mtr_ithd_r);
DEBUG_PRINT_L2( "%03d # 05 [1byte] Wch drm mtr fwd h-speed\r\n", lfsAcs.setValue.wchCtrl.drm_mtr_hspd_f);
DEBUG_PRINT_L2( "%03d # 06 [1byte] Wch drm mtr rvs h-speed\r\n", lfsAcs.setValue.wchCtrl.drm_mtr_hspd_r);
DEBUG_PRINT_L2( "%03d # 09 [1byte] Wch No2 mtr fwd h-speed\r\n", lfsAcs.setValue.wchCtrl.no2_mtr_hspd_f);
DEBUG_PRINT_L2( "%03d # 10 [1byte] Wch No2 mtr rvs h-speed\r\n", lfsAcs.setValue.wchCtrl.no2_mtr_hspd_r);
DEBUG_PRINT_L2( "%05d # 13 [2byte] Wch dram diameter x100\r\n", lfsAcs.setValue.wchCtrl.dram_dmtr_x100);
DEBUG_PRINT_L2( "%05d # 14 [2byte] Wch adjust value x10000\r\n", lfsAcs.setValue.wchCtrl.adj_val_x10000);
DEBUG_PRINT_L2( "%03d # 15 [1byte] Wch resolver resolution\r\n", lfsAcs.setValue.wchCtrl.res_resolution);
DEBUG_PRINT_L2( "%03d # 16 [1byte] reserved 1\r\n", lfsAcs.setValue.wchCtrl.reserved_1);
DEBUG_PRINT_L2( "%03d # 17 [1byte] reserved 2\r\n", lfsAcs.setValue.wchCtrl.reserved_2);
DEBUG_PRINT_L2( "%03d # 18 [1byte] reserved 3\r\n", lfsAcs.setValue.wchCtrl.reserved_3);
DEBUG_PRINT_L2( "%03d # 18 [1byte] reserved 3\r\n", lfsAcs.setValue.wchCtrl.reserved_4);
DEBUG_PRINT_L2( "%03d # 18 [1byte] reserved 3\r\n", lfsAcs.setValue.wchCtrl.reserved_5);
DEBUG_PRINT_L2( "%03d # 18 [1byte] reserved 3\r\n", lfsAcs.setValue.wchCtrl.reserved_6);
DEBUG_PRINT_L2( "%03d # 18 [1byte] reserved 3\r\n", lfsAcs.setValue.wchCtrl.reserved_7);
DEBUG_PRINT_L0("Bd0> LFS read OK\r\n");
led.led_main_blink(2);
wMusic.pi(2);
// **************************************************
// Send Calculation Data to Resolver Controller
// **************************************************
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
DEBUG_PRINT_L0("Bd0> 3. Send the Calculation base data to Resolver Controller\r\n");
DEBUG_PRINT_L0("Bd0> -----------------------------------------------------------\r\n");
DEBUG_PRINT_L0("Bd0> Dram diameter(x100) : %d\r\n", lfsAcs.setValue.wchCtrl.dram_dmtr_x100);
DEBUG_PRINT_L0("Bd0> Adjust value(x10000): %d\r\n", lfsAcs.setValue.wchCtrl.adj_val_x10000);
DEBUG_PRINT_L0("Bd0> Resolver resolution): %d\r\n", lfsAcs.setValue.wchCtrl.res_resolution);
DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
I2C_cmd[I2C_CP_COMMAND] = 'R'; // 01: Preset resolver base data
I2C_cmd[I2C_CP_WDRAM_DIA_UPPER] = (uint8_t)((lfsAcs.setValue.wchCtrl.dram_dmtr_x100>>8)&0xFF); // Dram diameter upper
I2C_cmd[I2C_CP_WDRAM_DIA_LOWER] = (uint8_t)((lfsAcs.setValue.wchCtrl.dram_dmtr_x100)&0xFF); // Dram diameter lower
I2C_cmd[I2C_CP_CCABLE_DIA_UPPER] = (uint8_t)((lfsAcs.setValue.wchCtrl.adj_val_x10000>>8)&0xFF); // Cable diameter upper
I2C_cmd[I2C_CP_CCABLE_DIA_LOWER] = (uint8_t)((lfsAcs.setValue.wchCtrl.adj_val_x10000)&0xFF); // Cable diameter lower
I2C_cmd[I2C_CP_RESOLVER_RESO] = lfsAcs.setValue.wchCtrl.res_resolution; // Resolver resolution
// Send base calculation data to Resolver controller
i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand);
Thread::wait(500);
// Set all motor current thresold
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_RFCRW, MOTOR_DIR_FWD, lfsAcs.setValue.crwCtrl.rf_mtr_ithd_f );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_RFCRW, MOTOR_DIR_RVS, lfsAcs.setValue.crwCtrl.rf_mtr_ithd_r );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_LBCRW, MOTOR_DIR_FWD, lfsAcs.setValue.crwCtrl.lb_mtr_ithd_f );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_LBCRW, MOTOR_DIR_RVS, lfsAcs.setValue.crwCtrl.lb_mtr_ithd_r );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_RFTFM, MOTOR_DIR_FWD, lfsAcs.setValue.tfmCtrl.rf_mtr_ithd_f );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_RFTFM, MOTOR_DIR_RVS, lfsAcs.setValue.tfmCtrl.rf_mtr_ithd_r );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_LBTFM, MOTOR_DIR_FWD, lfsAcs.setValue.tfmCtrl.lb_mtr_ithd_f );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_LBTFM, MOTOR_DIR_RVS, lfsAcs.setValue.tfmCtrl.lb_mtr_ithd_r );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_CMPAN, MOTOR_DIR_FWD, lfsAcs.setValue.ptlCtrl.pan_mtr_ithd_f );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_CMPAN, MOTOR_DIR_RVS, lfsAcs.setValue.ptlCtrl.pan_mtr_ithd_r );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_CTILT, MOTOR_DIR_FWD, lfsAcs.setValue.ptlCtrl.tlt_mtr_ithd_f );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_CTILT, MOTOR_DIR_RVS, lfsAcs.setValue.ptlCtrl.tlt_mtr_ithd_r );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_WINCH, MOTOR_DIR_FWD, lfsAcs.setValue.wchCtrl.drm_mtr_ithd_f );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_WINCH, MOTOR_DIR_RVS, lfsAcs.setValue.wchCtrl.drm_mtr_ithd_r );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_WINCH, MOTOR_DIR_FWD, lfsAcs.setValue.wchCtrl.no2_mtr_ithd_f );
i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_WINCH, MOTOR_DIR_RVS, lfsAcs.setValue.wchCtrl.no2_mtr_ithd_r );
led.led_main_blink(3);
wMusic.pi(3);
DEBUG_PRINT_L0( "Bd0> -----------------------------------\r\n");
DEBUG_PRINT_L0( "Bd0> >>>> Initializing completed !! <<<<\r\n");
DEBUG_PRINT_L0( "Bd0> -----------------------------------\r\n");
led.led_on(1); // Initializing is OK then Power Indicator LED ON
led.led_off(4); //
wMusic.knkk(); // System OK, then music start ( Kono-ki nannoki kininaruki~~ )
// -----------------------------------------------------------------
// Communicate with client PC program.
// TCP connection:
// -----------------------------------------------------------------
while( true )
{
if( flg_ethernet == true ) // in case of Ethernet OK
{
Thread::wait(3);
tcp_server.accept(tcp_client);
// tcp_client.set_blocking(false, 10000); // Timeout after (10000) msec
tcp_client.set_blocking(false, 5000); // Timeout after (5000) msec
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");
led.led_on(2);
led.led_main_blink(4);
wMusic.pi(4);
// vol, freq, time
wMusic.wave( 0.2, 440.0*6, 0.2 ); // 440Hz 0.3s
wMusic.wave( 0.0, 440.0, 0.1 ); // No sound
wMusic.wave( 0.2, 440.0*6, 0.2 ); // 440Hz 0.3s
wMusic.wave( 0.0, 440.0, 0.1 ); // No sound
wMusic.wave( 0.2, 440.0*6, 0.2 ); // 440Hz 0.3s
wMusic.wave( 0.0, 440.0, 0.1 ); // No sound
while(true){
led.led_main_on();
// --------------------------------------------------------------
// Following instructions are blocking when no ethernet access
// --------------------------------------------------------------
rcv_data_cnt = tcp_client.receive(rbuf, sizeof(rbuf));
if( rcv_data_cnt <= 0 ){
DEBUG_PRINT_L3("Bd0> ###!!!### : TCP Receive packet fail\r\n");
Thread::wait(3);
// vol, freq, time
wMusic.wave( 0.2, 440.0, 0.3 ); // 440Hz 0.3s
wMusic.wave( 0.0, 440.0, 0.3 ); // No sound
wMusic.wave( 0.2, 440.0, 0.3 ); // 440Hz 0.3s
wMusic.wave( 0.0, 440.0, 0.3 ); // No sound
wMusic.wave( 0.2, 440.0, 0.3 ); // 440Hz 0.3s
wMusic.wave( 0.0, 440.0, 0.3 ); // No sound
wMusic.wave( 0.2, 440.0, 0.3 ); // 440Hz 0.3s
wMusic.wave( 0.0, 440.0, 0.3 ); // No sound
wMusic.wave( 0.2, 440.0, 0.3 ); // 440Hz 0.3s
wMusic.wave( 0.0, 440.0, 0.3 ); // No sound
break;
}
else{
// Transform, Crawler part valid
if( !strncmp( rbuf, "Hello", 5 ) )
{
led.led_on(3);
// Send the information of platform
// ==> B1: B1Demo, B2:B2Demo
sprintf( hbuf,PLATFORM );
hbuf[6] = '\0';
tcp_client.send_all(hbuf, sizeof(sbuf));
led.led_off(3);
}
else if( !strncmp( rbuf, "OO", 2 ) )
{
led.led_on(3);
/* When change valid part from tfm/crm to winch, then send calculation base data to resolver controller */
DEBUG_PRINT_L0("Bd0> Send winch position clear command to Resolver Controller\r\n");
I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Encorder reset
// Send base calculation data to Resolver controller
if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
{
DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
}
led.led_off(3);
}
else if( !strncmp( rbuf, "OX", 2 ))
{
led.led_on(3);
I2C_cmd[I2C_CP_COMMAND] = 0x43; // Set motor current base value
// Send base calculation data to Resolver controller
if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
{
DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
}
led.led_off(3);
}
// Pan/Tilt, winch part valid
else if( !strncmp( rbuf, "XO", 2 ))
{
led.led_on(3);
I2C_cmd[I2C_CP_COMMAND] = 0x43; // Set motor current base value
// Send base calculation data to Resolver controller
if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
{
DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
}
Thread::wait(500);
/* When change valid part from tfm/crm to winch, then send calculation base data to resolver controller */
DEBUG_PRINT_L0("Bd0> Send winch position calculation base data to Resolver Controller\r\n");
I2C_cmd[I2C_CP_COMMAND] = 'R'; // 01: Preset resolver base data
I2C_cmd[I2C_CP_WDRAM_DIA_UPPER] = (uint8_t)((lfsAcs.setValue.wchCtrl.dram_dmtr_x100>>8)&0xFF); // Dram diameter upper
I2C_cmd[I2C_CP_WDRAM_DIA_LOWER] = (uint8_t)((lfsAcs.setValue.wchCtrl.dram_dmtr_x100)&0xFF); // Dram diameter lower
I2C_cmd[I2C_CP_CCABLE_DIA_UPPER] = (uint8_t)((lfsAcs.setValue.wchCtrl.adj_val_x10000>>8)&0xFF); // Cable diameter upper
I2C_cmd[I2C_CP_CCABLE_DIA_LOWER] = (uint8_t)((lfsAcs.setValue.wchCtrl.adj_val_x10000)&0xFF); // Cable diameter lower
I2C_cmd[I2C_CP_RESOLVER_RESO] = lfsAcs.setValue.wchCtrl.res_resolution; // Resolver resolution
// Send base calculation data to Resolver controller
if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
{
DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
}
led.led_off(3);
}
else if( !strncmp( rbuf, "XX", 2 ))
{
strncpy( tmp_b, rbuf+9, 2); // direction
tmp_b[2]='\0';
direction = atoi(tmp_b);
strncpy( tmp_b, rbuf+12, 4); // speed
tmp_b[4]='\0';
speed = atoi(tmp_b);
strncpy( tmp_b, rbuf+8, 1); // speed
tmp_b[1]='\0';
lswstop = atoi(tmp_b);
//------------------------------------
// Cmd: "WICH" : B2DP Winch cintrol
//------------------------------------
/* if( direction == 2 )
{
if( flg_mlock_check == false )
{
i2cAcs.i2cClearMLCnt(I2C_ADDRESS_RESOLVER);
flg_mlock_check = true;
}
}*/
if( !strncmp( rbuf+3, "WICH", 4 ))
{
// Winch
if( direction == 0 )
{
led.led_on(3);
speed = ( speed * lfsAcs.setValue.wchCtrl.drm_mtr_hspd_f ) / 10;
}
else if ( direction == 1 )
{
led.led_on(3);
speed = ( speed * lfsAcs.setValue.wchCtrl.drm_mtr_hspd_r ) / 10 * -1;
}
else // direction == 2 is JS off
{
flg_WICH_motor_stop = false; // flag release when JS off
speed = 0;
}
if( flg_WICH_motor_stop == false )
{
DEBUG_PRINT_L3("Bd0> XX_WICH [ dir: %d, speed: %d]", direction,speed/10);
mtrAcs.cmdControl( "XX_WICH", 7, speed ); // Motor on
}
// Get motor current here from LPC824-max
if( direction == 0 )
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_WINCH, MOTOR_DIR_FWD);
}
else
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_WINCH, MOTOR_DIR_RVS);
}
// Get information and send to client
winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
if( i2cAcs.flg_motor_lock == true )
{
DEBUG_PRINT_L3("Bd0> ### WINCH LOCK !!! ###\r\n");
mtrAcs.cmdControl( "XX_WICH", 7, 0 );
flg_WICH_motor_stop = true;
winchPosition = 9999; // motor lock
}
sprintf( sbuf, "%04d", winchPosition );
sbuf[4] = '\0';
tcp_client.send_all(sbuf, sizeof(sbuf));
led.led_off(3);
}
//------------------------------------
// Cmd: "CPAN" : B1DP Camera PAN
//------------------------------------
else if( !strncmp( rbuf+3, "CPAN", 4 ))
{
if( direction == 0 )
{
led.led_on(3);
speed = ( speed * lfsAcs.setValue.wchCtrl.no2_mtr_hspd_f ) / 10;
// speed /= 3; // Tmp setting until formal motor comming
}
else if( direction == 1 )
{
led.led_on(3);
speed = ( speed * lfsAcs.setValue.wchCtrl.no2_mtr_hspd_r ) / -10;
// speed /= 3; // Tmp setting until formal motor comming
}
else
{
flg_CPAN_motor_stop = false; // flag release when JS off
speed = 0;
}
if( flg_CPAN_motor_stop == false )
{
// DEBUG_PRINT_L3("Bd0> XX_CPAN [ dir: %d, speed: %d]", direction,speed/10);
DEBUG_PRINT_L3("\t, XX_CPAN [ dir: %d, speed: %d]", direction,speed/10);
mtrAcs.cmdControl( "XX_CPAN", 7, speed ); // Motor on
}
// Get motor current
if( speed != 0 )
{
if( direction == 0 )
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_CMPAN, MOTOR_DIR_FWD);
}
else
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_CMPAN, MOTOR_DIR_RVS);
}
// Get information
winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
if( i2cAcs.flg_motor_lock == true )
{
DEBUG_PRINT_L3("Bd0> ### CAMERA PAN LOCK !!! ###\r\n");
mtrAcs.cmdControl( "XX_CPAN", 7, 0 );
flg_CPAN_motor_stop = true;
winchPosition = 9999; // motor lock
}
}
// Send motor lock status to handy controller
if( direction != 2 )
{
sprintf( sbuf, "%04d", winchPosition );
sbuf[4] = '\0';
tcp_client.send_all(sbuf, sizeof(sbuf));
}
DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
led.led_off(3);
}
//------------------------------------
// Cmd: "CTLT" : B1DP Camera TILT
//------------------------------------
else if( !strncmp( rbuf+3, "CTLT", 4 ))
{
if( direction == 0 )
{
led.led_on(3);
speed = ( speed * lfsAcs.setValue.wchCtrl.no2_mtr_hspd_f ) / 10;
speed /= 4; // Max 6v motor
}
else if( direction == 1 )
{
led.led_on(3);
speed = ( speed * lfsAcs.setValue.wchCtrl.no2_mtr_hspd_r ) / -10;
speed /= 4; // Max 6v motor
}
else
{
flg_CTLT_motor_stop = false; // flag release when JS off
speed = 0;
}
if( flg_CTLT_motor_stop == false )
{
DEBUG_PRINT_L4("Bd0> XX_CTLT [ dir: %d, speed: %d]", direction,speed/10);
mtrAcs.cmdControl( "XX_CTLT", 7, speed ); // Motor on
}
// Get motor current
if( speed != 0 )
{
if( direction == 0 )
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_CTILT, MOTOR_DIR_FWD);
}
else
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_CTILT, MOTOR_DIR_RVS);
}
// Get information
winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
if( i2cAcs.flg_motor_lock == true )
{
DEBUG_PRINT_L3("Bd0> ### CAMERA PAN LOCK !!! ###\r\n");
mtrAcs.cmdControl( "XX_CTLT", 7, 0 );
flg_CTLT_motor_stop = true;
winchPosition = 9999; // motor lock
}
}
// Send motor lock status to handy controller
if( direction != 2 )
{
sprintf( sbuf, "%04d", winchPosition );
sbuf[4] = '\0';
tcp_client.send_all(sbuf, sizeof(sbuf));
}
led.led_off(3);
}
//------------------------------------
// Cmd: "CLRF" : B1/B2DP RF Crawler
//------------------------------------
else if( !strncmp( rbuf+3, "CLRF", 4 ))
{
if( direction == 0 )
{
led.led_on(3);
speed = ( speed * lfsAcs.setValue.crwCtrl.rf_mtr_hspd_r ) / 10;
}
else if( direction == 1 )
{
led.led_on(3);
speed = ( speed * lfsAcs.setValue.crwCtrl.rf_mtr_hspd_f ) / -10;
}
else // direction == 2 is JS off
{
flg_CLRF_motor_stop = false; // flag release when JS off
speed = 0;
}
if( flg_CLRF_motor_stop == false )
{
DEBUG_PRINT_L4("Bd0> XX_CLRF [ dir: %d, speed: %d]", direction,speed/10);
mtrAcs.cmdControl( "XX_CLRF", 7, speed );
}
// Pass motor current check for B1B2 Demo Crawler motor
/*
// Get motor current
if( speed != 0 )
{
if( direction == 0 )
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_RFCRW, MOTOR_DIR_FWD);
}
else
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_RFCRW, MOTOR_DIR_RVS);
}
// Get information
winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
if( i2cAcs.flg_motor_lock == true )
{
DEBUG_PRINT_L3("Bd0> ### RF CRAWLER LOCK !!! ###\r\n");
mtrAcs.cmdControl( "XX_CLRF", 7, 0 );
flg_CLRF_motor_stop = true;
}
}
*/
DEBUG_PRINT_L4("\r\n");
// Don't Send motor lock status to handy controller
led.led_off(3);
}
//------------------------------------
// Cmd: "CLLB" : B1/B2DP LB Crawler
//------------------------------------
else if( !strncmp( rbuf+3, "CLLB", 4 ))
{
if( direction == 0 )
{
led.led_on(3);
speed = ( speed * lfsAcs.setValue.crwCtrl.lb_mtr_hspd_f ) / 10;
}
else if( direction == 1 )
{
led.led_on(3);
speed = ( speed * lfsAcs.setValue.crwCtrl.lb_mtr_hspd_r ) / -10;
}
else // direction == 2 is JS off
{
flg_CLLB_motor_stop = false; // flag release when JS off
speed = 0;
}
DEBUG_PRINT_L4("Bd0> XX_CLLB [%d]\r\n", speed);
if( flg_CLLB_motor_stop == false )
{
mtrAcs.cmdControl( "XX_CLLB", 7, speed );
}
// Pass motor current check for B1B2 Demo Crawler motor
/*
// Get motor current
if( speed != 0 )
{
if( direction == 0 )
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_LBCRW, MOTOR_DIR_FWD);
}
else
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_LBCRW, MOTOR_DIR_RVS);
}
// Get information
winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
if( i2cAcs.flg_motor_lock == true )
{
DEBUG_PRINT_L3("Bd0> ### LB CRAWLER LOCK !!! ###\r\n");
mtrAcs.cmdControl( "XX_CLLB", 7, 0 );
flg_CLLB_motor_stop = true;
}
}
*/
/*
// Send motor lock status to handy controller
if(( flg_CLRF_motor_stop == true ) || ( flg_CLLB_motor_stop == true ))
{
winchPosition = 9999; // motor lock
}
if( direction != 2 )
{
sprintf( sbuf, "%04d", winchPosition );
sbuf[4] = '\0';
tcp_client.send_all(sbuf, sizeof(sbuf));
}
*/
DEBUG_PRINT_L4("\r\n");
led.led_off(3);
}
//------------------------------------
// Cmd: "TFRF" : B1/B2DP RF transform
//------------------------------------
else if( !strncmp( rbuf+3, "TFRF", 4 ))
{
if( direction == 0 )
{
led.led_on(3);
if( ( lswstop == 1 ) && ( limit_sw_rf == 0 ) )
{
DEBUG_PRINT_L3("RF limit sw ON\r\n");
speed = 0;
}
else
{
speed = ( speed * lfsAcs.setValue.tfmCtrl.rf_mtr_hspd_f ) / 10;
}
}
else if( direction == 1 ) // reverse
{
led.led_on(3);
if( ( lswstop == 1 ) && ( limit_sw_rf == 0 ) )
{
DEBUG_PRINT_L3("RF limit sw ON\r\n");
speed = 0;
}
else
{
speed = ( speed * lfsAcs.setValue.tfmCtrl.rf_mtr_hspd_r ) / -10;
}
}
else
{
flg_TFRF_motor_stop = false; // flag release when JS off
speed = 0;
}
if( flg_TFRF_motor_stop == false )
{
DEBUG_PRINT_L4("Bd0> XX_TFRF [%d]\r\n", speed);
mtrAcs.cmdControl( "XX_TFRF", 7, speed );
}
// Get motor current
if( speed != 0 )
{
if( direction == 0 )
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_RFTFM, MOTOR_DIR_FWD);
}
else
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_RFTFM, MOTOR_DIR_RVS);
}
// Get information
winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
if( i2cAcs.flg_motor_lock == true )
{
DEBUG_PRINT_L3("Bd0> ### RF TRANSFORM LOCK !!! ###\r\n");
mtrAcs.cmdControl( "XX_TFRF", 7, 0 );
flg_TFRF_motor_stop = true;
winchPosition = 9999; // motor lock
// flg_mlock_check = false;
i2cAcs.i2cClearMLCnt(I2C_ADDRESS_RESOLVER);
}
}
// Send motor lock status to handy controller
if( direction != 2 )
{
sprintf( sbuf, "%04d", winchPosition );
sbuf[4] = '\0';
tcp_client.send_all(sbuf, sizeof(sbuf));
}
led.led_off(3);
}
//------------------------------------
// Cmd: "TFLB" : B1/B2DP LB transform
//------------------------------------
else if( !strncmp( rbuf+3, "TFLB", 4 ))
{
if( direction == 0 )
{
led.led_on(3);
if( ( lswstop == 1 ) && ( limit_sw_lb == 0 ) )
{
DEBUG_PRINT_L3("LB limit sw ON\r\n");
speed = 0;
}
else
{
speed = ( speed * lfsAcs.setValue.tfmCtrl.lb_mtr_hspd_f ) / 10;
}
}
else if( direction == 1 )
{
led.led_on(3);
if( ( lswstop == 1 ) && ( limit_sw_lb == 0 ) )
{
DEBUG_PRINT_L3("LB limit sw ON\r\n");
speed = 0;
}
else
{
speed = ( speed * lfsAcs.setValue.tfmCtrl.lb_mtr_hspd_r ) / -10;
}
}
else
{
flg_TFLB_motor_stop = false; // flag release when JS off
speed = 0;
}
if( flg_TFLB_motor_stop == false )
{
DEBUG_PRINT_L4("Bd0> XX_TFLB [%d]\r\n", speed);
mtrAcs.cmdControl( "XX_TFLB", 7, speed );
}
if( speed != 0 )
{
if( direction == 0 )
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_LBTFM, MOTOR_DIR_FWD);
}
else
{
i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_LBTFM, MOTOR_DIR_RVS);
}
// Read information
winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
if( i2cAcs.flg_motor_lock == true )
{
DEBUG_PRINT_L3("Bd0> ### LB TRANSFORM LOCK !!! ###\r\n");
mtrAcs.cmdControl( "XX_TFLB", 7, 0 );
flg_TFLB_motor_stop = true;
winchPosition = 9999; // motor lock
i2cAcs.i2cClearMLCnt(I2C_ADDRESS_RESOLVER);
// flg_mlock_check = false;
}
}
// Send motor lock status to handy controller
if( direction != 2 )
{
sprintf( sbuf, "%04d", winchPosition );
sbuf[4] = '\0';
tcp_client.send_all(sbuf, sizeof(sbuf));
}
led.led_off(3);
}
//------------------------------------
// All motor stop
//------------------------------------
else
{
i2cAcs.flg_motor_lock = false; // no lock
mtrAcs.cmdControl( "XX_TFRF", 7, 0 );
mtrAcs.cmdControl( "XX_TFLB", 7, 0 );
mtrAcs.cmdControl( "XX_CLRF", 7, 0 );
mtrAcs.cmdControl( "XX_CLLB", 7, 0 );
mtrAcs.cmdControl( "XX_WICH", 7, 0 );
mtrAcs.cmdControl( "XX_CPAN", 7, 0 );
mtrAcs.cmdControl( "XX_CTLT", 7, 0 );
// i2cAcs.i2cClearMLCnt(I2C_ADDRESS_RESOLVER);
}
}
//Thread::wait(20); // <== never insert this !
}
}
tcp_client.close();
}
}
}
else{
led.led_main_on();
wMusic.bz_error(5);
while( true ){
led.led_error();
led.led_main_blink(5);
DEBUG_PRINT_L0("Bd0> ***ERROR*** Eternat connect Fali\r\n");
DEBUG_PRINT_L0("Bd0> This programis booting in Stand alone mode.\r\n");
Thread::wait(1000);
}
}
}
else{
led.led_main_on();
wMusic.bz_error(5);
while( true ){
led.led_error();
led.led_main_blink(5);
DEBUG_PRINT_L0("Bd0> *** ERROR*** Eternat init Fail\r\n");
DEBUG_PRINT_L0("Bd0> This program is booting in Stand alone mode.\r\n");
Thread::wait(1000);
}
}
led.led_main_off();
led.led_off(2);
}