z ysaito / Mbed OS APro_B2DTst_0_Can_Class

2018.07.26

Dependencies:   WebSocketClient

P0_main.cpp@0:b3376afd10d8, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:20:04 2018 +0000
Revision:
0:b3376afd10d8
2018.07.26

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:b3376afd10d8 1 /*
sayzyas 0:b3376afd10d8 2 //////////////////////////////////////////////////////////
sayzyas 0:b3376afd10d8 3 Project: B2Dash Fix falling winch test model
sayzyas 0:b3376afd10d8 4 Data: 2017.July to September
sayzyas 0:b3376afd10d8 5 Title: 1F-1 Explorer Project
sayzyas 0:b3376afd10d8 6 Target: mbed LPC1768
sayzyas 0:b3376afd10d8 7 Author: zStranger
sayzyas 0:b3376afd10d8 8 -----------------------------------------------
sayzyas 0:b3376afd10d8 9 ##### ##### ##### ##### ##### #####
sayzyas 0:b3376afd10d8 10 # # # # # #
sayzyas 0:b3376afd10d8 11 # # # # # #
sayzyas 0:b3376afd10d8 12 # # # # # #
sayzyas 0:b3376afd10d8 13 # # # # # #
sayzyas 0:b3376afd10d8 14 # # # # # #
sayzyas 0:b3376afd10d8 15 ##### ##### ##### ##### ##### #####
sayzyas 0:b3376afd10d8 16 -----------------------------------------------
sayzyas 0:b3376afd10d8 17 X X XXX XXX X XXXX XX XX X
sayzyas 0:b3376afd10d8 18 XX X X X X X X X X X X x XX
sayzyas 0:b3376afd10d8 19 XXX X X X X X X XXX X X XXX
sayzyas 0:b3376afd10d8 20 XXXX X XXX X X X X X XX XXXX
sayzyas 0:b3376afd10d8 21 XXX X X X X X X X X x XXX
sayzyas 0:b3376afd10d8 22 XX X X X X X X X X X X X XX
sayzyas 0:b3376afd10d8 23 X XXXX X XXX X X XXX XX X
sayzyas 0:b3376afd10d8 24
sayzyas 0:b3376afd10d8 25 mbed LPC1768
sayzyas 0:b3376afd10d8 26 +-------------USB-----------+
sayzyas 0:b3376afd10d8 27 | GND VOUT(3.3V) |
sayzyas 0:b3376afd10d8 28 | VIN VU(5.0V OUT)|
sayzyas 0:b3376afd10d8 29 | VB IF- |
sayzyas 0:b3376afd10d8 30 | mR IF+ |
sayzyas 0:b3376afd10d8 31 | p5 mosi Ether RD- |
sayzyas 0:b3376afd10d8 32 | p6 miso Ether RD+ |
sayzyas 0:b3376afd10d8 33 | p7 sck Ether TD- |
sayzyas 0:b3376afd10d8 34 | p8 Ether TD+ |
sayzyas 0:b3376afd10d8 35 IIC SDA | p9 tx sdi USB D- |
sayzyas 0:b3376afd10d8 36 IIC SCL | p10 rx scl USB D+ |
sayzyas 0:b3376afd10d8 37 | p11 mosi CAN rd p30 |
sayzyas 0:b3376afd10d8 38 | p12 miso CAN td p29 |
sayzyas 0:b3376afd10d8 39 | p13 tx sck sda tx p28 | Tx(SDC2130)
sayzyas 0:b3376afd10d8 40 | p14 rx scl rx P27 | Rx(SDC2130)
sayzyas 0:b3376afd10d8 41 Int | p15 AIn PWM P26 |
sayzyas 0:b3376afd10d8 42 RJS U/D | p16 AIn PWM P25 |
sayzyas 0:b3376afd10d8 43 LJS U/D | p17 AIn PWM p24 | SW(Auto/Manuu)
sayzyas 0:b3376afd10d8 44 | p18 AIn AOut PWM p23 |
sayzyas 0:b3376afd10d8 45 | p19 AIn PWM p22 |
sayzyas 0:b3376afd10d8 46 | p20 AIn PWM p21 |
sayzyas 0:b3376afd10d8 47 +---------------------------+
sayzyas 0:b3376afd10d8 48
sayzyas 0:b3376afd10d8 49 //////////////////////////////////////////////////////////
sayzyas 0:b3376afd10d8 50 */
sayzyas 0:b3376afd10d8 51
sayzyas 0:b3376afd10d8 52 #include "mbed.h"
sayzyas 0:b3376afd10d8 53 #include "TCPSocket.h"
sayzyas 0:b3376afd10d8 54 #include <EthernetInterface.h>
sayzyas 0:b3376afd10d8 55 #include "TCPServer.h"
sayzyas 0:b3376afd10d8 56 #include "TCPSocket.h"
sayzyas 0:b3376afd10d8 57 #include "common.h"
sayzyas 0:b3376afd10d8 58 #include "debugprint.h"
sayzyas 0:b3376afd10d8 59 #include "mtrAccess.h"
sayzyas 0:b3376afd10d8 60 #include "i2cAccess.h"
sayzyas 0:b3376afd10d8 61 #include "lfsAccess.h"
sayzyas 0:b3376afd10d8 62 #include "music.h"
sayzyas 0:b3376afd10d8 63 #include "Websocket.h"
sayzyas 0:b3376afd10d8 64
sayzyas 0:b3376afd10d8 65 /* ====================================================================== */
sayzyas 0:b3376afd10d8 66 /* System configuration */
sayzyas 0:b3376afd10d8 67 /* ====================================================================== */
sayzyas 0:b3376afd10d8 68 Thread thread;
sayzyas 0:b3376afd10d8 69 // One time timer
sayzyas 0:b3376afd10d8 70 Timeout onetimer;
sayzyas 0:b3376afd10d8 71
sayzyas 0:b3376afd10d8 72 /* Ethernet */
sayzyas 0:b3376afd10d8 73 EthernetInterface eth;
sayzyas 0:b3376afd10d8 74 TCPSocket socket;
sayzyas 0:b3376afd10d8 75 TCPServer tcp_server; // TCP Server
sayzyas 0:b3376afd10d8 76 TCPSocket tcp_client;
sayzyas 0:b3376afd10d8 77 SocketAddress tcp_client_addr;
sayzyas 0:b3376afd10d8 78
sayzyas 0:b3376afd10d8 79 /* Serial */
sayzyas 0:b3376afd10d8 80 Serial pc(USBTX, USBRX); // UART
sayzyas 0:b3376afd10d8 81 //Serial sdc2130(p28, p27); // Communicate with RpboteQ Driver by tx, rx
sayzyas 0:b3376afd10d8 82 /* IIC */
sayzyas 0:b3376afd10d8 83 //I2C i2c(p9, p10); // I2C SDA, SCL is good
sayzyas 0:b3376afd10d8 84 /* Digital IO */
sayzyas 0:b3376afd10d8 85 DigitalOut onbdled_1(LED1);
sayzyas 0:b3376afd10d8 86 DigitalOut onbdled_2(LED2);
sayzyas 0:b3376afd10d8 87 DigitalOut onbdled_3(LED3);
sayzyas 0:b3376afd10d8 88 DigitalOut onbdled_4(LED4);
sayzyas 0:b3376afd10d8 89 DigitalIn wch_opmode(p24); // SW: Winch operation auto = 1, Manual = 0
sayzyas 0:b3376afd10d8 90 /* Analig I/O */
sayzyas 0:b3376afd10d8 91 AnalogIn Rjsin_UD(p16); // R-JotStick Up/Down
sayzyas 0:b3376afd10d8 92 AnalogIn Ljsin_UD(p17); // L-JoyStick Up/Down : Manual setting only
sayzyas 0:b3376afd10d8 93
sayzyas 0:b3376afd10d8 94 InterruptIn intfrom824(p15); // Interrupt pin from 824MAX
sayzyas 0:b3376afd10d8 95 bool flg_JSon = false;
sayzyas 0:b3376afd10d8 96
sayzyas 0:b3376afd10d8 97 mtrAccess mtrAcs;
sayzyas 0:b3376afd10d8 98 bool flgMtrStp = false;
sayzyas 0:b3376afd10d8 99
sayzyas 0:b3376afd10d8 100 waveMusic wMusic;
sayzyas 0:b3376afd10d8 101 Mutex music_mutex;
sayzyas 0:b3376afd10d8 102
sayzyas 0:b3376afd10d8 103 // Local File System
sayzyas 0:b3376afd10d8 104 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
sayzyas 0:b3376afd10d8 105
sayzyas 0:b3376afd10d8 106 /* ====================================================================== */
sayzyas 0:b3376afd10d8 107 /* Interrupt handler */
sayzyas 0:b3376afd10d8 108 /* ====================================================================== */
sayzyas 0:b3376afd10d8 109 void intRise(void) { // Port Rising
sayzyas 0:b3376afd10d8 110 // Process
sayzyas 0:b3376afd10d8 111 //mtrAcs.cmdControl( "XX_CABL", 7, 0 ); // Motor on
sayzyas 0:b3376afd10d8 112 onbdled_3 = 1;
sayzyas 0:b3376afd10d8 113 flgMtrStp = true;
sayzyas 0:b3376afd10d8 114 }
sayzyas 0:b3376afd10d8 115
sayzyas 0:b3376afd10d8 116 void intFall(void) { // Port Falling
sayzyas 0:b3376afd10d8 117 //onbdled_3 = 0;
sayzyas 0:b3376afd10d8 118 //flgMtrStp = false;
sayzyas 0:b3376afd10d8 119 }
sayzyas 0:b3376afd10d8 120
sayzyas 0:b3376afd10d8 121 int16_t adjust_jsvalue( int16_t data )
sayzyas 0:b3376afd10d8 122 {
sayzyas 0:b3376afd10d8 123 float speed;
sayzyas 0:b3376afd10d8 124
sayzyas 0:b3376afd10d8 125 speed = (float)data * (float)data * (float)100 / 16384;
sayzyas 0:b3376afd10d8 126 return (int16_t)speed;
sayzyas 0:b3376afd10d8 127 }
sayzyas 0:b3376afd10d8 128
sayzyas 0:b3376afd10d8 129
sayzyas 0:b3376afd10d8 130 uint8_t rjs_center_value_ud = 0x80;
sayzyas 0:b3376afd10d8 131 uint8_t ljs_center_value_ud = 0x80;
sayzyas 0:b3376afd10d8 132 /* ====================================================================== */
sayzyas 0:b3376afd10d8 133 /*
sayzyas 0:b3376afd10d8 134 Joystick calibration joystick
sayzyas 0:b3376afd10d8 135 255 Upper
sayzyas 0:b3376afd10d8 136 +---+
sayzyas 0:b3376afd10d8 137 |127|
sayzyas 0:b3376afd10d8 138 +---+
sayzyas 0:b3376afd10d8 139 0 Lower
sayzyas 0:b3376afd10d8 140 */
sayzyas 0:b3376afd10d8 141 /* ====================================================================== */
sayzyas 0:b3376afd10d8 142 bool calibrate_joystick( uint8_t rdata, uint8_t ldata ){
sayzyas 0:b3376afd10d8 143 uint16_t tmp;
sayzyas 0:b3376afd10d8 144
sayzyas 0:b3376afd10d8 145 tmp = (uint16_t)rjs_center_value_ud;
sayzyas 0:b3376afd10d8 146 tmp += (uint16_t)rdata;
sayzyas 0:b3376afd10d8 147 tmp /= 2;
sayzyas 0:b3376afd10d8 148 rjs_center_value_ud = (uint8_t)tmp;
sayzyas 0:b3376afd10d8 149
sayzyas 0:b3376afd10d8 150 tmp = (uint16_t)ljs_center_value_ud;
sayzyas 0:b3376afd10d8 151 tmp += (uint16_t)ldata;
sayzyas 0:b3376afd10d8 152 tmp /= 2;
sayzyas 0:b3376afd10d8 153 ljs_center_value_ud = (uint8_t)tmp;
sayzyas 0:b3376afd10d8 154
sayzyas 0:b3376afd10d8 155 if
sayzyas 0:b3376afd10d8 156 (
sayzyas 0:b3376afd10d8 157 (((127-10) < rjs_center_value_ud )&&(rjs_center_value_ud < (127+10))) &&
sayzyas 0:b3376afd10d8 158 (((127-10) < ljs_center_value_ud )&&(ljs_center_value_ud < (127+10)))
sayzyas 0:b3376afd10d8 159 )
sayzyas 0:b3376afd10d8 160 {
sayzyas 0:b3376afd10d8 161 return true;
sayzyas 0:b3376afd10d8 162 }
sayzyas 0:b3376afd10d8 163 else
sayzyas 0:b3376afd10d8 164 {
sayzyas 0:b3376afd10d8 165 return false;
sayzyas 0:b3376afd10d8 166 }
sayzyas 0:b3376afd10d8 167 }
sayzyas 0:b3376afd10d8 168
sayzyas 0:b3376afd10d8 169 lfsAccess lfsAcs;
sayzyas 0:b3376afd10d8 170
sayzyas 0:b3376afd10d8 171
sayzyas 0:b3376afd10d8 172 void set_motor_interval_flag()
sayzyas 0:b3376afd10d8 173 {
sayzyas 0:b3376afd10d8 174 mtrAcs.flg_timerint_motor = true;
sayzyas 0:b3376afd10d8 175 }
sayzyas 0:b3376afd10d8 176
sayzyas 0:b3376afd10d8 177 /* ====================================================================== */
sayzyas 0:b3376afd10d8 178 /* Task(Thread) : JoyStick and Switch check and operation */
sayzyas 0:b3376afd10d8 179 /* ====================================================================== */
sayzyas 0:b3376afd10d8 180 //void sw_task( void const *){ // for OS2
sayzyas 0:b3376afd10d8 181 void sw_task(){ // for OS5
sayzyas 0:b3376afd10d8 182 uint16_t rjs_ud_data;
sayzyas 0:b3376afd10d8 183 uint8_t rjs_ud_undata;
sayzyas 0:b3376afd10d8 184 uint16_t ljs_ud_data;
sayzyas 0:b3376afd10d8 185 uint8_t ljs_ud_undata;
sayzyas 0:b3376afd10d8 186 // bool jsjg = true;
sayzyas 0:b3376afd10d8 187
sayzyas 0:b3376afd10d8 188 int16_t rjs_speed;
sayzyas 0:b3376afd10d8 189 int16_t ljs_speed;
sayzyas 0:b3376afd10d8 190 int16_t sp_wdram;
sayzyas 0:b3376afd10d8 191 int16_t sp_wcabl;
sayzyas 0:b3376afd10d8 192
sayzyas 0:b3376afd10d8 193 char I2C_cmd[12];
sayzyas 0:b3376afd10d8 194 i2cAccess i2cAcs;
sayzyas 0:b3376afd10d8 195
sayzyas 0:b3376afd10d8 196 char msg[128];
sayzyas 0:b3376afd10d8 197 //int dropAmount = 0;
sayzyas 0:b3376afd10d8 198
sayzyas 0:b3376afd10d8 199 mtrAcs.setBaudRate(115200);
sayzyas 0:b3376afd10d8 200
sayzyas 0:b3376afd10d8 201 DEBUG_PRINT_L0("LPC1768> Start ststem initializing ...\r\n");
sayzyas 0:b3376afd10d8 202 DEBUG_PRINT_L0("LPC1768> -------------------------------------\r\n");
sayzyas 0:b3376afd10d8 203 DEBUG_PRINT_L0("LPC1768> Initalizing Ethernet ...\r\n");
sayzyas 0:b3376afd10d8 204 DEBUG_PRINT_L0("LPC1768> -------------------------------------\r\n");
sayzyas 0:b3376afd10d8 205
sayzyas 0:b3376afd10d8 206 // Send drop amount calculation base data to main
sayzyas 0:b3376afd10d8 207 I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Winch is stopping (send calculate base data)
sayzyas 0:b3376afd10d8 208 I2C_cmd[I2C_CP_WDRAM_DIA_X100_UPPER] = ( lfsAcs.setValue.wchCtrl.dram_dmtr_x100 >> 8 ) & 0xFF;
sayzyas 0:b3376afd10d8 209 I2C_cmd[I2C_CP_WDRAM_DIA_X100_LOWER] = lfsAcs.setValue.wchCtrl.dram_dmtr_x100 & 0xFF;
sayzyas 0:b3376afd10d8 210 I2C_cmd[I2C_CP_ADJUST_VALUE_X10000_UPPER] = ( lfsAcs.setValue.wchCtrl.adj_val_x10000 >> 8 ) & 0xFF;
sayzyas 0:b3376afd10d8 211 I2C_cmd[I2C_CP_ADJUST_VALUE_X10000_LOWER] = lfsAcs.setValue.wchCtrl.adj_val_x10000 & 0xFF;
sayzyas 0:b3376afd10d8 212 I2C_cmd[I2C_CP_RESOLVER_RESO] = lfsAcs.setValue.wchCtrl.res_resolution & 0xFF;
sayzyas 0:b3376afd10d8 213 if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
sayzyas 0:b3376afd10d8 214 {
sayzyas 0:b3376afd10d8 215 DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
sayzyas 0:b3376afd10d8 216 }
sayzyas 0:b3376afd10d8 217
sayzyas 0:b3376afd10d8 218 #ifdef __NETWORK_ACCESS__
sayzyas 0:b3376afd10d8 219 const char* ip_address = "192.168.11.31";
sayzyas 0:b3376afd10d8 220 const char* subnet_mask = "255.255.255.0";
sayzyas 0:b3376afd10d8 221 const char* gateway = "192.168.0.0";
sayzyas 0:b3376afd10d8 222 DEBUG_PRINT_L0("LPC1768> -- set -------------------------------\r\n");
sayzyas 0:b3376afd10d8 223 DEBUG_PRINT_L0("LPC1768> ip address : %s\r\n", ip_address);
sayzyas 0:b3376afd10d8 224 DEBUG_PRINT_L0("LPC1768> subnet mask : %s\r\n", subnet_mask);
sayzyas 0:b3376afd10d8 225 DEBUG_PRINT_L0("LPC1768> default gateway: %s\r\n", gateway);
sayzyas 0:b3376afd10d8 226 DEBUG_PRINT_L0("LPC1768> --------------------------------------\r\n");
sayzyas 0:b3376afd10d8 227 //// eth.init(ip_address, subnet_mask, gateway); // For mbed OS 2.X ??
sayzyas 0:b3376afd10d8 228 //eth.set_network( ip_address, subnet_mask, gateway ); // For mbed OS5 or later ??
sayzyas 0:b3376afd10d8 229 eth.connect();
sayzyas 0:b3376afd10d8 230 DEBUG_PRINT_L0("LPC1768> -- got -------------------------------\r\n");
sayzyas 0:b3376afd10d8 231 DEBUG_PRINT_L0("LPC1768> The target ip address : '%s'\r\n", eth.get_ip_address());
sayzyas 0:b3376afd10d8 232 DEBUG_PRINT_L0("LPC1768> The target netmask : '%s'\r\n", eth.get_netmask());
sayzyas 0:b3376afd10d8 233 DEBUG_PRINT_L0("LPC1768> The target gateway : '%s'\r\n", eth.get_gateway());
sayzyas 0:b3376afd10d8 234 DEBUG_PRINT_L0("LPC1768> The target mac address: '%s'\r\n", eth.get_mac_address());
sayzyas 0:b3376afd10d8 235 DEBUG_PRINT_L0("LPC1768> --------------------------------------\r\n");
sayzyas 0:b3376afd10d8 236 #endif // __NETWORK_ACCESS__
sayzyas 0:b3376afd10d8 237
sayzyas 0:b3376afd10d8 238 wMusic.FFPre();
sayzyas 0:b3376afd10d8 239
sayzyas 0:b3376afd10d8 240 while(1)
sayzyas 0:b3376afd10d8 241 {
sayzyas 0:b3376afd10d8 242 #ifdef __NETWORK_ACCESS__
sayzyas 0:b3376afd10d8 243 // Create a websocket instance
sayzyas 0:b3376afd10d8 244 Websocket ws("ws://192.168.11.10:9999/", &eth);
sayzyas 0:b3376afd10d8 245 bool connect_error = ws.connect();
sayzyas 0:b3376afd10d8 246 // tcp_server.open(&eth);
sayzyas 0:b3376afd10d8 247 // tcp_server.bind(TCP_CMDSERVER_PORT);
sayzyas 0:b3376afd10d8 248 // tcp_server.listen(5);
sayzyas 0:b3376afd10d8 249 #endif // __NETWORK_ACCESS__
sayzyas 0:b3376afd10d8 250 if( connect_error != false )
sayzyas 0:b3376afd10d8 251 {
sayzyas 0:b3376afd10d8 252 // Send drop amount calculation base data to main
sayzyas 0:b3376afd10d8 253 I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Winch is stopping (send calculate base data)
sayzyas 0:b3376afd10d8 254 I2C_cmd[I2C_CP_WDRAM_DIA_X100_UPPER] = ( lfsAcs.setValue.wchCtrl.dram_dmtr_x100 >> 8 ) & 0xFF;
sayzyas 0:b3376afd10d8 255 I2C_cmd[I2C_CP_WDRAM_DIA_X100_LOWER] = lfsAcs.setValue.wchCtrl.dram_dmtr_x100 & 0xFF;
sayzyas 0:b3376afd10d8 256 I2C_cmd[I2C_CP_ADJUST_VALUE_X10000_UPPER] = ( lfsAcs.setValue.wchCtrl.adj_val_x10000 >> 8 ) & 0xFF;
sayzyas 0:b3376afd10d8 257 I2C_cmd[I2C_CP_ADJUST_VALUE_X10000_LOWER] = lfsAcs.setValue.wchCtrl.adj_val_x10000 & 0xFF;
sayzyas 0:b3376afd10d8 258 I2C_cmd[I2C_CP_RESOLVER_RESO] = lfsAcs.setValue.wchCtrl.res_resolution & 0xFF;
sayzyas 0:b3376afd10d8 259 if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
sayzyas 0:b3376afd10d8 260 {
sayzyas 0:b3376afd10d8 261 DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
sayzyas 0:b3376afd10d8 262 }
sayzyas 0:b3376afd10d8 263
sayzyas 0:b3376afd10d8 264 if(lfsAcs.setValue.wchCtrl.move_interval_cw == 0)
sayzyas 0:b3376afd10d8 265 {
sayzyas 0:b3376afd10d8 266 mtrAcs.move_interval_cw = 1; // set 1us for timer because timer does not work when time is set to 0 (temporary setting)
sayzyas 0:b3376afd10d8 267 }
sayzyas 0:b3376afd10d8 268 else
sayzyas 0:b3376afd10d8 269 {
sayzyas 0:b3376afd10d8 270 mtrAcs.move_interval_cw = lfsAcs.setValue.wchCtrl.move_interval_cw;
sayzyas 0:b3376afd10d8 271 }
sayzyas 0:b3376afd10d8 272
sayzyas 0:b3376afd10d8 273 if(lfsAcs.setValue.wchCtrl.move_interval_ccw == 0)
sayzyas 0:b3376afd10d8 274 {
sayzyas 0:b3376afd10d8 275 mtrAcs.move_interval_ccw = 1; // set 1us for timer because timer does not work when time is set to 0 (temporary setting)
sayzyas 0:b3376afd10d8 276 }
sayzyas 0:b3376afd10d8 277 else
sayzyas 0:b3376afd10d8 278 {
sayzyas 0:b3376afd10d8 279 mtrAcs.move_interval_ccw = lfsAcs.setValue.wchCtrl.move_interval_ccw;
sayzyas 0:b3376afd10d8 280 }
sayzyas 0:b3376afd10d8 281 #ifdef __NETWORK_ACCESS__
sayzyas 0:b3376afd10d8 282 int error_c = ws.read(msg);
sayzyas 0:b3376afd10d8 283 DEBUG_PRINT_L0("Received %s", msg);
sayzyas 0:b3376afd10d8 284 #endif // __NETWORK_ACCESS__
sayzyas 0:b3376afd10d8 285
sayzyas 0:b3376afd10d8 286 Thread::wait(10);
sayzyas 0:b3376afd10d8 287 #ifdef __NETWORK_ACCESS__
sayzyas 0:b3376afd10d8 288 // tcp_server.accept(&tcp_client, &tcp_client_addr);
sayzyas 0:b3376afd10d8 289 #endif // __NETWORK_ACCESS__
sayzyas 0:b3376afd10d8 290
sayzyas 0:b3376afd10d8 291 DEBUG_PRINT_L1("Calibrating joystick ... ");
sayzyas 0:b3376afd10d8 292 for( int i = 0; i < CALIBRATION_COUNT; i++){
sayzyas 0:b3376afd10d8 293 rjs_ud_data = Rjsin_UD.read_u16();
sayzyas 0:b3376afd10d8 294 ljs_ud_data = Ljsin_UD.read_u16();
sayzyas 0:b3376afd10d8 295 rjs_ud_undata = (uint8_t)(rjs_ud_data >> 8);
sayzyas 0:b3376afd10d8 296 ljs_ud_undata = (uint8_t)(ljs_ud_data >> 8);
sayzyas 0:b3376afd10d8 297 calibrate_joystick( rjs_ud_undata, ljs_ud_undata );
sayzyas 0:b3376afd10d8 298 }
sayzyas 0:b3376afd10d8 299 while(true)
sayzyas 0:b3376afd10d8 300 {
sayzyas 0:b3376afd10d8 301 i2cAcs.i2cReadParameters( I2C_ADDRESS_RESOLVER );
sayzyas 0:b3376afd10d8 302 DEBUG_PRINT_SW("LPC1768> ");
sayzyas 0:b3376afd10d8 303 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 );
sayzyas 0:b3376afd10d8 304 DEBUG_PRINT_SW("DropAmt: %04d\t | ", i2cAcs.drop_ammount );
sayzyas 0:b3376afd10d8 305
sayzyas 0:b3376afd10d8 306 #ifdef __NETWORK_ACCESS__
sayzyas 0:b3376afd10d8 307 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 );
sayzyas 0:b3376afd10d8 308 error_c = ws.send(msg);
sayzyas 0:b3376afd10d8 309 DEBUG_PRINT_SW("Send(%d): %s, ", error_c, msg);
sayzyas 0:b3376afd10d8 310 if( error_c == -1 )
sayzyas 0:b3376afd10d8 311 {
sayzyas 0:b3376afd10d8 312 DEBUG_PRINT_SW("** send error **\r\n");
sayzyas 0:b3376afd10d8 313 break;
sayzyas 0:b3376afd10d8 314 }
sayzyas 0:b3376afd10d8 315 error_c = ws.read(msg);
sayzyas 0:b3376afd10d8 316 DEBUG_PRINT_SW("Recv(%d): %s\t", error_c, msg);
sayzyas 0:b3376afd10d8 317 #endif // __NETWORK_ACCESS__
sayzyas 0:b3376afd10d8 318
sayzyas 0:b3376afd10d8 319 rjs_ud_data = Rjsin_UD.read_u16();
sayzyas 0:b3376afd10d8 320 ljs_ud_data = Ljsin_UD.read_u16();
sayzyas 0:b3376afd10d8 321
sayzyas 0:b3376afd10d8 322 rjs_ud_undata = (uint8_t)(rjs_ud_data >> 8);
sayzyas 0:b3376afd10d8 323 ljs_ud_undata = (uint8_t)(ljs_ud_data >> 8);
sayzyas 0:b3376afd10d8 324
sayzyas 0:b3376afd10d8 325 if(wch_opmode==1) // SW off is auto mode (RJS only mode)
sayzyas 0:b3376afd10d8 326 {
sayzyas 0:b3376afd10d8 327 if( // Dead zone motor stop
sayzyas 0:b3376afd10d8 328 ( rjs_ud_undata >= (rjs_center_value_ud - DEAD_BAND)) &&
sayzyas 0:b3376afd10d8 329 ( rjs_ud_undata <= (rjs_center_value_ud + DEAD_BAND))
sayzyas 0:b3376afd10d8 330 ){
sayzyas 0:b3376afd10d8 331 if( flg_JSon == true )
sayzyas 0:b3376afd10d8 332 {
sayzyas 0:b3376afd10d8 333 onetimer.detach(); // stop onetimer to call flip
sayzyas 0:b3376afd10d8 334 mtrAcs.flg_timerint_motor = false;
sayzyas 0:b3376afd10d8 335 }
sayzyas 0:b3376afd10d8 336 rjs_speed = 0;
sayzyas 0:b3376afd10d8 337 sp_wdram = rjs_speed;
sayzyas 0:b3376afd10d8 338 sp_wcabl = rjs_speed;
sayzyas 0:b3376afd10d8 339
sayzyas 0:b3376afd10d8 340 // DEBUG_PRINT_SW("LPC1768> ");
sayzyas 0:b3376afd10d8 341 DEBUG_PRINT_SW("RJSonly: %03d,%03d [%3d] : ", sp_wdram, sp_wcabl, rjs_ud_undata );
sayzyas 0:b3376afd10d8 342 DEBUG_PRINT_SW("Dead\r\n");
sayzyas 0:b3376afd10d8 343 mtrAcs.cmdControl( "XX_BOTH", 7, 0, 0 ); // Motor on
sayzyas 0:b3376afd10d8 344 flg_JSon = false;
sayzyas 0:b3376afd10d8 345 flgMtrStp = false;
sayzyas 0:b3376afd10d8 346 mtrAcs.flg_ival_motor_cw = false;
sayzyas 0:b3376afd10d8 347 mtrAcs.flg_ival_motor_ccw = false;
sayzyas 0:b3376afd10d8 348 onbdled_3 = 0;
sayzyas 0:b3376afd10d8 349 }
sayzyas 0:b3376afd10d8 350 // Up motor reverse
sayzyas 0:b3376afd10d8 351 else if( // data: 127 to 255
sayzyas 0:b3376afd10d8 352 ( rjs_ud_undata > (rjs_center_value_ud))
sayzyas 0:b3376afd10d8 353 ){
sayzyas 0:b3376afd10d8 354 if( flg_JSon == false )
sayzyas 0:b3376afd10d8 355 {
sayzyas 0:b3376afd10d8 356 // Attch one time timer interrupt
sayzyas 0:b3376afd10d8 357 // m1 --> timer(+/-) us --> M2
sayzyas 0:b3376afd10d8 358 onetimer.attach_us(&set_motor_interval_flag, abs(mtrAcs.move_interval_ccw*1000)); // setup onetimer to call flip after 1 seconds
sayzyas 0:b3376afd10d8 359 }
sayzyas 0:b3376afd10d8 360 rjs_speed = adjust_jsvalue((int16_t)(rjs_ud_undata - rjs_center_value_ud));
sayzyas 0:b3376afd10d8 361 sp_wdram = rjs_speed * i2cAcs.sp_wdram_r / 100;
sayzyas 0:b3376afd10d8 362 sp_wcabl = rjs_speed * i2cAcs.sp_wcabl_r / 100;
sayzyas 0:b3376afd10d8 363 sp_wdram *= -1;
sayzyas 0:b3376afd10d8 364 sp_wcabl *= -1;
sayzyas 0:b3376afd10d8 365
sayzyas 0:b3376afd10d8 366 // DEBUG_PRINT_SW("LPC1768> ");
sayzyas 0:b3376afd10d8 367 DEBUG_PRINT_SW("RJSonly: %03d,%03d [%3d] : ", sp_wdram, sp_wcabl, rjs_ud_undata );
sayzyas 0:b3376afd10d8 368 DEBUG_PRINT_SW("Up(CCW)\r\n");
sayzyas 0:b3376afd10d8 369
sayzyas 0:b3376afd10d8 370 if ( flgMtrStp == true ) // true is interrupted from motor control board
sayzyas 0:b3376afd10d8 371 {
sayzyas 0:b3376afd10d8 372 mtrAcs.cmdControl( "XX_BOTH", 7, 0, 0); // Motor off
sayzyas 0:b3376afd10d8 373 wMusic.pi(2);
sayzyas 0:b3376afd10d8 374 mtrAcs.flg_ival_motor_cw = false;
sayzyas 0:b3376afd10d8 375 mtrAcs.flg_ival_motor_ccw = false;
sayzyas 0:b3376afd10d8 376 }
sayzyas 0:b3376afd10d8 377 else
sayzyas 0:b3376afd10d8 378 {
sayzyas 0:b3376afd10d8 379 mtrAcs.cmdControl( "XX_BOTH", 7, sp_wdram*10, sp_wcabl*5); // Motor on
sayzyas 0:b3376afd10d8 380 mtrAcs.flg_ival_motor_ccw = true;
sayzyas 0:b3376afd10d8 381 }
sayzyas 0:b3376afd10d8 382 flg_JSon = true;
sayzyas 0:b3376afd10d8 383 }
sayzyas 0:b3376afd10d8 384 // Down motor forward
sayzyas 0:b3376afd10d8 385 else if( // data : 127 to 0
sayzyas 0:b3376afd10d8 386 ( rjs_ud_undata < (rjs_center_value_ud))
sayzyas 0:b3376afd10d8 387 ){
sayzyas 0:b3376afd10d8 388 if( flg_JSon == false )
sayzyas 0:b3376afd10d8 389 {
sayzyas 0:b3376afd10d8 390 // Attch one time timer interrupt
sayzyas 0:b3376afd10d8 391 // m1 --> timer(+/-) us --> M2
sayzyas 0:b3376afd10d8 392 onetimer.attach_us(&set_motor_interval_flag, abs(mtrAcs.move_interval_cw*1000)); // setup onetimer to call flip after 1 seconds
sayzyas 0:b3376afd10d8 393 }
sayzyas 0:b3376afd10d8 394 rjs_speed = adjust_jsvalue((int16_t)(rjs_center_value_ud - rjs_ud_undata));
sayzyas 0:b3376afd10d8 395 sp_wdram = rjs_speed * i2cAcs.sp_wdram_f / 100;
sayzyas 0:b3376afd10d8 396 sp_wcabl = rjs_speed * i2cAcs.sp_wcabl_f / 100;
sayzyas 0:b3376afd10d8 397
sayzyas 0:b3376afd10d8 398 // DEBUG_PRINT_SW("LPC1768> ");
sayzyas 0:b3376afd10d8 399 DEBUG_PRINT_SW("RJSonly: %03d,%03d [%3d] : ", sp_wdram, sp_wcabl, rjs_ud_undata );
sayzyas 0:b3376afd10d8 400 DEBUG_PRINT_SW("Down(CW)\r\n");
sayzyas 0:b3376afd10d8 401
sayzyas 0:b3376afd10d8 402 if ( flgMtrStp == true ) // true is interrupted from motor control board
sayzyas 0:b3376afd10d8 403 {
sayzyas 0:b3376afd10d8 404 mtrAcs.cmdControl( "XX_BOTH", 7, 0, 0); // Motor off
sayzyas 0:b3376afd10d8 405 wMusic.pi(4);
sayzyas 0:b3376afd10d8 406 mtrAcs.flg_ival_motor_cw = false;
sayzyas 0:b3376afd10d8 407 mtrAcs.flg_ival_motor_ccw = false;
sayzyas 0:b3376afd10d8 408 }
sayzyas 0:b3376afd10d8 409 else
sayzyas 0:b3376afd10d8 410 {
sayzyas 0:b3376afd10d8 411 mtrAcs.cmdControl( "XX_BOTH", 7, sp_wdram*10, sp_wcabl*5); // Motor on
sayzyas 0:b3376afd10d8 412 mtrAcs.flg_ival_motor_cw = true;
sayzyas 0:b3376afd10d8 413 }
sayzyas 0:b3376afd10d8 414 flg_JSon = true;
sayzyas 0:b3376afd10d8 415 }
sayzyas 0:b3376afd10d8 416 }
sayzyas 0:b3376afd10d8 417 else // SW on is manual mode (RJS and LJS independent mode)
sayzyas 0:b3376afd10d8 418 {
sayzyas 0:b3376afd10d8 419 /* R JS : Dram motor control */
sayzyas 0:b3376afd10d8 420 if( // Dead zone
sayzyas 0:b3376afd10d8 421 ( rjs_ud_undata >= (rjs_center_value_ud - DEAD_BAND)) &&
sayzyas 0:b3376afd10d8 422 ( rjs_ud_undata <= (rjs_center_value_ud + DEAD_BAND))
sayzyas 0:b3376afd10d8 423 ){
sayzyas 0:b3376afd10d8 424 rjs_speed = 0;
sayzyas 0:b3376afd10d8 425 sp_wdram = rjs_speed;
sayzyas 0:b3376afd10d8 426 // DEBUG_PRINT_SW("LPC1768> ");
sayzyas 0:b3376afd10d8 427 DEBUG_PRINT_SW("R: %03d [%3d] : ", rjs_speed, rjs_ud_undata );
sayzyas 0:b3376afd10d8 428 DEBUG_PRINT_SW("Dead\t");
sayzyas 0:b3376afd10d8 429 mtrAcs.cmdControl( "XX_DRAM", 7, 0, 0 ); // Motor on
sayzyas 0:b3376afd10d8 430 }
sayzyas 0:b3376afd10d8 431 // Up motor reverse
sayzyas 0:b3376afd10d8 432 else if( // data: 127 to 255
sayzyas 0:b3376afd10d8 433 ( rjs_ud_undata > (rjs_center_value_ud))
sayzyas 0:b3376afd10d8 434 ){
sayzyas 0:b3376afd10d8 435 rjs_speed = adjust_jsvalue((int16_t)(rjs_ud_undata - rjs_center_value_ud));
sayzyas 0:b3376afd10d8 436 sp_wdram = rjs_speed * i2cAcs.sp_wdram_r / 100;
sayzyas 0:b3376afd10d8 437 sp_wdram *= -1;
sayzyas 0:b3376afd10d8 438
sayzyas 0:b3376afd10d8 439 // DEBUG_PRINT_SW("LPC1768> ");
sayzyas 0:b3376afd10d8 440 DEBUG_PRINT_SW("R: %03d [%3d] : ", sp_wdram, rjs_ud_undata );
sayzyas 0:b3376afd10d8 441 DEBUG_PRINT_SW("Up(CCW)\t");
sayzyas 0:b3376afd10d8 442 mtrAcs.cmdControl( "XX_DRAM", 7, sp_wdram*10, 0); // Motor on
sayzyas 0:b3376afd10d8 443 }
sayzyas 0:b3376afd10d8 444 // Down motor forward
sayzyas 0:b3376afd10d8 445 else if( // data : 127 to 0
sayzyas 0:b3376afd10d8 446 ( rjs_ud_undata < (rjs_center_value_ud))
sayzyas 0:b3376afd10d8 447 ){
sayzyas 0:b3376afd10d8 448 rjs_speed = adjust_jsvalue((int16_t)(rjs_center_value_ud - rjs_ud_undata));
sayzyas 0:b3376afd10d8 449 sp_wdram = rjs_speed * i2cAcs.sp_wdram_f / 100;
sayzyas 0:b3376afd10d8 450
sayzyas 0:b3376afd10d8 451 // DEBUG_PRINT_SW("LPC1768> ");
sayzyas 0:b3376afd10d8 452 DEBUG_PRINT_SW("R:%03d [%3d] : ", sp_wdram, rjs_ud_undata );
sayzyas 0:b3376afd10d8 453 DEBUG_PRINT_SW("Down(CW)\t");
sayzyas 0:b3376afd10d8 454 mtrAcs.cmdControl( "XX_DRAM", 7, sp_wdram*10, 0); // Motor on
sayzyas 0:b3376afd10d8 455 }
sayzyas 0:b3376afd10d8 456
sayzyas 0:b3376afd10d8 457 /* L JS : Cable motor control */
sayzyas 0:b3376afd10d8 458 if( // Dead zone
sayzyas 0:b3376afd10d8 459 ( ljs_ud_undata >= (ljs_center_value_ud - DEAD_BAND)) &&
sayzyas 0:b3376afd10d8 460 ( ljs_ud_undata <= (ljs_center_value_ud + DEAD_BAND))
sayzyas 0:b3376afd10d8 461 ){
sayzyas 0:b3376afd10d8 462 ljs_speed = 0;
sayzyas 0:b3376afd10d8 463 sp_wcabl = ljs_speed;
sayzyas 0:b3376afd10d8 464
sayzyas 0:b3376afd10d8 465 // DEBUG_PRINT_SW("LPC1768> ");
sayzyas 0:b3376afd10d8 466 DEBUG_PRINT_SW("L: %03d [%3d] : ", sp_wcabl, ljs_ud_undata );
sayzyas 0:b3376afd10d8 467 DEBUG_PRINT_SW("Dead\r\n");
sayzyas 0:b3376afd10d8 468 mtrAcs.cmdControl( "XX_CABL", 7, 0, 0 ); // Motor on
sayzyas 0:b3376afd10d8 469 }
sayzyas 0:b3376afd10d8 470 // Up motor reverse
sayzyas 0:b3376afd10d8 471 else if( // data: 127 to 255
sayzyas 0:b3376afd10d8 472 ( ljs_ud_undata > (ljs_center_value_ud))
sayzyas 0:b3376afd10d8 473 ){
sayzyas 0:b3376afd10d8 474 ljs_speed = adjust_jsvalue((int16_t)(ljs_ud_undata - ljs_center_value_ud));
sayzyas 0:b3376afd10d8 475 sp_wcabl = ljs_speed * i2cAcs.sp_wcabl_r / 100;
sayzyas 0:b3376afd10d8 476 sp_wcabl *= -1;
sayzyas 0:b3376afd10d8 477
sayzyas 0:b3376afd10d8 478 // DEBUG_PRINT_SW("LPC1768> ");
sayzyas 0:b3376afd10d8 479 DEBUG_PRINT_SW("L:%03d [%3d] : ", sp_wcabl, ljs_ud_undata );
sayzyas 0:b3376afd10d8 480 DEBUG_PRINT_SW("Up(CCW)\r\n");
sayzyas 0:b3376afd10d8 481 mtrAcs.cmdControl( "XX_CABL", 7, 0, sp_wcabl*5); // Motor on
sayzyas 0:b3376afd10d8 482 }
sayzyas 0:b3376afd10d8 483 // Down motor forward
sayzyas 0:b3376afd10d8 484 else if( // data : 127 to 0
sayzyas 0:b3376afd10d8 485 ( ljs_ud_undata < (ljs_center_value_ud))
sayzyas 0:b3376afd10d8 486 ){
sayzyas 0:b3376afd10d8 487 ljs_speed = adjust_jsvalue((int16_t)(ljs_center_value_ud - ljs_ud_undata));
sayzyas 0:b3376afd10d8 488 sp_wcabl = ljs_speed * i2cAcs.sp_wcabl_f / 100;
sayzyas 0:b3376afd10d8 489
sayzyas 0:b3376afd10d8 490 // DEBUG_PRINT_SW("LPC1768> ");
sayzyas 0:b3376afd10d8 491 DEBUG_PRINT_SW("L: %d [%d] : ", sp_wcabl, ljs_ud_undata );
sayzyas 0:b3376afd10d8 492 DEBUG_PRINT_SW("Down(CW)\r\n");
sayzyas 0:b3376afd10d8 493 mtrAcs.cmdControl( "XX_CABL", 7, 0, sp_wcabl*5); // Motor on
sayzyas 0:b3376afd10d8 494 }
sayzyas 0:b3376afd10d8 495 }
sayzyas 0:b3376afd10d8 496
sayzyas 0:b3376afd10d8 497 if(( flg_JSon == false )&&( flgMtrStp == false ))
sayzyas 0:b3376afd10d8 498 {
sayzyas 0:b3376afd10d8 499 I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Winch is stopping (send calculate base data)
sayzyas 0:b3376afd10d8 500 I2C_cmd[I2C_CP_WDRAM_DIA_X100_UPPER] = ( lfsAcs.setValue.wchCtrl.dram_dmtr_x100 >> 8 ) & 0xFF;
sayzyas 0:b3376afd10d8 501 I2C_cmd[I2C_CP_WDRAM_DIA_X100_LOWER] = lfsAcs.setValue.wchCtrl.dram_dmtr_x100 & 0xFF;
sayzyas 0:b3376afd10d8 502 I2C_cmd[I2C_CP_ADJUST_VALUE_X10000_UPPER] = ( lfsAcs.setValue.wchCtrl.adj_val_x10000 >> 8 ) & 0xFF;
sayzyas 0:b3376afd10d8 503 I2C_cmd[I2C_CP_ADJUST_VALUE_X10000_LOWER] = lfsAcs.setValue.wchCtrl.adj_val_x10000 & 0xFF;
sayzyas 0:b3376afd10d8 504 I2C_cmd[I2C_CP_RESOLVER_RESO] = lfsAcs.setValue.wchCtrl.res_resolution & 0xFF;
sayzyas 0:b3376afd10d8 505 if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
sayzyas 0:b3376afd10d8 506 {
sayzyas 0:b3376afd10d8 507 DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
sayzyas 0:b3376afd10d8 508 }
sayzyas 0:b3376afd10d8 509 }
sayzyas 0:b3376afd10d8 510 else // Winch is operating
sayzyas 0:b3376afd10d8 511 {
sayzyas 0:b3376afd10d8 512 I2C_cmd[I2C_CP_COMMAND] = 'N';
sayzyas 0:b3376afd10d8 513 // DEBUG_PRINT_L0("N\r\n");
sayzyas 0:b3376afd10d8 514 if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
sayzyas 0:b3376afd10d8 515 {
sayzyas 0:b3376afd10d8 516 DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
sayzyas 0:b3376afd10d8 517 }
sayzyas 0:b3376afd10d8 518 }
sayzyas 0:b3376afd10d8 519 Thread::wait(20);
sayzyas 0:b3376afd10d8 520 }
sayzyas 0:b3376afd10d8 521 }
sayzyas 0:b3376afd10d8 522 }
sayzyas 0:b3376afd10d8 523 }
sayzyas 0:b3376afd10d8 524
sayzyas 0:b3376afd10d8 525
sayzyas 0:b3376afd10d8 526 /* ====================================================================== */
sayzyas 0:b3376afd10d8 527 /* Main routine */
sayzyas 0:b3376afd10d8 528 /* ====================================================================== */
sayzyas 0:b3376afd10d8 529 int main() {
sayzyas 0:b3376afd10d8 530
sayzyas 0:b3376afd10d8 531 intfrom824.rise(&intRise); // Interrupt handler registration
sayzyas 0:b3376afd10d8 532 intfrom824.fall(&intFall); // Interrupt handler registration
sayzyas 0:b3376afd10d8 533
sayzyas 0:b3376afd10d8 534 wch_opmode.mode(PullUp);
sayzyas 0:b3376afd10d8 535 intfrom824.mode(PullUp);
sayzyas 0:b3376afd10d8 536
sayzyas 0:b3376afd10d8 537 // Serial baudrate
sayzyas 0:b3376afd10d8 538 pc.baud(115200);
sayzyas 0:b3376afd10d8 539
sayzyas 0:b3376afd10d8 540 mtrAccess mtrAcs;
sayzyas 0:b3376afd10d8 541 mtrAcs.setBaudRate(115200);
sayzyas 0:b3376afd10d8 542
sayzyas 0:b3376afd10d8 543 DEBUG_PRINT_L0("\r\n");
sayzyas 0:b3376afd10d8 544 DEBUG_PRINT_L0("LPC1768> +-------------------------------------------------------------\r\n");
sayzyas 0:b3376afd10d8 545 DEBUG_PRINT_L0("LPC1768> | Project: B2Dash Debris Explorer Winch test machine\r\n");
sayzyas 0:b3376afd10d8 546 DEBUG_PRINT_L0("LPC1768> |-------------------------------------------------------------\r\n");
sayzyas 0:b3376afd10d8 547 DEBUG_PRINT_L0("LPC1768> | This is: Main Control Program\r\n");
sayzyas 0:b3376afd10d8 548 DEBUG_PRINT_L0("LPC1768> | Target MCU: mbed LPC1768\r\n");
sayzyas 0:b3376afd10d8 549 DEBUG_PRINT_L0("LPC1768> | Letest update: %s\r\n", LatestUpDate);
sayzyas 0:b3376afd10d8 550 DEBUG_PRINT_L0("LPC1768> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 0:b3376afd10d8 551 DEBUG_PRINT_L0("LPC1768> | Author: %s\r\n", Author);
sayzyas 0:b3376afd10d8 552 DEBUG_PRINT_L0("LPC1768> | Copyright(C) 2017 %s Allright Reserved\r\n", Company);
sayzyas 0:b3376afd10d8 553 DEBUG_PRINT_L0("LPC1768> +-------------------------------------------------------------\r\n");
sayzyas 0:b3376afd10d8 554
sayzyas 0:b3376afd10d8 555 lfsAcs.readSetting();
sayzyas 0:b3376afd10d8 556 DEBUG_PRINT_L0("\r\n");
sayzyas 0:b3376afd10d8 557 DEBUG_PRINT_L0("LPC1768> <Setting from LFS>\r\n");
sayzyas 0:b3376afd10d8 558 DEBUG_PRINT_L0("LPC1768> %04d # 01 [2byte] Cnt th; winch dram Motor ( forward )\r\n", lfsAcs.setValue.wchCtrl.cth_dram_mtr_f);
sayzyas 0:b3376afd10d8 559 DEBUG_PRINT_L0("LPC1768> %04d # 02 [2byte] Cnt th; winch dram Motor ( reverse )\r\n", lfsAcs.setValue.wchCtrl.cth_dram_mtr_r);
sayzyas 0:b3376afd10d8 560 DEBUG_PRINT_L0("LPC1768> %04d # 03 [2byte] Cnt th; winch cabl Motor ( forward )\r\n", lfsAcs.setValue.wchCtrl.cth_cabl_mtr_f);
sayzyas 0:b3376afd10d8 561 DEBUG_PRINT_L0("LPC1768> %04d # 04 [2byte] Cnt th; winch cabl Motor ( reverse )\r\n", lfsAcs.setValue.wchCtrl.cth_cabl_mtr_r);
sayzyas 0:b3376afd10d8 562 DEBUG_PRINT_L0("LPC1768> %05d # 04 [2byte] Winch dram diameter x 100\r\n", lfsAcs.setValue.wchCtrl.dram_dmtr_x100);
sayzyas 0:b3376afd10d8 563 DEBUG_PRINT_L0("LPC1768> %05d # 04 [2byte] Winch adjust value x 10000\r\n", lfsAcs.setValue.wchCtrl.adj_val_x10000);
sayzyas 0:b3376afd10d8 564 DEBUG_PRINT_L0("LPC1768> %03d # 04 [2byte] Winch resolver resolution (bit)\r\n", lfsAcs.setValue.wchCtrl.res_resolution);
sayzyas 0:b3376afd10d8 565
sayzyas 0:b3376afd10d8 566 wMusic.m_poteto(1);
sayzyas 0:b3376afd10d8 567
sayzyas 0:b3376afd10d8 568 DEBUG_PRINT_L0("LPC1768> ------------------\r\n");
sayzyas 0:b3376afd10d8 569 DEBUG_PRINT_L0("LPC1768> Start the task\r\n");
sayzyas 0:b3376afd10d8 570 thread.start(sw_task);
sayzyas 0:b3376afd10d8 571 //Thread thread_swd(sw_task, NULL, osPriorityNormal, 256 * 4); // for OS2
sayzyas 0:b3376afd10d8 572 DEBUG_PRINT_L0("LPC1768> ------------------\r\n"); // for OS5
sayzyas 0:b3376afd10d8 573
sayzyas 0:b3376afd10d8 574 while(1) {
sayzyas 0:b3376afd10d8 575 onbdled_1 = 1;
sayzyas 0:b3376afd10d8 576 Thread::wait(30);
sayzyas 0:b3376afd10d8 577 onbdled_1 = 0;
sayzyas 0:b3376afd10d8 578 onbdled_2 = 1;
sayzyas 0:b3376afd10d8 579 Thread::wait(30);
sayzyas 0:b3376afd10d8 580 onbdled_2 = 0;
sayzyas 0:b3376afd10d8 581 // onbdled_3 = 1;
sayzyas 0:b3376afd10d8 582 // Thread::wait(30);
sayzyas 0:b3376afd10d8 583 // onbdled_3 = 0;
sayzyas 0:b3376afd10d8 584 // onbdled_4 = 1;
sayzyas 0:b3376afd10d8 585 // Thread::wait(30);
sayzyas 0:b3376afd10d8 586 // onbdled_4 = 0;
sayzyas 0:b3376afd10d8 587 Thread::wait(200);
sayzyas 0:b3376afd10d8 588 }
sayzyas 0:b3376afd10d8 589 }