z ysaito
2018.07.26
Diff: P0_main.cpp
- Revision:
- 0:b3376afd10d8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/P0_main.cpp Thu Jul 26 00:20:04 2018 +0000
@@ -0,0 +1,589 @@
+/*
+//////////////////////////////////////////////////////////
+ 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);
+ }
+}