z ysaito
2018.07.26
Revision 0:b3376afd10d8, committed 2018-07-26
- Comitter:
- sayzyas
- Date:
- Thu Jul 26 00:20:04 2018 +0000
- Commit message:
- 2018.07.26
Changed in this revision
--- /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);
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/WebSocketClient.lib Thu Jul 26 00:20:04 2018 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/mbed_example/code/WebSocketClient/#efa2c147bee1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/common.h Thu Jul 26 00:20:04 2018 +0000
@@ -0,0 +1,95 @@
+/* Information */
+#define LatestUpDate "2017.08.30"
+#define ProgramRevision "Rev 2.30"
+#define Author "Y.Saito(zinsor)"
+#define Company "Revast Co.,Ltd"
+
+
+/* ****************************************** */
+/* Motor */
+/* ****************************************** */
+enum{
+ MOTOR_RFCRW, // RF crawler
+ MOTOR_LBCRW, // LB crawler
+ MOTOR_RFTFM, // RF Transform
+ MOTOR_LBTFM, // LB transform
+ MOTOR_WINCH, // Winch
+ MOTOR_CMPAN, // Pan
+ MOTOR_CTILT, // Tilt
+ MOTOR_CABLE // Cable transfar
+};
+
+enum{
+ MOTOR_DIR_FWD, // Motor forwaed rotation
+ MOTOR_DIR_RVS, // Motor reverse rotation
+ MOTOR_DIR_STP // Motor stop
+};
+
+
+enum{
+ MOTOR_NO0,
+ MOTOR_NO1,
+ MOTOR_NO2,
+ MOTOR_NO3
+};
+
+/* ****************************************** */
+/* I2C */
+/* ****************************************** */
+#define NumberOfI2CCommand 12
+enum{
+ I2C_CP_COMMAND, // instruction command
+ I2C_CP_MOTORNO, // motor number
+ I2C_CP_M_DIR, // motor rotation direction
+ I2C_CP_M_CNTTH_U, // motor current limit detection threshold upper byte
+ I2C_CP_M_CNTTH_L, // motor current limit detection threshold lower byte
+ I2C_CP_WDRAM_DIA_X100_UPPER, // winch dram motor diameter upper
+ I2C_CP_WDRAM_DIA_X100_LOWER, // winch dram motor diameter lower
+ I2C_CP_ADJUST_VALUE_X10000_UPPER, // cable diameter upper byte
+ I2C_CP_ADJUST_VALUE_X10000_LOWER, // cable diameter lower byte
+ I2C_CP_RESOLVER_RESO, // resolver resolution (bit)
+ I2C_CP_PRESET_CPOS_UPPER, // preset position upper
+ I2C_CP_PRESET_CPOS_LOWER // preset position lower
+};
+
+#define I2C_ADDRESS_RESOLVER 0x02
+
+
+/* ****************************************** */
+/* Parameters */
+/* ****************************************** */
+#define MAX_DROP_AMOUNT 3495 /* Maxmun dropping distance : ex 3500 (mm) */
+
+#define CALIBRATION_COUNT 300 /* Joystick calibration count */
+#define DEAD_BAND 5 /* Joystick dead band wide */
+
+#define ROTATE_PER_RESOLUTION 24 /* For QEI */
+#define PAI 3.14159265359
+
+#define LED_ON 0
+#define LED_OFF 1
+
+#define __NETWORK_ACCESS__
+#define TCP_CMDSERVER_PORT 10000
+
+/* ****************************************** */
+/* Structure */
+/* ****************************************** */
+/* Winch setting value */
+typedef struct {
+ uint16_t cth_dram_mtr_f; // 2 Current threshold: winch dram Motor ( forward )
+ uint16_t cth_dram_mtr_r; // 2 Current threshold: winch dram Motor ( reverse )
+ uint16_t cth_cabl_mtr_f; // 2 Current threshold: winch cable Motor ( forward )
+ uint16_t cth_cabl_mtr_r; // 2 Current threshold: winch cable Motor ( reverse )
+ uint16_t dram_dmtr_x100; // 2 Winch dram diameter x 100
+ uint16_t adj_val_x10000; // 2 Winch adjust value x 100
+ uint8_t res_resolution; // 1 Winch resolver resolution (bit)
+ uint8_t reserved_1; // 1 reserved for future use
+ int16_t move_interval_cw; // 2 motor moving interval between m1 and m2 at CW rotation
+ int16_t move_interval_ccw; // 2 motor moving interval between m1 and m2 at CCW rotation
+} wch_SetValue_t;
+
+typedef struct SetValue {
+ wch_SetValue_t wchCtrl;
+} setValue_t;
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/debugprint.h Thu Jul 26 00:20:04 2018 +0000 @@ -0,0 +1,55 @@ +// ====================================================================== +// For Debugging +// ====================================================================== +#define __DEBUG_PRINT_SW__ // Display SW Status to console +#define __DEBUG_L0__ +#define __DEBUG_L1__ +//#define __DEBUG_L2__ +#define __DEBUG_L3__ +//#define __DEBUG_L4__ +//#define __DEBUG_L5__ + +#define __DEBUG_WINCH_DATA__ + +#ifdef __DEBUG_WINCH_DATA__ + #define DEBUG_PRINT_WINCH_DATA(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_WINCH_DATA(...) +#endif + +#ifdef __DEBUG_PRINT_SW__ + #define DEBUG_PRINT_SW(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_SW(...) +#endif + +#ifdef __DEBUG_L0__ + #define DEBUG_PRINT_L0(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_L0(...) +#endif +#ifdef __DEBUG_L1__ + #define DEBUG_PRINT_L1(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_L1(...) +#endif +#ifdef __DEBUG_L2__ + #define DEBUG_PRINT_L2(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_L2(...) +#endif +#ifdef __DEBUG_L3__ + #define DEBUG_PRINT_L3(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_L3(...) +#endif +#ifdef __DEBUG_L4__ + #define DEBUG_PRINT_L4(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_L4(...) +#endif +#ifdef __DEBUG_L5__ + #define DEBUG_PRINT_L5(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_L5(...) +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/i2cAccess.cpp Thu Jul 26 00:20:04 2018 +0000
@@ -0,0 +1,309 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "stdio.h"
+#include "common.h"
+//#include "com_func.h"
+#include "i2cAccess.h"
+
+I2C i2c(p9, p10); // I2C SDA, SCL is good
+
+i2cAccess::i2cAccess()
+{
+ sp_wdram_f = 100; // motor speed
+ sp_wdram_r = 100; // motor speed
+ sp_wcabl_f = 100; // motor speed
+ sp_wcabl_r = 100; // motor speed
+ flg_motor_lock = false;
+}
+
+bool i2cAccess::i2c_read( int address, const char* data, int length)
+{
+;
+}
+
+bool i2cAccess::i2c_write( int address, const char* data, int length )
+{
+ int rts;
+ rts = i2c.write(address, data, length);
+ if( rts == 0 ) // i2c write success = 0
+ {
+ return true;
+ }
+ else // non 0 is failure
+ {
+ return false;
+ }
+}
+
+// --------------------------------------------------------------------------------------
+// I2C read motor current
+// --------------------------------------------------------------------------------------
+int16_t i2cAccess::i2cReadMotorCurrent(
+ int32_t i2c_addr, // i2c address
+ int motor_id, // Motor CAN ID
+ int motor_no, // Motor number: 0 or 1
+ int motor_dir, // Motor direction 0:Fwd, 1:Rvs
+ int threshold // Motor current threshold
+){
+
+ int motor_current = 100;
+
+ // read motor current several times and judgemant.
+
+ if(true)
+ {
+ return 0; // motor unlock
+ }
+ else
+ {
+ return -1; // motor lock
+ }
+}
+
+/*
+// ============================================================
+// Read motor current
+// ============================================================
+int read_motorCurrent(
+ int motor_id, // Motor CAN ID
+ int motor_no, // Motor Number ( 1 or 2 )
+ int motor_dir // Motor direction
+){
+
+ float motor_current;
+ int i;
+ int mc_abs_pct = 0;
+ int mc_Threshold = 0;
+
+ if( motor_id == MCTR_CANID_PANTILTWCH )
+ {
+ if (motor_no == MOTOR_2 ){
+ //motor_current = mcnt_panwdm.read()*100.0f;
+ mc_abs_pct = abs((int)((motor_current - motor1_current_center_value)*100.0f));
+ DEBUG_PRINT_L1("Bd0> M1:%lf/%lf= [%03d%%] th:%3.2f< <%3.2f\t", motor_current, motor1_current_center_value, mc_abs_pct, motor1_current_rvs_thd, motor1_current_fwd_thd );
+
+ for( i = 0; i < (mc_abs_pct/10); i++){
+ DEBUG_PRINT_L1(">");
+ }
+ DEBUG_PRINT_L4("\r\n");
+
+ if( motor_dir == MOTOR_FWD ){
+ mc_Threshold = (int)motor1_current_fwd_thd;
+ DEBUG_PRINT_L1("Bd0> Upper threshold: %d\r\n", mc_Threshold);
+ }
+ else{
+ mc_Threshold = (int)motor1_current_rvs_thd;
+ DEBUG_PRINT_L1("Bd0> Lower threshold: %d\r\n", mc_Threshold);
+ }
+ if( mc_abs_pct > mc_Threshold ){
+ DEBUG_PRINT_L1("Bd0> **** MC1 Over the Limit [%d] ****\r\n", motor1_lock_count );
+ motor_pantiltwch_1_lock_count += 1;
+ if( motor_pantiltwch_1_lock_count >= MC_LOCK_COUNT ){
+ stdio_mutex.lock(); // Mutex Lock
+ flg_motor_pantiltwch_1_lock = 1;
+ stdio_mutex.unlock(); // Mutex Release
+ DEBUG_PRINT_L1("Bd0> #### MOTOR1 LOCK ! #### (%d)\r\n", flg_motor_pantiltwch_1_lock);
+ }
+ }
+ else{
+ DEBUG_PRINT_L1("Bd0> Pass\r\n");
+ if( motor_pantiltwch_1_lock_count > 0 ) flg_motor_pantiltwch_1_lock -= 1;
+ else motor_pantiltwch_1_lock_count = 0;
+ }
+ }
+ }
+ else if( motor_id == MCTR_CANID_TFM )
+ {
+ }
+ else if( motor_id == MCTR_CANID_CRW )
+ {
+ }
+ return mc_abs_pct;
+}
+*/
+
+
+// int16_t sp_wdram_f; // motor speed
+// int16_t sp_wdram_r; // motor speed
+// int16_t sp_wcabl_f; // motor speed
+// int16_t sp_wcabl_r; // motor speed
+bool i2cAccess::i2cReadParameters(
+ int32_t i2c_addr // i2c address
+){
+ char I2C_data[12];
+ /*
+ [00]: 0x12
+ [01]: motot speed dram motor forward (CW) upper
+ [02]: motot speed dram motor forward (CW) lower
+ [03]: motot speed dram motor reverse (CCW) upper
+ [04]: motot speed dram motor reverse (CCW) lower
+ [05]: motot speed cable motor forward (CW) upper
+ [06]: motot speed cable motor forward (CW) lower
+ [07]: motot speed cable motor forward (CCW) upper
+ [08]: motot speed cable motor forward (CCW) lower
+ [09]: winch dropped ammount upper
+ [10]: winch dropped ammount lower
+ [11]: 0x34
+ */
+ Thread::wait(5);
+ i2c.read(i2c_addr, I2C_data, 12); // Read
+
+ if(( I2C_data[0] == 0x12 )&&( I2C_data[11] == 0x34 ))
+ {
+ sp_wdram_f = (I2C_data[2] << 8) | I2C_data[1];
+ if( sp_wdram_f <= 3 )
+ sp_wdram_f = 0;
+ else if( sp_wdram_f >= 97 )
+ sp_wdram_f = 100;
+
+ sp_wdram_r = (I2C_data[4] << 8) | I2C_data[3];
+ if( sp_wdram_r <= 3 )
+ sp_wdram_r = 0;
+ else if( sp_wdram_r >= 97 )
+ sp_wdram_r = 100;
+
+ sp_wcabl_f = (I2C_data[6] << 8) | I2C_data[5];
+ if( sp_wcabl_f <= 3 )
+ sp_wcabl_f = 0;
+ else if( sp_wcabl_f >= 97 )
+ sp_wcabl_f = 100;
+
+ sp_wcabl_r = (I2C_data[8] << 8) | I2C_data[7];
+ if( sp_wcabl_r <= 3 )
+ sp_wcabl_r = 0;
+ else if( sp_wcabl_r >= 97 )
+ sp_wcabl_r = 100;
+
+ drop_ammount = (I2C_data[10] << 8) | I2C_data[9];
+
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+
+// --------------------------------------------------------------------------------------
+// I2C read winch current position
+// --------------------------------------------------------------------------------------
+int16_t i2cAccess::i2cReadInformation(
+ int32_t i2c_addr, // i2c address
+ int winchOffsetValue // Winch position offset value
+){
+ char I2C_data[9];
+ int16_t res_position = 0;
+
+ /*
+ [0]: 0x12
+ [1]: Position lower
+ [2]: Position upper
+ [3]: Motor lock flag 1:lock, 0:nolock
+ [4]: -
+ [5]: -
+ [6]: -
+ [7]: -
+ [8]: 0x34
+ */
+ Thread::wait(5);
+ i2c.read(i2c_addr, I2C_data, 9); // Read
+
+ if(( I2C_data[0] == 0x12 )&&( I2C_data[8] == 0x34 ))
+ {
+ res_position = (I2C_data[2] << 8) | I2C_data[1];
+ if( res_position == -1 ){
+ res_position = 8888;
+ }
+ else{
+ res_position += winchOffsetValue;
+ }
+ }
+ else{
+ res_position = 9999;
+ }
+
+ if( I2C_data[3] == 1 )
+ {
+ flg_motor_lock = true;
+ }
+ else{
+ flg_motor_lock = false;
+ }
+
+ return res_position;
+}
+
+
+// --------------------------------------------------------------------------------------
+// I2C write acccess: set motor threshold all
+// --------------------------------------------------------------------------------------
+bool i2cAccess::i2cSetMotorThreshold(
+ int32_t i2c_addr,
+ int8_t moror_number,
+ int8_t motor_dir,
+ int16_t motor_thresold
+){
+ char sbuf[NumberOfI2CCommand];
+/*
+ I2C_CP_COMMAND, // instruction command
+ I2C_CP_MOTORNO, // motor number
+ I2C_CP_M_DIR, // motor rotation direction
+ I2C_CP_M_CNTTH_U, // motor current limit detection threshold upper byte
+ I2C_CP_M_CNTTH_L, // motor current limit detection threshold lower byte
+ I2C_CP_WDRAM_DIA_UPPER, // winch dram motor diameter upper
+ I2C_CP_WDRAM_DIA_LOWER, // winch dram motor diameter lower
+ I2C_CP_CCABLE_DIA_UPPER, // cable diameter upper byte
+ I2C_CP_CCABLE_DIA_LOWER, // cable diameter lower byte
+ I2C_CP_RESOLVER_RESO, // resolver resolution (bit)
+ I2C_CP_PRESET_CPOS_UPPER, // preset position upper
+ I2C_CP_PRESET_CPOS_LOWER // preset position lower
+*/
+ sbuf[I2C_CP_COMMAND] = 'T'; // instruction command
+ sbuf[I2C_CP_MOTORNO] = moror_number; // motor number
+ sbuf[I2C_CP_M_DIR] = motor_dir; // motor rotation direction
+ sbuf[I2C_CP_M_CNTTH_U] = ( motor_thresold >> 8 ) & 0xFF;
+ sbuf[I2C_CP_M_CNTTH_L] = ( motor_thresold & 0xFF );
+
+ i2c_write( i2c_addr, sbuf, NumberOfI2CCommand );
+ wait_ms(1);
+
+ return true;
+}
+
+
+// --------------------------------------------------------------------------------------
+// I2C write acccess: clear motor lock detection countter
+// --------------------------------------------------------------------------------------
+bool i2cAccess::i2cClearMLCnt(
+ int32_t i2c_addr
+){
+ char i2c_cmd[NumberOfI2CCommand];
+
+ i2c_cmd[I2C_CP_COMMAND] = 0x4f; // 'O' instruction command
+ i2c_cmd[I2C_CP_MOTORNO] = 0; // instruction command
+ i2c_cmd[I2C_CP_M_DIR] = 0; // motor rotation direction
+ i2c_write( i2c_addr, i2c_cmd, NumberOfI2CCommand );
+
+ return true;
+}
+
+
+// --------------------------------------------------------------------------------------
+// I2C write acccess: get motor current and judgement
+// --------------------------------------------------------------------------------------
+bool i2cAccess::i2cGetMotorCurrent(
+ int32_t i2c_addr,
+ int8_t moror_number,
+ int8_t motor_dir
+){
+ char i2c_cmd[NumberOfI2CCommand];
+
+ i2c_cmd[I2C_CP_COMMAND] = 'G'; // instruction command
+ i2c_cmd[I2C_CP_MOTORNO] = moror_number; // instruction command
+ i2c_cmd[I2C_CP_M_DIR] = motor_dir; // motor rotation direction
+
+ i2c_write( i2c_addr, i2c_cmd, NumberOfI2CCommand );
+
+ return true;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/i2cAccess.h Thu Jul 26 00:20:04 2018 +0000
@@ -0,0 +1,37 @@
+class i2cAccess
+{
+private:
+
+
+public:
+
+ bool flg_motor_lock;
+ /*
+ 7: -
+ 6:
+ 5: LB transform reverse
+ 4: LB transform forward
+ 3: RF transform reverse
+ 2: RF transform forward
+ 1: winch motor reverse
+ 0: winch motor forward
+ */
+
+ int16_t sp_wdram_f; // motor speed
+ int16_t sp_wdram_r; // motor speed
+ int16_t sp_wcabl_f; // motor speed
+ int16_t sp_wcabl_r; // motor speed
+
+ int16_t drop_ammount; // Winch drop ammount
+
+ i2cAccess();
+ bool i2c_read( int , const char* , int );
+ bool i2c_write( int , const char* , int );
+ int16_t i2cReadMotorCurrent( int32_t, int, int, int, int );
+ int16_t i2cReadInformation( int32_t, int );
+ bool i2cGetMotorCurrent( int32_t, int8_t, int8_t );
+ bool i2cSetMotorThreshold( int32_t, int8_t, int8_t, int16_t );
+ bool i2cClearMLCnt( int32_t );
+ bool i2cReadParameters( int32_t );
+
+};
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/lfsAccess.cpp Thu Jul 26 00:20:04 2018 +0000
@@ -0,0 +1,61 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "stdio.h"
+#include "common.h"
+#include "lfsAccess.h"
+
+
+bool lfsAccess::readSetting( void )
+{
+ FILE *fp;
+ char *fname = "/local/dset.txt";
+ char s[150];
+ int c;
+ int data;
+
+ fp = fopen(fname, "r");
+ if( fp != NULL ){ // Open "set.txt" on the local file system for writing
+ c = getc(fp);
+ if( c != '#' ){
+ return false;
+ }
+ else{
+ fgets(s,100,fp);
+ fscanf(fp,"%04d",&data); setValue.wchCtrl.cth_dram_mtr_f = data; fgets(s,100,fp);
+ fscanf(fp,"%04d",&data); setValue.wchCtrl.cth_dram_mtr_r = data; fgets(s,100,fp);
+ fscanf(fp,"%04d",&data); setValue.wchCtrl.cth_cabl_mtr_f = data; fgets(s,100,fp);
+ fscanf(fp,"%04d",&data); setValue.wchCtrl.cth_cabl_mtr_r = data; fgets(s,100,fp);
+ fscanf(fp,"%05d",&data); setValue.wchCtrl.dram_dmtr_x100 = data; fgets(s,100,fp);
+ fscanf(fp,"%05d",&data); setValue.wchCtrl.adj_val_x10000 = data; fgets(s,100,fp);
+ fscanf(fp,"%03d",&data); setValue.wchCtrl.res_resolution = data; fgets(s,100,fp);
+ fscanf(fp,"%03d",&data); setValue.wchCtrl.reserved_1 = data; fgets(s,100,fp);
+ fscanf(fp,"%06d",&data); setValue.wchCtrl.move_interval_cw = data; fgets(s,100,fp);
+ fscanf(fp,"%06d",&data); setValue.wchCtrl.move_interval_ccw = data; fgets(s,100,fp);
+ }
+ fclose(fp);
+ }
+ else{
+ return false;
+ }
+ return true;
+}
+
+
+bool lfsAccess::write_LFS_data( char *fname, char* data )
+{
+ FILE *fp;
+
+ fp = fopen( fname, "a" );
+ if( fp != NULL ){
+ fprintf(fp, data );
+ fprintf(fp, "\r\n" );
+ Thread::wait(30);
+ fclose(fp);
+ Thread::wait(30);
+ }
+ else{
+ return false;
+ }
+ return true;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/lfsAccess.h Thu Jul 26 00:20:04 2018 +0000
@@ -0,0 +1,12 @@
+class lfsAccess
+{
+private:
+ // Global Parameter of setting
+
+ bool lfsAccess::write_LFS_data( char*, char* );
+
+public:
+ setValue_t setValue; // Setting Data
+
+ bool readSetting( void ); // read setting from LFS
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Thu Jul 26 00:20:04 2018 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os/#e2617cc0e17f5c3fc2bae6a589c9bcfd3d1a717b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mtrAccess.cpp Thu Jul 26 00:20:04 2018 +0000
@@ -0,0 +1,154 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "stdio.h"
+#include "common.h"
+//#include "com_func.h"
+#include "mtrAccess.h"
+
+Serial sdc2130(p28, p27); // Communicate with RpboteQ Driver by tx, rx
+Mutex stdio_mutex;
+
+
+mtrAccess::mtrAccess()
+{
+ MCTR_CANID_PTORWCH = 3; // PAN,TILT(B1) or Winch(B2) motor ID
+ MCTR_CANID_TRANSFORM = 2; // TRANSFORM motor ID
+ MCTR_CANID_CRAWLER = 1; // CRAWLER motor ID
+
+ flg_timerint_motor = false;
+ flg_ival_motor_cw = false;
+ flg_ival_motor_ccw = false;
+}
+
+// ======================================================================
+// Send Motor command to motor controller
+// ======================================================================
+bool mtrAccess::sndCmd2MC(
+ int rcan_id, // RoboCAN Motor controller id
+ int no, // Motor number (1 or 2)
+ int speed // Motor Speed <-- this is 10x data
+){
+ sdc2130.printf( "@%02d!G %d %d\r", rcan_id, no, speed );
+ return true;
+}
+
+
+bool mtrAccess::setBaudRate( int baudrate )
+{
+ sdc2130.baud(baudrate);
+ return true;
+}
+
+// ======================================================================
+// Get command from client and send to motor controller
+// ======================================================================
+bool mtrAccess::cmdControl(
+ char* cmd, // Operationg Command
+ int sizeofcmd, // Command size
+ int speed_m1, // real speed x 10 for motor1
+ int speed_m2 // real speed x 10 for motor2
+){
+ // Winch
+ if ( !strncmp( cmd, "XX_DRAM", sizeofcmd ) ) {
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );
+ }
+ else if ( !strncmp(cmd, "XX_CABL", sizeofcmd) ) {
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );
+ }
+ else if ( !strncmp( cmd,"XX_BOTH", sizeofcmd) )
+ {
+ if(( speed_m1 == 0 )||(speed_m2 == 0))
+ {
+ if( speed_m1 == 0 )
+ {
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, 0 );
+ }
+ if( speed_m2 == 0 )
+ {
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, 0 );
+ }
+ }
+ else{
+ if( flg_ival_motor_cw == true )
+ {
+ if( move_interval_cw >=0 )
+ {
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );
+ if( flg_timerint_motor == true )
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );
+ }
+ else
+ {
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );
+ if( flg_timerint_motor == true )
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );
+ }
+ }
+ else if ( flg_ival_motor_ccw == true )
+ {
+ if( move_interval_ccw >=0 )
+ {
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );
+ if( flg_timerint_motor == true )
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );
+ }
+ else
+ {
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );
+ if( flg_timerint_motor == true )
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );
+ }
+ }
+ }
+ }
+
+/*
+ // Crawler
+ else if ( !strncmp( cmd, "XX_CLRF", sizeofcmd ) ) {
+ sndCmd2MC( MCTR_CANID_CRAWLER, MOTOR_NO1, speed );
+ }
+ else if ( !strncmp(cmd, "XX_CLLB", sizeofcmd) ) {
+ sndCmd2MC( MCTR_CANID_CRAWLER, MOTOR_NO2, speed );
+ }
+ // Crawler
+ else if ( !strncmp( cmd, "XX_TFRF", sizeofcmd ) ) {
+ sndCmd2MC( MCTR_CANID_TRANSFORM, MOTOR_NO1, speed );
+ }
+ else if ( !strncmp(cmd, "XX_TFLB", sizeofcmd) ) {
+ sndCmd2MC( MCTR_CANID_TRANSFORM, MOTOR_NO2, speed );
+ }
+ // Camera PAN
+ else if ( !strncmp(cmd, "XX_CPAN", sizeofcmd) ) {
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed );
+ }
+ else if ( !strncmp(cmd, "XX_CPAN", sizeofcmd) ) {
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed );
+ }
+ // Camera PAN
+ else if ( !strncmp(cmd, "XX_CTLT", sizeofcmd) ) {
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed );
+ }
+ else if ( !strncmp(cmd, "XX_CTLT", sizeofcmd) ) {
+ sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed );
+ }
+*/
+ return true;
+}
+
+// ============================================================
+// Read motor current
+// ============================================================
+int mtrAccess::readMotorCurrent(
+ int motor_id, // Motor CAN ID
+ int motor_no, // Motor Number ( 1 or 2 )
+ int motor_dir // Motor direction
+){
+
+ float motor_current;
+ int i;
+ int mc_abs_pct = 0;
+ int mc_Threshold = 0;
+
+
+ return mc_abs_pct;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mtrAccess.h Thu Jul 26 00:20:04 2018 +0000
@@ -0,0 +1,41 @@
+class mtrAccess
+{
+private:
+
+ float motor1_current_center_value;
+ float motor2_current_center_value;
+ float motor3_current_center_value;
+ float motor1_current_fwd_thd;
+ float motor1_current_rvs_thd;
+ float motor2_current_fwd_thd;
+ float motor2_current_rvs_thd;
+ uint32_t motor_pantiltwch_1_lock_count;
+ uint32_t motor_pantiltwch_2_lock_count;
+ // Motor lock
+ volatile bool flg_motor_pantiltwch_1_lockcup;
+ volatile bool flg_motor_pantiltwch_2_lockcup;
+ volatile uint32_t flg_motor_pantiltwch_1_lock;
+ volatile uint32_t flg_motor_pantiltwch_2_lock;
+
+
+ int MCTR_CANID_CRAWLER; // CRAWLER motor ID
+ int MCTR_CANID_PTORWCH; // PAN,TILT(B1) or Winch(B2) motor ID
+ int MCTR_CANID_TRANSFORM; // TRANSFORM motor ID
+
+ bool sndCmd2MC( int rcan_id, int no, int speed );
+
+
+public:
+
+ bool flg_timerint_motor;
+ bool flg_ival_motor_cw;
+ bool flg_ival_motor_ccw;
+
+ int16_t move_interval_cw; // moving interval m1 to m2 cw rotation
+ int16_t move_interval_ccw; // moving interval m1 to m2 ccw rotation
+
+ mtrAccess();
+ bool setBaudRate( int baudrate );
+ bool cmdControl( char* cmd, int sizeofcmd, int speed_m1, int speed_m2 );
+ int readMotorCurrent( int motor_id, int motor_no, int motor_dir );
+};
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/music.cpp Thu Jul 26 00:20:04 2018 +0000
@@ -0,0 +1,150 @@
+#include "mbed.h"
+#include "stdio.h"
+#include "common.h"
+#include "music.h"
+
+// JKSoft Blue mbed Board Specific Seting
+AnalogOut DACout(p18);
+DigitalOut AMPEnable(p12);
+
+void waveMusic::wave(float volume , float fq , float time)
+{
+ float w_time = 1.0 / fq;
+
+ AMPEnable = 0;
+ for (float i=0; i<time / w_time; i++) {
+ DACout = volume;
+ wait(w_time/2);
+ DACout = 0.0;
+ wait(w_time/2);
+ }
+ AMPEnable = 1;
+
+}
+
+void waveMusic::pi( int count )
+{
+ for( int i=0; i < count; i++)
+ {
+ // vol freq time
+ wave( 0.8, 3000.0 * 1, 0.1 );
+ wave( 0.0, 3000.0 * 1, 0.1 );
+ }
+ Thread::wait(80); // msec
+}
+
+
+void waveMusic::bz( int count )
+{
+ for( int i=0; i < count; i++)
+ {
+ // vol freq time
+ wave( 0.6, 3000.0, 0.1 );
+ wave( 0.0, 3000.0, 0.1 );
+ }
+ Thread::wait(150); // msec
+}
+
+void waveMusic::bz_error( int count )
+{
+ for( int i=0; i < count; i++)
+ {
+ // vol freq time
+ wave( 0.6, 3000.0, 0.3 );
+ wave( 0.0, 3000.0, 0.3 );
+ }
+ Thread::wait(300); // msec
+}
+
+
+void waveMusic::note(
+ float on_freq,
+ float on_time,
+ float off_time
+){
+ wave( 0.03, on_freq, on_time );
+ wave( 0.0, on_freq, off_time );
+}
+
+void waveMusic::m_poteto( int count )
+{
+ for( int i=0; i < count; i++)
+ {
+ Thread::wait(30); // msec
+ note( 392.00*3, 0.2, 0.01 ); // so
+ note( 329.00*3, 0.3, 0.01 ); // mi
+ note( 392.00*3, 0.2, 0.01 ); // so
+ Thread::wait(30); // msec
+ }
+}
+
+void waveMusic::knkk( void )
+{
+ Thread::wait(300); // msec
+ note( 146.82*3, 0.3, 0.08 ); // shi
+ note( 220.00*3, 0.6, 0.08 ); // ra
+ note( 246.94*3, 0.3, 0.08 ); // shi
+/* note( 261.63*3, 0.1, 0.02 ); // do
+ note( 261.63*3, 0.1, 0.02 ); // do
+ note( 246.94*3, 0.1, 0.02 ); // shi
+ note( 220.00*3, 0.1, 0.02 ); // ra
+ note( 146.835*3, 0.1, 0.02 ); // re
+ note( 146.835*3, 0.1, 0.02 ); // re
+ note( 146.835*3, 0.1, 0.02); // re
+ note( 164.82*3, 0.1, 0.02 ); // mi
+ note( 174.62*3, 0.3, 0.02 ); // fa */
+ Thread::wait(300); // msec
+}
+
+void waveMusic::PC9801( void )
+{
+ Thread::wait(300); // msec
+ note( 2000.0, 0.2, 0.05 ); // pi
+ note( 1000.0, 0.2, 0.05 ); // po
+ Thread::wait(300); // msec
+}
+
+//#define ON_TIME 0.08
+//#define OFF_TIME 0.008
+#define ON_TIME 0.08
+#define OFF_TIME 0.008
+void waveMusic::FFPre(void)
+{
+ Thread::wait(300); // msec
+// note( 130.815*3, ON_TIME, OFF_TIME ); // do
+// note( 146.835*3, ON_TIME, OFF_TIME ); // re
+// note( 164.820*3, ON_TIME, OFF_TIME ); // mi
+// note( 196.000*3, ON_TIME, OFF_TIME ); // so
+
+ note( 261.630*2, ON_TIME, OFF_TIME ); // do
+ note( 293.670*2, ON_TIME, OFF_TIME ); // re
+ note( 329.630*2, ON_TIME, OFF_TIME ); // mi
+ note( 392.000*2, ON_TIME, OFF_TIME ); // so
+
+ note( 523.230*2, ON_TIME, OFF_TIME ); // do
+ note( 587.340*2, ON_TIME, OFF_TIME ); // re
+ note( 659.250*2, ON_TIME, OFF_TIME ); // mi
+ note( 783.980*2, ON_TIME, OFF_TIME*2 ); // so
+
+ note( 1046.500*2, ON_TIME, OFF_TIME ); // do
+ note( 1174.700*2, ON_TIME, OFF_TIME ); // re
+ note( 1318.600*2, ON_TIME, OFF_TIME ); // mi
+ note( 1567.960*2, ON_TIME, OFF_TIME ); // so
+
+ note( 2093.000*2, ON_TIME, OFF_TIME ); // do
+ note( 1567.960*2, ON_TIME, OFF_TIME ); // so
+ note( 1318.600*2, ON_TIME, OFF_TIME ); // mi
+ note( 1174.700*2, ON_TIME, OFF_TIME ); // re
+
+ note( 1046.500*2, ON_TIME, OFF_TIME ); // do
+ note( 783.980*2, ON_TIME, OFF_TIME ); // so
+ note( 659.250*2, ON_TIME, OFF_TIME ); // mi
+ note( 587.340*2, ON_TIME, OFF_TIME ); // re
+
+ note( 523.230*2, ON_TIME, OFF_TIME ); // do
+ note( 392.000*2, ON_TIME, OFF_TIME ); // so
+ note( 329.630*2, ON_TIME, OFF_TIME ); // mi
+ note( 293.670*2, ON_TIME, OFF_TIME ); // re
+
+// Thread::wait(300); // msec
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/music.h Thu Jul 26 00:20:04 2018 +0000
@@ -0,0 +1,15 @@
+class waveMusic
+{
+private:
+
+public:
+ void wave(float volume , float fq , float time);
+ void bz( int count );
+ void pi( int count );
+ void bz_error( int count );
+ void note( float on_time, float on_freq, float off_time );
+ void knkk( void );
+ void m_poteto( int count );
+ void PC9801( void );
+ void FFPre( void );
+};
\ No newline at end of file