z ysaito / Mbed 2 deprecated CPro_DemoB1B2_0_Can_Class

2018.07.26

Dependencies:   EthernetInterface TextLCD USBDevice USBHost mbed

P1_0Y_main.cpp@1:fdf87a1a724b, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:26:07 2018 +0000
Revision:
1:fdf87a1a724b
Parent:
0:19075177391c 
2018.07.26

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:19075177391c 1 /*
sayzyas 0:19075177391c 2 ////////////////////////////////////////////////////////////////////////////
sayzyas 0:19075177391c 3 Project: B1/B2 DebrisProbe Demonstration model
sayzyas 0:19075177391c 4 Title: Debris Prober Ctrl Main
sayzyas 0:19075177391c 5 Target: mbed LPC1768
sayzyas 0:19075177391c 6 Author: sayzyas as ZNR (Z is not a staff of R)
sayzyas 0:19075177391c 7 -----------------------------------------------
sayzyas 0:19075177391c 8 The Final Project of this company ! ... may be.
sayzyas 0:19075177391c 9 -----------------------------------------------
sayzyas 0:19075177391c 10
sayzyas 0:19075177391c 11 ## ##### ## #####
sayzyas 0:19075177391c 12 ## ## ## ## ## ##
sayzyas 0:19075177391c 13 ## ## ## ##### #####
sayzyas 0:19075177391c 14 ## ## ## ## ## ##
sayzyas 0:19075177391c 15 ## ## ##### #####
sayzyas 0:19075177391c 16
sayzyas 0:19075177391c 17 mbed LPC1768
sayzyas 0:19075177391c 18 +-------------USB-----------+
sayzyas 0:19075177391c 19 | GND VOUT(3.3V) |
sayzyas 0:19075177391c 20 | VIN VU(5.0V OUT)|
sayzyas 0:19075177391c 21 | VB IF- |
sayzyas 0:19075177391c 22 | mR IF+ |
sayzyas 0:19075177391c 23 | p5 mosi Ether RD- |
sayzyas 0:19075177391c 24 | p6 miso Ether RD+ |
sayzyas 0:19075177391c 25 | p7 sck Ether TD- |
sayzyas 0:19075177391c 26 | p8 Ether TD+ |
sayzyas 0:19075177391c 27 | p9 tx sdi USB D- |
sayzyas 0:19075177391c 28 | p10 rx scl USB D+ |
sayzyas 0:19075177391c 29 | p11 mosi CAN rd p30 |
sayzyas 0:19075177391c 30 | p12 miso CAN td p29 |
sayzyas 0:19075177391c 31 | p13 tx sck sda tx p28 |
sayzyas 0:19075177391c 32 | p14 rx scl rx P27 |
sayzyas 0:19075177391c 33 | p15 AIn PWM P26 |
sayzyas 0:19075177391c 34 | p16 AIn PWM P25 |
sayzyas 0:19075177391c 35 | p17 AIn PWM p24 |
sayzyas 0:19075177391c 36 | p18 AIn AOut PWM p23 |
sayzyas 0:19075177391c 37 | p19 AIn PWM p22 |
sayzyas 0:19075177391c 38 | p20 AIn PWM p21 |
sayzyas 0:19075177391c 39 +---------------------------+
sayzyas 0:19075177391c 40
sayzyas 0:19075177391c 41 ////////////////////////////////////////////////////////////////////////////
sayzyas 0:19075177391c 42 */
sayzyas 0:19075177391c 43 #include "mbed.h"
sayzyas 0:19075177391c 44 #include "USBHostGamepad.h"
sayzyas 0:19075177391c 45 #include "USBSerial.h"
sayzyas 0:19075177391c 46 #include "rtos.h"
sayzyas 0:19075177391c 47 #include "EthernetInterface.h"
sayzyas 0:19075177391c 48 #include "common.h"
sayzyas 0:19075177391c 49 #include "stdio.h"
sayzyas 0:19075177391c 50 #include "com_func.h"
sayzyas 0:19075177391c 51 #include "lfsAccess.h"
sayzyas 0:19075177391c 52 #include "i2cAccess.h"
sayzyas 0:19075177391c 53 #include "mtrAccess.h"
sayzyas 0:19075177391c 54 #include "ledCtrl.h"
sayzyas 0:19075177391c 55 #include "music.h"
sayzyas 0:19075177391c 56
sayzyas 0:19075177391c 57 // USBSerial serial setting
sayzyas 0:19075177391c 58 Serial pc(USBTX, USBRX); // UART
sayzyas 0:19075177391c 59 //Serial sdc2130(p28, p27); // Communicate with RpboteQ Driver by tx, rx
sayzyas 0:19075177391c 60
sayzyas 0:19075177391c 61 // Digital I/O setting
sayzyas 0:19075177391c 62 DigitalIn limit_sw_rf(p15); // Limit switch (RF-h) for Transform controller only
sayzyas 0:19075177391c 63 DigitalIn limit_sw_lb(p16); // Limit switch (LB-I) for Transform controller only
sayzyas 0:19075177391c 64
sayzyas 0:19075177391c 65 // Ethernet
sayzyas 0:19075177391c 66 EthernetInterface eth;
sayzyas 0:19075177391c 67 TCPSocketServer tcp_server; // TCP Server
sayzyas 0:19075177391c 68 TCPSocketConnection tcp_client;
sayzyas 0:19075177391c 69 UDPSocket udp_server; // UDP Server
sayzyas 0:19075177391c 70 Endpoint client;
sayzyas 0:19075177391c 71
sayzyas 0:19075177391c 72 // Local File System
sayzyas 0:19075177391c 73 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
sayzyas 0:19075177391c 74
sayzyas 0:19075177391c 75 // JKSoft Blue mbed Board Specific Seting
sayzyas 0:19075177391c 76 //AnalogOut DACout(p18);
sayzyas 0:19075177391c 77 //DigitalOut AMPEnable(p12);
sayzyas 0:19075177391c 78 DigitalIn obd_sw1(p25); // On board physical SW
sayzyas 0:19075177391c 79 DigitalIn obd_sw2(p26); // On board physical SW
sayzyas 0:19075177391c 80
sayzyas 0:19075177391c 81
sayzyas 0:19075177391c 82 // **********************************************************************
sayzyas 0:19075177391c 83 //
sayzyas 0:19075177391c 84 // Main Function of this program
sayzyas 0:19075177391c 85 //
sayzyas 0:19075177391c 86 // **********************************************************************
sayzyas 0:19075177391c 87 int main()
sayzyas 0:19075177391c 88 {
sayzyas 0:19075177391c 89 Mutex file_access_mutex;
sayzyas 0:19075177391c 90 char I2C_cmd[NumberOfI2CCommand];
sayzyas 0:19075177391c 91 int try_cnt;
sayzyas 0:19075177391c 92 int rcv_data_cnt;
sayzyas 0:19075177391c 93
sayzyas 0:19075177391c 94 bool flg_ethernet = false;
sayzyas 0:19075177391c 95
sayzyas 0:19075177391c 96 char ttt[20];
sayzyas 0:19075177391c 97 char tmp_b[6];
sayzyas 0:19075177391c 98 int direction;
sayzyas 0:19075177391c 99 int speed;
sayzyas 0:19075177391c 100 int lswstop;
sayzyas 0:19075177391c 101 char hbuf[128] = ""; // TCP send buffer
sayzyas 0:19075177391c 102 char sbuf[128] = ""; // TCP send buffer
sayzyas 0:19075177391c 103 char rbuf[128]; // TCP receive buffer
sayzyas 0:19075177391c 104 int winchPosition = 0;
sayzyas 0:19075177391c 105
sayzyas 0:19075177391c 106 bool flg_CLRF_motor_stop = false;
sayzyas 0:19075177391c 107 bool flg_CLLB_motor_stop = false;
sayzyas 0:19075177391c 108 bool flg_TFRF_motor_stop = false;
sayzyas 0:19075177391c 109 bool flg_TFLB_motor_stop = false;
sayzyas 0:19075177391c 110 bool flg_WICH_motor_stop = false;
sayzyas 0:19075177391c 111 bool flg_CPAN_motor_stop = false;
sayzyas 0:19075177391c 112 bool flg_CTLT_motor_stop = false;
sayzyas 0:19075177391c 113
sayzyas 0:19075177391c 114 bool flg_mlock_check = false;
sayzyas 0:19075177391c 115
sayzyas 0:19075177391c 116 i2cAccess i2cAcs;
sayzyas 0:19075177391c 117 lfsAccess lfsAcs;
sayzyas 0:19075177391c 118 mtrAccess mtrAcs;
sayzyas 0:19075177391c 119 ledCtrl led;
sayzyas 0:19075177391c 120 waveMusic wMusic;
sayzyas 0:19075177391c 121
sayzyas 0:19075177391c 122 obd_sw1.mode(PullUp);
sayzyas 0:19075177391c 123 obd_sw2.mode(PullUp);
sayzyas 0:19075177391c 124
sayzyas 0:19075177391c 125 // Serial baudrate
sayzyas 0:19075177391c 126 pc.baud(115200);
sayzyas 0:19075177391c 127 mtrAcs.setBaudRate(115200);
sayzyas 0:19075177391c 128
sayzyas 0:19075177391c 129 led.led_demo( 10, 15 );
sayzyas 0:19075177391c 130
sayzyas 0:19075177391c 131 /*
sayzyas 0:19075177391c 132 LED1: System boot OK
sayzyas 0:19075177391c 133 LED2
sayzyas 0:19075177391c 134 LED3: Ethernet access
sayzyas 0:19075177391c 135 LED4: Ethernet OK
sayzyas 0:19075177391c 136 */
sayzyas 0:19075177391c 137
sayzyas 0:19075177391c 138 wMusic.PC9801();
sayzyas 0:19075177391c 139 led.led_off(1);
sayzyas 0:19075177391c 140 led.led_off(2);
sayzyas 0:19075177391c 141 led.led_off(3);
sayzyas 0:19075177391c 142 led.led_off(4);
sayzyas 0:19075177391c 143 led.led_main_off();
sayzyas 0:19075177391c 144
sayzyas 0:19075177391c 145 DEBUG_PRINT_L0("\r\n");
sayzyas 0:19075177391c 146 DEBUG_PRINT_L0("Bd0> /_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_\r\n");
sayzyas 0:19075177391c 147 DEBUG_PRINT_L0("Bd0> | Project: B1/B2 Debris Prover Demonstration machine\r\n");
sayzyas 0:19075177391c 148 DEBUG_PRINT_L0("Bd0> |---------\r\n");
sayzyas 0:19075177391c 149 DEBUG_PRINT_L0("Bd0> | This is: Main Control Program of Main Controller\r\n");
sayzyas 0:19075177391c 150 DEBUG_PRINT_L0("Bd0> | Target MCU: mbed LPC1768\r\n");
sayzyas 0:19075177391c 151 DEBUG_PRINT_L0("Bd0> | Letest update: %s\r\n", LatestUpDate);
sayzyas 0:19075177391c 152 DEBUG_PRINT_L0("Bd0> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 0:19075177391c 153 DEBUG_PRINT_L0("Bd0> | Author: %s\r\n", Author);
sayzyas 0:19075177391c 154 DEBUG_PRINT_L0("Bd0> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
sayzyas 0:19075177391c 155 DEBUG_PRINT_L0("Bd0> /_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_\r\n");
sayzyas 0:19075177391c 156 sprintf( ttt, "%s", ProgramRevision );
sayzyas 0:19075177391c 157
sayzyas 0:19075177391c 158 limit_sw_rf.mode( PullUp ); // use internal pullup
sayzyas 0:19075177391c 159 limit_sw_lb.mode( PullUp ); // use internal pullup
sayzyas 0:19075177391c 160
sayzyas 0:19075177391c 161 DEBUG_PRINT_L0("Bd0> Start ststem initializing ...\r\n");
sayzyas 0:19075177391c 162 DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas 0:19075177391c 163 DEBUG_PRINT_L0("Bd0> 1. Initalizing Ethernet ...\r\n");
sayzyas 0:19075177391c 164 DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas 0:19075177391c 165
sayzyas 0:19075177391c 166 const char* ip_address = "192.168.0.24";
sayzyas 0:19075177391c 167 const char* subnet_mask = "255.255.255.0";
sayzyas 0:19075177391c 168 const char* gateway = "192.168.0.0";
sayzyas 0:19075177391c 169
sayzyas 0:19075177391c 170 DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
sayzyas 0:19075177391c 171 DEBUG_PRINT_L0("Bd0> ip address : %s\r\n", ip_address);
sayzyas 0:19075177391c 172 DEBUG_PRINT_L0("Bd0> subnet mask : %s\r\n", subnet_mask);
sayzyas 0:19075177391c 173 DEBUG_PRINT_L0("Bd0> default gateway: %s\r\n", gateway);
sayzyas 0:19075177391c 174 DEBUG_PRINT_L0("Bd0> --------------------------------------\r\n");
sayzyas 0:19075177391c 175
sayzyas 0:19075177391c 176 if( eth.init( ip_address, subnet_mask, gateway ) == 0 ){
sayzyas 0:19075177391c 177 DEBUG_PRINT_L0("Bd0> Eternet init ... OK\r\n");
sayzyas 0:19075177391c 178 if( eth.connect(7000) == 0 ){
sayzyas 0:19075177391c 179 DEBUG_PRINT_L0("Bd0> Eternat connect ... OK\r\n");
sayzyas 0:19075177391c 180 DEBUG_PRINT_L0("Bd0> [ IP Address : %s ]\r\n", eth.getIPAddress());
sayzyas 0:19075177391c 181 udp_server.bind(UDP_SERVER_PORT);
sayzyas 0:19075177391c 182 tcp_server.bind(TCP_CMDSERVER_PORT);
sayzyas 0:19075177391c 183 tcp_server.listen();
sayzyas 0:19075177391c 184 flg_ethernet = true;
sayzyas 0:19075177391c 185
sayzyas 0:19075177391c 186 led.led_main_blink(1);
sayzyas 0:19075177391c 187 wMusic.pi(1);
sayzyas 0:19075177391c 188
sayzyas 0:19075177391c 189 //---------------------------------------------------
sayzyas 0:19075177391c 190 // Read CrExp setting value from Local File System
sayzyas 0:19075177391c 191 // setting file "SET.DAT".
sayzyas 0:19075177391c 192 // When error occured, LED1 will be blinking shortly.
sayzyas 0:19075177391c 193 //---------------------------------------------------
sayzyas 0:19075177391c 194 DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas 0:19075177391c 195 DEBUG_PRINT_L0("Bd0> 2. Read setting value from LFS\r\n");
sayzyas 0:19075177391c 196 DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas 0:19075177391c 197
sayzyas 0:19075177391c 198 // --------------------------------------------------------------------
sayzyas 0:19075177391c 199 // Read setting from local file system and set to internal structure
sayzyas 0:19075177391c 200 // --------------------------------------------------------------------
sayzyas 0:19075177391c 201 try_cnt = LFS_READ_COUNT;
sayzyas 0:19075177391c 202
sayzyas 0:19075177391c 203 while( 1 ){
sayzyas 0:19075177391c 204 if( lfsAcs.readSetting(1) == true )
sayzyas 0:19075177391c 205 {
sayzyas 0:19075177391c 206 break;
sayzyas 0:19075177391c 207 }
sayzyas 0:19075177391c 208 else
sayzyas 0:19075177391c 209 {
sayzyas 0:19075177391c 210 try_cnt -= 1;
sayzyas 0:19075177391c 211 }
sayzyas 0:19075177391c 212 if( try_cnt == 0 ){
sayzyas 0:19075177391c 213 DEBUG_PRINT_L0("Bd0> ***ERROR*** LFS read error\r\n");
sayzyas 0:19075177391c 214 while(1){
sayzyas 0:19075177391c 215 led.led_error();
sayzyas 0:19075177391c 216 led.led_main_error();
sayzyas 0:19075177391c 217 wMusic.wave( 0.6, 1500.0, 0.3 );
sayzyas 0:19075177391c 218 wMusic.wave( 0.0, 1500.0, 0.1 );
sayzyas 0:19075177391c 219 }
sayzyas 0:19075177391c 220 }
sayzyas 0:19075177391c 221 }
sayzyas 0:19075177391c 222
sayzyas 0:19075177391c 223 DEBUG_PRINT_L2( "%04d # 01-01 [2byte] Crw RF mtr fwd cnt thd\r\n", lfsAcs.setValue.crwCtrl.rf_mtr_ithd_f);
sayzyas 0:19075177391c 224 DEBUG_PRINT_L2( "%04d # 01-02 [2byte] Crw RF mtr rvs cnt thd\r\n", lfsAcs.setValue.crwCtrl.rf_mtr_ithd_r);
sayzyas 0:19075177391c 225 DEBUG_PRINT_L2( "%04d # 01-03 [2byte] Crw LB mtr fwd cnt thd\r\n", lfsAcs.setValue.crwCtrl.lb_mtr_ithd_f);
sayzyas 0:19075177391c 226 DEBUG_PRINT_L2( "%04d # 01-04 [2byte] Crw LB mtr rvs cnt thd\r\n", lfsAcs.setValue.crwCtrl.lb_mtr_ithd_r);
sayzyas 0:19075177391c 227 DEBUG_PRINT_L2( "%03d # 01-05 [1byte] Crw RF mtr fwd speed\r\n", lfsAcs.setValue.crwCtrl.rf_mtr_hspd_f);
sayzyas 0:19075177391c 228 DEBUG_PRINT_L2( "%03d # 01-06 [1byte] Crw RF mtr rvs speed\r\n", lfsAcs.setValue.crwCtrl.rf_mtr_hspd_r);
sayzyas 0:19075177391c 229 DEBUG_PRINT_L2( "%03d # 01-07 [1byte] Crw LB mtr fwd speed\r\n", lfsAcs.setValue.crwCtrl.lb_mtr_hspd_f);
sayzyas 0:19075177391c 230 DEBUG_PRINT_L2( "%03d # 01-08 [1byte] Crw LB mtr rvs speed\r\n", lfsAcs.setValue.crwCtrl.lb_mtr_hspd_r);
sayzyas 0:19075177391c 231 DEBUG_PRINT_L2( "%03d # 01-09 [1byte] reserved 1\r\n", lfsAcs.setValue.crwCtrl.reserved_1);
sayzyas 0:19075177391c 232 DEBUG_PRINT_L2( "%03d # 01-10 [1byte] reserved 2\r\n", lfsAcs.setValue.crwCtrl.reserved_2);
sayzyas 0:19075177391c 233 DEBUG_PRINT_L2( "%03d # 01-11 [1byte] reserved 3\r\n", lfsAcs.setValue.crwCtrl.reserved_3);
sayzyas 0:19075177391c 234 DEBUG_PRINT_L2( "%03d # 01-12 [1byte] reserved 4\r\n", lfsAcs.setValue.crwCtrl.reserved_4);
sayzyas 0:19075177391c 235
sayzyas 0:19075177391c 236 DEBUG_PRINT_L2( "%04d # 02-01 [2byte] Tfm RF mtr fwd cnt thd\r\n", lfsAcs.setValue.tfmCtrl.rf_mtr_ithd_f);
sayzyas 0:19075177391c 237 DEBUG_PRINT_L2( "%04d # 02-02 [2byte] Tfm RF mtr rvs cnt thd\r\n", lfsAcs.setValue.tfmCtrl.rf_mtr_ithd_r);
sayzyas 0:19075177391c 238 DEBUG_PRINT_L2( "%04d # 02-03 [2byte] Tfm LB mtr fwd cnt thd\r\n", lfsAcs.setValue.tfmCtrl.lb_mtr_ithd_f);
sayzyas 0:19075177391c 239 DEBUG_PRINT_L2( "%04d # 02-04 [2byte] Tfm LB mtr rvs cnt thd\r\n", lfsAcs.setValue.tfmCtrl.lb_mtr_ithd_r);
sayzyas 0:19075177391c 240 DEBUG_PRINT_L2( "%03d # 02-05 [1byte] Tfm RF mtr fwd speed\r\n", lfsAcs.setValue.tfmCtrl.rf_mtr_hspd_f);
sayzyas 0:19075177391c 241 DEBUG_PRINT_L2( "%03d # 02-06 [1byte] Tfm RF mtr rvs speed\r\n", lfsAcs.setValue.tfmCtrl.rf_mtr_hspd_r);
sayzyas 0:19075177391c 242 DEBUG_PRINT_L2( "%03d # 02-07 [1byte] Tfm LB mtr fwd speed\r\n", lfsAcs.setValue.tfmCtrl.lb_mtr_hspd_f);
sayzyas 0:19075177391c 243 DEBUG_PRINT_L2( "%03d # 02-08 [1byte] Tfm LB mtr rvs speed\r\n", lfsAcs.setValue.tfmCtrl.lb_mtr_hspd_r);
sayzyas 0:19075177391c 244 DEBUG_PRINT_L2( "%03d # 02-09 [1byte] reserved 1\r\n", lfsAcs.setValue.tfmCtrl.reserved_1);
sayzyas 0:19075177391c 245 DEBUG_PRINT_L2( "%03d # 02-10 [1byte] reserved 2\r\n", lfsAcs.setValue.tfmCtrl.reserved_2);
sayzyas 0:19075177391c 246 DEBUG_PRINT_L2( "%03d # 02-11 [1byte] reserved 3\r\n", lfsAcs.setValue.tfmCtrl.reserved_3);
sayzyas 0:19075177391c 247 DEBUG_PRINT_L2( "%03d # 02-12 [1byte] reserved 4\r\n", lfsAcs.setValue.tfmCtrl.reserved_4);
sayzyas 0:19075177391c 248
sayzyas 0:19075177391c 249 DEBUG_PRINT_L2( "%04d # 03-01 [2byte] Pan mtr fwd cnt thd\r\n", lfsAcs.setValue.ptlCtrl.pan_mtr_ithd_f);
sayzyas 0:19075177391c 250 DEBUG_PRINT_L2( "%04d # 03-02 [2byte] Pan mtr rvs cnt thd\r\n", lfsAcs.setValue.ptlCtrl.pan_mtr_ithd_r);
sayzyas 0:19075177391c 251 DEBUG_PRINT_L2( "%04d # 03-03 [2byte] Tlt mtr fwd cnt thd\r\n", lfsAcs.setValue.ptlCtrl.tlt_mtr_ithd_f);
sayzyas 0:19075177391c 252 DEBUG_PRINT_L2( "%04d # 03-04 [2byte] Tlt mtr rvs cnt thd\r\n", lfsAcs.setValue.ptlCtrl.tlt_mtr_ithd_r);
sayzyas 0:19075177391c 253 DEBUG_PRINT_L2( "%03d # 03-05 [1byte] Pan mtr fwd h-speed\r\n", lfsAcs.setValue.ptlCtrl.pan_mtr_hspd_f);
sayzyas 0:19075177391c 254 DEBUG_PRINT_L2( "%03d # 03-06 [1byte] Pan mtr rvs h-speed\r\n", lfsAcs.setValue.ptlCtrl.pan_mtr_hspd_r);
sayzyas 0:19075177391c 255 DEBUG_PRINT_L2( "%03d # 03-07 [1byte] Tlt mtr fwd h-speed\r\n", lfsAcs.setValue.ptlCtrl.tlt_mtr_hspd_f);
sayzyas 0:19075177391c 256 DEBUG_PRINT_L2( "%03d # 03-08 [1byte] Tlt mtr rvs h-speed\r\n", lfsAcs.setValue.ptlCtrl.tlt_mtr_hspd_r);
sayzyas 0:19075177391c 257 DEBUG_PRINT_L2( "%03d # 03-09 [1byte] reserved 1\r\n", lfsAcs.setValue.ptlCtrl.reserved_1);
sayzyas 0:19075177391c 258 DEBUG_PRINT_L2( "%03d # 03-10 [1byte] reserved 2\r\n", lfsAcs.setValue.ptlCtrl.reserved_2);
sayzyas 0:19075177391c 259 DEBUG_PRINT_L2( "%03d # 03-11 [1byte] reserved 3\r\n", lfsAcs.setValue.ptlCtrl.reserved_3);
sayzyas 0:19075177391c 260 DEBUG_PRINT_L2( "%03d # 03-12 [1byte] reserved 3\r\n", lfsAcs.setValue.ptlCtrl.reserved_4);
sayzyas 0:19075177391c 261
sayzyas 0:19075177391c 262 DEBUG_PRINT_L2( "%04d # 01 [2byte] Wch drm mtr fwd cnt thd\r\n", lfsAcs.setValue.wchCtrl.drm_mtr_ithd_f);
sayzyas 0:19075177391c 263 DEBUG_PRINT_L2( "%04d # 02 [2byte] Wch drm mtr rvs cnt thd\r\n", lfsAcs.setValue.wchCtrl.drm_mtr_ithd_r);
sayzyas 0:19075177391c 264 DEBUG_PRINT_L2( "%04d # 03 [2byte] Wch No2 mtr fwd cnt thd\r\n", lfsAcs.setValue.wchCtrl.no2_mtr_ithd_f);
sayzyas 0:19075177391c 265 DEBUG_PRINT_L2( "%04d # 04 [2byte] Wch No2 mtr rvs cnt thd\r\n", lfsAcs.setValue.wchCtrl.no2_mtr_ithd_r);
sayzyas 0:19075177391c 266 DEBUG_PRINT_L2( "%03d # 05 [1byte] Wch drm mtr fwd h-speed\r\n", lfsAcs.setValue.wchCtrl.drm_mtr_hspd_f);
sayzyas 0:19075177391c 267 DEBUG_PRINT_L2( "%03d # 06 [1byte] Wch drm mtr rvs h-speed\r\n", lfsAcs.setValue.wchCtrl.drm_mtr_hspd_r);
sayzyas 0:19075177391c 268 DEBUG_PRINT_L2( "%03d # 09 [1byte] Wch No2 mtr fwd h-speed\r\n", lfsAcs.setValue.wchCtrl.no2_mtr_hspd_f);
sayzyas 0:19075177391c 269 DEBUG_PRINT_L2( "%03d # 10 [1byte] Wch No2 mtr rvs h-speed\r\n", lfsAcs.setValue.wchCtrl.no2_mtr_hspd_r);
sayzyas 0:19075177391c 270 DEBUG_PRINT_L2( "%05d # 13 [2byte] Wch dram diameter x100\r\n", lfsAcs.setValue.wchCtrl.dram_dmtr_x100);
sayzyas 0:19075177391c 271 DEBUG_PRINT_L2( "%05d # 14 [2byte] Wch adjust value x10000\r\n", lfsAcs.setValue.wchCtrl.adj_val_x10000);
sayzyas 0:19075177391c 272 DEBUG_PRINT_L2( "%03d # 15 [1byte] Wch resolver resolution\r\n", lfsAcs.setValue.wchCtrl.res_resolution);
sayzyas 0:19075177391c 273 DEBUG_PRINT_L2( "%03d # 16 [1byte] reserved 1\r\n", lfsAcs.setValue.wchCtrl.reserved_1);
sayzyas 0:19075177391c 274 DEBUG_PRINT_L2( "%03d # 17 [1byte] reserved 2\r\n", lfsAcs.setValue.wchCtrl.reserved_2);
sayzyas 0:19075177391c 275 DEBUG_PRINT_L2( "%03d # 18 [1byte] reserved 3\r\n", lfsAcs.setValue.wchCtrl.reserved_3);
sayzyas 0:19075177391c 276 DEBUG_PRINT_L2( "%03d # 18 [1byte] reserved 3\r\n", lfsAcs.setValue.wchCtrl.reserved_4);
sayzyas 0:19075177391c 277 DEBUG_PRINT_L2( "%03d # 18 [1byte] reserved 3\r\n", lfsAcs.setValue.wchCtrl.reserved_5);
sayzyas 0:19075177391c 278 DEBUG_PRINT_L2( "%03d # 18 [1byte] reserved 3\r\n", lfsAcs.setValue.wchCtrl.reserved_6);
sayzyas 0:19075177391c 279 DEBUG_PRINT_L2( "%03d # 18 [1byte] reserved 3\r\n", lfsAcs.setValue.wchCtrl.reserved_7);
sayzyas 0:19075177391c 280
sayzyas 0:19075177391c 281
sayzyas 0:19075177391c 282
sayzyas 0:19075177391c 283 DEBUG_PRINT_L0("Bd0> LFS read OK\r\n");
sayzyas 0:19075177391c 284 led.led_main_blink(2);
sayzyas 0:19075177391c 285 wMusic.pi(2);
sayzyas 0:19075177391c 286
sayzyas 0:19075177391c 287 // **************************************************
sayzyas 0:19075177391c 288 // Send Calculation Data to Resolver Controller
sayzyas 0:19075177391c 289 // **************************************************
sayzyas 0:19075177391c 290 DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas 0:19075177391c 291 DEBUG_PRINT_L0("Bd0> 3. Send the Calculation base data to Resolver Controller\r\n");
sayzyas 0:19075177391c 292 DEBUG_PRINT_L0("Bd0> -----------------------------------------------------------\r\n");
sayzyas 0:19075177391c 293 DEBUG_PRINT_L0("Bd0> Dram diameter(x100) : %d\r\n", lfsAcs.setValue.wchCtrl.dram_dmtr_x100);
sayzyas 0:19075177391c 294 DEBUG_PRINT_L0("Bd0> Adjust value(x10000): %d\r\n", lfsAcs.setValue.wchCtrl.adj_val_x10000);
sayzyas 0:19075177391c 295 DEBUG_PRINT_L0("Bd0> Resolver resolution): %d\r\n", lfsAcs.setValue.wchCtrl.res_resolution);
sayzyas 0:19075177391c 296 DEBUG_PRINT_L0("Bd0> =============================================================\r\n");
sayzyas 0:19075177391c 297 I2C_cmd[I2C_CP_COMMAND] = 'R'; // 01: Preset resolver base data
sayzyas 0:19075177391c 298 I2C_cmd[I2C_CP_WDRAM_DIA_UPPER] = (uint8_t)((lfsAcs.setValue.wchCtrl.dram_dmtr_x100>>8)&0xFF); // Dram diameter upper
sayzyas 0:19075177391c 299 I2C_cmd[I2C_CP_WDRAM_DIA_LOWER] = (uint8_t)((lfsAcs.setValue.wchCtrl.dram_dmtr_x100)&0xFF); // Dram diameter lower
sayzyas 0:19075177391c 300 I2C_cmd[I2C_CP_CCABLE_DIA_UPPER] = (uint8_t)((lfsAcs.setValue.wchCtrl.adj_val_x10000>>8)&0xFF); // Cable diameter upper
sayzyas 0:19075177391c 301 I2C_cmd[I2C_CP_CCABLE_DIA_LOWER] = (uint8_t)((lfsAcs.setValue.wchCtrl.adj_val_x10000)&0xFF); // Cable diameter lower
sayzyas 0:19075177391c 302 I2C_cmd[I2C_CP_RESOLVER_RESO] = lfsAcs.setValue.wchCtrl.res_resolution; // Resolver resolution
sayzyas 0:19075177391c 303 // Send base calculation data to Resolver controller
sayzyas 0:19075177391c 304 i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand);
sayzyas 0:19075177391c 305 Thread::wait(500);
sayzyas 0:19075177391c 306
sayzyas 0:19075177391c 307 // Set all motor current thresold
sayzyas 0:19075177391c 308 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_RFCRW, MOTOR_DIR_FWD, lfsAcs.setValue.crwCtrl.rf_mtr_ithd_f );
sayzyas 0:19075177391c 309 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_RFCRW, MOTOR_DIR_RVS, lfsAcs.setValue.crwCtrl.rf_mtr_ithd_r );
sayzyas 0:19075177391c 310 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_LBCRW, MOTOR_DIR_FWD, lfsAcs.setValue.crwCtrl.lb_mtr_ithd_f );
sayzyas 0:19075177391c 311 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_LBCRW, MOTOR_DIR_RVS, lfsAcs.setValue.crwCtrl.lb_mtr_ithd_r );
sayzyas 0:19075177391c 312
sayzyas 0:19075177391c 313 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_RFTFM, MOTOR_DIR_FWD, lfsAcs.setValue.tfmCtrl.rf_mtr_ithd_f );
sayzyas 0:19075177391c 314 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_RFTFM, MOTOR_DIR_RVS, lfsAcs.setValue.tfmCtrl.rf_mtr_ithd_r );
sayzyas 0:19075177391c 315 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_LBTFM, MOTOR_DIR_FWD, lfsAcs.setValue.tfmCtrl.lb_mtr_ithd_f );
sayzyas 0:19075177391c 316 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_LBTFM, MOTOR_DIR_RVS, lfsAcs.setValue.tfmCtrl.lb_mtr_ithd_r );
sayzyas 0:19075177391c 317
sayzyas 0:19075177391c 318 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_CMPAN, MOTOR_DIR_FWD, lfsAcs.setValue.ptlCtrl.pan_mtr_ithd_f );
sayzyas 0:19075177391c 319 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_CMPAN, MOTOR_DIR_RVS, lfsAcs.setValue.ptlCtrl.pan_mtr_ithd_r );
sayzyas 0:19075177391c 320 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_CTILT, MOTOR_DIR_FWD, lfsAcs.setValue.ptlCtrl.tlt_mtr_ithd_f );
sayzyas 0:19075177391c 321 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_CTILT, MOTOR_DIR_RVS, lfsAcs.setValue.ptlCtrl.tlt_mtr_ithd_r );
sayzyas 0:19075177391c 322
sayzyas 0:19075177391c 323 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_WINCH, MOTOR_DIR_FWD, lfsAcs.setValue.wchCtrl.drm_mtr_ithd_f );
sayzyas 0:19075177391c 324 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_WINCH, MOTOR_DIR_RVS, lfsAcs.setValue.wchCtrl.drm_mtr_ithd_r );
sayzyas 0:19075177391c 325 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_WINCH, MOTOR_DIR_FWD, lfsAcs.setValue.wchCtrl.no2_mtr_ithd_f );
sayzyas 0:19075177391c 326 i2cAcs.i2cSetMotorThreshold( I2C_ADDRESS_RESOLVER, MOTOR_WINCH, MOTOR_DIR_RVS, lfsAcs.setValue.wchCtrl.no2_mtr_ithd_r );
sayzyas 0:19075177391c 327
sayzyas 0:19075177391c 328 led.led_main_blink(3);
sayzyas 0:19075177391c 329 wMusic.pi(3);
sayzyas 0:19075177391c 330
sayzyas 0:19075177391c 331 DEBUG_PRINT_L0( "Bd0> -----------------------------------\r\n");
sayzyas 0:19075177391c 332 DEBUG_PRINT_L0( "Bd0> >>>> Initializing completed !! <<<<\r\n");
sayzyas 0:19075177391c 333 DEBUG_PRINT_L0( "Bd0> -----------------------------------\r\n");
sayzyas 0:19075177391c 334
sayzyas 0:19075177391c 335 led.led_on(1); // Initializing is OK then Power Indicator LED ON
sayzyas 0:19075177391c 336 led.led_off(4); //
sayzyas 0:19075177391c 337
sayzyas 0:19075177391c 338 wMusic.knkk(); // System OK, then music start ( Kono-ki nannoki kininaruki~~ )
sayzyas 0:19075177391c 339
sayzyas 0:19075177391c 340 // -----------------------------------------------------------------
sayzyas 0:19075177391c 341 // Communicate with client PC program.
sayzyas 0:19075177391c 342 // TCP connection:
sayzyas 0:19075177391c 343 // -----------------------------------------------------------------
sayzyas 0:19075177391c 344 while( true )
sayzyas 0:19075177391c 345 {
sayzyas 0:19075177391c 346 if( flg_ethernet == true ) // in case of Ethernet OK
sayzyas 0:19075177391c 347 {
sayzyas 0:19075177391c 348 Thread::wait(3);
sayzyas 0:19075177391c 349 tcp_server.accept(tcp_client);
sayzyas 0:19075177391c 350 // tcp_client.set_blocking(false, 10000); // Timeout after (10000) msec
sayzyas 0:19075177391c 351 tcp_client.set_blocking(false, 5000); // Timeout after (5000) msec
sayzyas 0:19075177391c 352 DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
sayzyas 0:19075177391c 353 DEBUG_PRINT_L3("Bd0> TCP Connection from: %s\r\n", tcp_client.get_address());
sayzyas 0:19075177391c 354 DEBUG_PRINT_L3("Bd0> ----------------------------\r\n");
sayzyas 0:19075177391c 355 led.led_on(2);
sayzyas 0:19075177391c 356 led.led_main_blink(4);
sayzyas 0:19075177391c 357 wMusic.pi(4);
sayzyas 0:19075177391c 358
sayzyas 0:19075177391c 359 // vol, freq, time
sayzyas 0:19075177391c 360 wMusic.wave( 0.2, 440.0*6, 0.2 ); // 440Hz 0.3s
sayzyas 0:19075177391c 361 wMusic.wave( 0.0, 440.0, 0.1 ); // No sound
sayzyas 0:19075177391c 362 wMusic.wave( 0.2, 440.0*6, 0.2 ); // 440Hz 0.3s
sayzyas 0:19075177391c 363 wMusic.wave( 0.0, 440.0, 0.1 ); // No sound
sayzyas 0:19075177391c 364 wMusic.wave( 0.2, 440.0*6, 0.2 ); // 440Hz 0.3s
sayzyas 0:19075177391c 365 wMusic.wave( 0.0, 440.0, 0.1 ); // No sound
sayzyas 0:19075177391c 366
sayzyas 0:19075177391c 367 while(true){
sayzyas 0:19075177391c 368 led.led_main_on();
sayzyas 0:19075177391c 369 // --------------------------------------------------------------
sayzyas 0:19075177391c 370 // Following instructions are blocking when no ethernet access
sayzyas 0:19075177391c 371 // --------------------------------------------------------------
sayzyas 0:19075177391c 372 rcv_data_cnt = tcp_client.receive(rbuf, sizeof(rbuf));
sayzyas 0:19075177391c 373 if( rcv_data_cnt <= 0 ){
sayzyas 0:19075177391c 374 DEBUG_PRINT_L3("Bd0> ###!!!### : TCP Receive packet fail\r\n");
sayzyas 0:19075177391c 375 Thread::wait(3);
sayzyas 0:19075177391c 376 // vol, freq, time
sayzyas 0:19075177391c 377 wMusic.wave( 0.2, 440.0, 0.3 ); // 440Hz 0.3s
sayzyas 0:19075177391c 378 wMusic.wave( 0.0, 440.0, 0.3 ); // No sound
sayzyas 0:19075177391c 379 wMusic.wave( 0.2, 440.0, 0.3 ); // 440Hz 0.3s
sayzyas 0:19075177391c 380 wMusic.wave( 0.0, 440.0, 0.3 ); // No sound
sayzyas 0:19075177391c 381 wMusic.wave( 0.2, 440.0, 0.3 ); // 440Hz 0.3s
sayzyas 0:19075177391c 382 wMusic.wave( 0.0, 440.0, 0.3 ); // No sound
sayzyas 0:19075177391c 383 wMusic.wave( 0.2, 440.0, 0.3 ); // 440Hz 0.3s
sayzyas 0:19075177391c 384 wMusic.wave( 0.0, 440.0, 0.3 ); // No sound
sayzyas 0:19075177391c 385 wMusic.wave( 0.2, 440.0, 0.3 ); // 440Hz 0.3s
sayzyas 0:19075177391c 386 wMusic.wave( 0.0, 440.0, 0.3 ); // No sound
sayzyas 0:19075177391c 387 break;
sayzyas 0:19075177391c 388 }
sayzyas 0:19075177391c 389 else{
sayzyas 0:19075177391c 390 // Transform, Crawler part valid
sayzyas 0:19075177391c 391 if( !strncmp( rbuf, "Hello", 5 ) )
sayzyas 0:19075177391c 392 {
sayzyas 0:19075177391c 393 led.led_on(3);
sayzyas 0:19075177391c 394 // Send the information of platform
sayzyas 0:19075177391c 395 // ==> B1: B1Demo, B2:B2Demo
sayzyas 0:19075177391c 396 sprintf( hbuf,PLATFORM );
sayzyas 0:19075177391c 397 hbuf[6] = '\0';
sayzyas 0:19075177391c 398 tcp_client.send_all(hbuf, sizeof(sbuf));
sayzyas 0:19075177391c 399 led.led_off(3);
sayzyas 0:19075177391c 400 }
sayzyas 0:19075177391c 401 else if( !strncmp( rbuf, "OO", 2 ) )
sayzyas 0:19075177391c 402 {
sayzyas 0:19075177391c 403 led.led_on(3);
sayzyas 0:19075177391c 404 /* When change valid part from tfm/crm to winch, then send calculation base data to resolver controller */
sayzyas 0:19075177391c 405 DEBUG_PRINT_L0("Bd0> Send winch position clear command to Resolver Controller\r\n");
sayzyas 0:19075177391c 406 I2C_cmd[I2C_CP_COMMAND] = 'Z'; // Encorder reset
sayzyas 0:19075177391c 407 // Send base calculation data to Resolver controller
sayzyas 0:19075177391c 408 if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
sayzyas 0:19075177391c 409 {
sayzyas 0:19075177391c 410 DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
sayzyas 0:19075177391c 411 }
sayzyas 0:19075177391c 412 led.led_off(3);
sayzyas 0:19075177391c 413 }
sayzyas 0:19075177391c 414 else if( !strncmp( rbuf, "OX", 2 ))
sayzyas 0:19075177391c 415 {
sayzyas 0:19075177391c 416 led.led_on(3);
sayzyas 0:19075177391c 417 I2C_cmd[I2C_CP_COMMAND] = 0x43; // Set motor current base value
sayzyas 0:19075177391c 418 // Send base calculation data to Resolver controller
sayzyas 0:19075177391c 419 if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
sayzyas 0:19075177391c 420 {
sayzyas 0:19075177391c 421 DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
sayzyas 0:19075177391c 422 }
sayzyas 0:19075177391c 423 led.led_off(3);
sayzyas 0:19075177391c 424 }
sayzyas 0:19075177391c 425 // Pan/Tilt, winch part valid
sayzyas 0:19075177391c 426 else if( !strncmp( rbuf, "XO", 2 ))
sayzyas 0:19075177391c 427 {
sayzyas 0:19075177391c 428 led.led_on(3);
sayzyas 0:19075177391c 429 I2C_cmd[I2C_CP_COMMAND] = 0x43; // Set motor current base value
sayzyas 0:19075177391c 430 // Send base calculation data to Resolver controller
sayzyas 0:19075177391c 431 if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
sayzyas 0:19075177391c 432 {
sayzyas 0:19075177391c 433 DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
sayzyas 0:19075177391c 434 }
sayzyas 0:19075177391c 435 Thread::wait(500);
sayzyas 0:19075177391c 436 /* When change valid part from tfm/crm to winch, then send calculation base data to resolver controller */
sayzyas 0:19075177391c 437 DEBUG_PRINT_L0("Bd0> Send winch position calculation base data to Resolver Controller\r\n");
sayzyas 0:19075177391c 438 I2C_cmd[I2C_CP_COMMAND] = 'R'; // 01: Preset resolver base data
sayzyas 0:19075177391c 439 I2C_cmd[I2C_CP_WDRAM_DIA_UPPER] = (uint8_t)((lfsAcs.setValue.wchCtrl.dram_dmtr_x100>>8)&0xFF); // Dram diameter upper
sayzyas 0:19075177391c 440 I2C_cmd[I2C_CP_WDRAM_DIA_LOWER] = (uint8_t)((lfsAcs.setValue.wchCtrl.dram_dmtr_x100)&0xFF); // Dram diameter lower
sayzyas 0:19075177391c 441 I2C_cmd[I2C_CP_CCABLE_DIA_UPPER] = (uint8_t)((lfsAcs.setValue.wchCtrl.adj_val_x10000>>8)&0xFF); // Cable diameter upper
sayzyas 0:19075177391c 442 I2C_cmd[I2C_CP_CCABLE_DIA_LOWER] = (uint8_t)((lfsAcs.setValue.wchCtrl.adj_val_x10000)&0xFF); // Cable diameter lower
sayzyas 0:19075177391c 443 I2C_cmd[I2C_CP_RESOLVER_RESO] = lfsAcs.setValue.wchCtrl.res_resolution; // Resolver resolution
sayzyas 0:19075177391c 444
sayzyas 0:19075177391c 445 // Send base calculation data to Resolver controller
sayzyas 0:19075177391c 446 if( i2cAcs.i2c_write(I2C_ADDRESS_RESOLVER, I2C_cmd, NumberOfI2CCommand) == false )
sayzyas 0:19075177391c 447 {
sayzyas 0:19075177391c 448 DEBUG_PRINT_L0("#### i2c write ERROR ####\r\n");
sayzyas 0:19075177391c 449 }
sayzyas 0:19075177391c 450 led.led_off(3);
sayzyas 0:19075177391c 451 }
sayzyas 0:19075177391c 452 else if( !strncmp( rbuf, "XX", 2 ))
sayzyas 0:19075177391c 453 {
sayzyas 0:19075177391c 454 strncpy( tmp_b, rbuf+9, 2); // direction
sayzyas 0:19075177391c 455 tmp_b[2]='\0';
sayzyas 0:19075177391c 456 direction = atoi(tmp_b);
sayzyas 0:19075177391c 457 strncpy( tmp_b, rbuf+12, 4); // speed
sayzyas 0:19075177391c 458 tmp_b[4]='\0';
sayzyas 0:19075177391c 459 speed = atoi(tmp_b);
sayzyas 0:19075177391c 460 strncpy( tmp_b, rbuf+8, 1); // speed
sayzyas 0:19075177391c 461 tmp_b[1]='\0';
sayzyas 0:19075177391c 462 lswstop = atoi(tmp_b);
sayzyas 0:19075177391c 463 //------------------------------------
sayzyas 0:19075177391c 464 // Cmd: "WICH" : B2DP Winch cintrol
sayzyas 0:19075177391c 465 //------------------------------------
sayzyas 0:19075177391c 466 /* if( direction == 2 )
sayzyas 0:19075177391c 467 {
sayzyas 0:19075177391c 468 if( flg_mlock_check == false )
sayzyas 0:19075177391c 469 {
sayzyas 0:19075177391c 470 i2cAcs.i2cClearMLCnt(I2C_ADDRESS_RESOLVER);
sayzyas 0:19075177391c 471 flg_mlock_check = true;
sayzyas 0:19075177391c 472 }
sayzyas 0:19075177391c 473 }*/
sayzyas 0:19075177391c 474 if( !strncmp( rbuf+3, "WICH", 4 ))
sayzyas 0:19075177391c 475 {
sayzyas 0:19075177391c 476 // Winch
sayzyas 0:19075177391c 477 if( direction == 0 )
sayzyas 0:19075177391c 478 {
sayzyas 0:19075177391c 479 led.led_on(3);
sayzyas 0:19075177391c 480 speed = ( speed * lfsAcs.setValue.wchCtrl.drm_mtr_hspd_f ) / 10;
sayzyas 0:19075177391c 481 }
sayzyas 0:19075177391c 482 else if ( direction == 1 )
sayzyas 0:19075177391c 483 {
sayzyas 0:19075177391c 484 led.led_on(3);
sayzyas 0:19075177391c 485 speed = ( speed * lfsAcs.setValue.wchCtrl.drm_mtr_hspd_r ) / 10 * -1;
sayzyas 0:19075177391c 486 }
sayzyas 0:19075177391c 487 else // direction == 2 is JS off
sayzyas 0:19075177391c 488 {
sayzyas 0:19075177391c 489 flg_WICH_motor_stop = false; // flag release when JS off
sayzyas 0:19075177391c 490 speed = 0;
sayzyas 0:19075177391c 491 }
sayzyas 0:19075177391c 492 if( flg_WICH_motor_stop == false )
sayzyas 0:19075177391c 493 {
sayzyas 0:19075177391c 494 DEBUG_PRINT_L3("Bd0> XX_WICH [ dir: %d, speed: %d]", direction,speed/10);
sayzyas 0:19075177391c 495 mtrAcs.cmdControl( "XX_WICH", 7, speed ); // Motor on
sayzyas 0:19075177391c 496 }
sayzyas 0:19075177391c 497 // Get motor current here from LPC824-max
sayzyas 0:19075177391c 498 if( direction == 0 )
sayzyas 0:19075177391c 499 {
sayzyas 0:19075177391c 500 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_WINCH, MOTOR_DIR_FWD);
sayzyas 0:19075177391c 501 }
sayzyas 0:19075177391c 502 else
sayzyas 0:19075177391c 503 {
sayzyas 0:19075177391c 504 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_WINCH, MOTOR_DIR_RVS);
sayzyas 0:19075177391c 505 }
sayzyas 0:19075177391c 506 // Get information and send to client
sayzyas 0:19075177391c 507 winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
sayzyas 0:19075177391c 508 DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
sayzyas 0:19075177391c 509 if( i2cAcs.flg_motor_lock == true )
sayzyas 0:19075177391c 510 {
sayzyas 0:19075177391c 511 DEBUG_PRINT_L3("Bd0> ### WINCH LOCK !!! ###\r\n");
sayzyas 0:19075177391c 512 mtrAcs.cmdControl( "XX_WICH", 7, 0 );
sayzyas 0:19075177391c 513 flg_WICH_motor_stop = true;
sayzyas 0:19075177391c 514 winchPosition = 9999; // motor lock
sayzyas 0:19075177391c 515 }
sayzyas 0:19075177391c 516 sprintf( sbuf, "%04d", winchPosition );
sayzyas 0:19075177391c 517 sbuf[4] = '\0';
sayzyas 0:19075177391c 518 tcp_client.send_all(sbuf, sizeof(sbuf));
sayzyas 0:19075177391c 519 led.led_off(3);
sayzyas 0:19075177391c 520 }
sayzyas 0:19075177391c 521 //------------------------------------
sayzyas 0:19075177391c 522 // Cmd: "CPAN" : B1DP Camera PAN
sayzyas 0:19075177391c 523 //------------------------------------
sayzyas 0:19075177391c 524 else if( !strncmp( rbuf+3, "CPAN", 4 ))
sayzyas 0:19075177391c 525 {
sayzyas 0:19075177391c 526 if( direction == 0 )
sayzyas 0:19075177391c 527 {
sayzyas 0:19075177391c 528 led.led_on(3);
sayzyas 0:19075177391c 529 speed = ( speed * lfsAcs.setValue.wchCtrl.no2_mtr_hspd_f ) / 10;
sayzyas 0:19075177391c 530 // speed /= 3; // Tmp setting until formal motor comming
sayzyas 0:19075177391c 531 }
sayzyas 0:19075177391c 532 else if( direction == 1 )
sayzyas 0:19075177391c 533 {
sayzyas 0:19075177391c 534 led.led_on(3);
sayzyas 0:19075177391c 535 speed = ( speed * lfsAcs.setValue.wchCtrl.no2_mtr_hspd_r ) / -10;
sayzyas 0:19075177391c 536 // speed /= 3; // Tmp setting until formal motor comming
sayzyas 0:19075177391c 537 }
sayzyas 0:19075177391c 538 else
sayzyas 0:19075177391c 539 {
sayzyas 0:19075177391c 540 flg_CPAN_motor_stop = false; // flag release when JS off
sayzyas 0:19075177391c 541 speed = 0;
sayzyas 0:19075177391c 542 }
sayzyas 0:19075177391c 543 if( flg_CPAN_motor_stop == false )
sayzyas 0:19075177391c 544 {
sayzyas 0:19075177391c 545 // DEBUG_PRINT_L3("Bd0> XX_CPAN [ dir: %d, speed: %d]", direction,speed/10);
sayzyas 0:19075177391c 546 DEBUG_PRINT_L3("\t, XX_CPAN [ dir: %d, speed: %d]", direction,speed/10);
sayzyas 0:19075177391c 547 mtrAcs.cmdControl( "XX_CPAN", 7, speed ); // Motor on
sayzyas 0:19075177391c 548 }
sayzyas 0:19075177391c 549 // Get motor current
sayzyas 0:19075177391c 550 if( speed != 0 )
sayzyas 0:19075177391c 551 {
sayzyas 0:19075177391c 552 if( direction == 0 )
sayzyas 0:19075177391c 553 {
sayzyas 0:19075177391c 554 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_CMPAN, MOTOR_DIR_FWD);
sayzyas 0:19075177391c 555 }
sayzyas 0:19075177391c 556 else
sayzyas 0:19075177391c 557 {
sayzyas 0:19075177391c 558 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_CMPAN, MOTOR_DIR_RVS);
sayzyas 0:19075177391c 559 }
sayzyas 0:19075177391c 560 // Get information
sayzyas 0:19075177391c 561 winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
sayzyas 0:19075177391c 562 if( i2cAcs.flg_motor_lock == true )
sayzyas 0:19075177391c 563 {
sayzyas 0:19075177391c 564 DEBUG_PRINT_L3("Bd0> ### CAMERA PAN LOCK !!! ###\r\n");
sayzyas 0:19075177391c 565 mtrAcs.cmdControl( "XX_CPAN", 7, 0 );
sayzyas 0:19075177391c 566 flg_CPAN_motor_stop = true;
sayzyas 0:19075177391c 567 winchPosition = 9999; // motor lock
sayzyas 0:19075177391c 568 }
sayzyas 0:19075177391c 569 }
sayzyas 0:19075177391c 570 // Send motor lock status to handy controller
sayzyas 0:19075177391c 571 if( direction != 2 )
sayzyas 0:19075177391c 572 {
sayzyas 0:19075177391c 573 sprintf( sbuf, "%04d", winchPosition );
sayzyas 0:19075177391c 574 sbuf[4] = '\0';
sayzyas 0:19075177391c 575 tcp_client.send_all(sbuf, sizeof(sbuf));
sayzyas 0:19075177391c 576 }
sayzyas 0:19075177391c 577 DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
sayzyas 0:19075177391c 578 led.led_off(3);
sayzyas 0:19075177391c 579 }
sayzyas 0:19075177391c 580 //------------------------------------
sayzyas 0:19075177391c 581 // Cmd: "CTLT" : B1DP Camera TILT
sayzyas 0:19075177391c 582 //------------------------------------
sayzyas 0:19075177391c 583 else if( !strncmp( rbuf+3, "CTLT", 4 ))
sayzyas 0:19075177391c 584 {
sayzyas 0:19075177391c 585 if( direction == 0 )
sayzyas 0:19075177391c 586 {
sayzyas 0:19075177391c 587 led.led_on(3);
sayzyas 0:19075177391c 588 speed = ( speed * lfsAcs.setValue.wchCtrl.no2_mtr_hspd_f ) / 10;
sayzyas 0:19075177391c 589 speed /= 4; // Max 6v motor
sayzyas 0:19075177391c 590 }
sayzyas 0:19075177391c 591 else if( direction == 1 )
sayzyas 0:19075177391c 592 {
sayzyas 0:19075177391c 593 led.led_on(3);
sayzyas 0:19075177391c 594 speed = ( speed * lfsAcs.setValue.wchCtrl.no2_mtr_hspd_r ) / -10;
sayzyas 0:19075177391c 595 speed /= 4; // Max 6v motor
sayzyas 0:19075177391c 596 }
sayzyas 0:19075177391c 597 else
sayzyas 0:19075177391c 598 {
sayzyas 0:19075177391c 599 flg_CTLT_motor_stop = false; // flag release when JS off
sayzyas 0:19075177391c 600 speed = 0;
sayzyas 0:19075177391c 601 }
sayzyas 0:19075177391c 602 if( flg_CTLT_motor_stop == false )
sayzyas 0:19075177391c 603 {
sayzyas 0:19075177391c 604 DEBUG_PRINT_L4("Bd0> XX_CTLT [ dir: %d, speed: %d]", direction,speed/10);
sayzyas 0:19075177391c 605 mtrAcs.cmdControl( "XX_CTLT", 7, speed ); // Motor on
sayzyas 0:19075177391c 606 }
sayzyas 0:19075177391c 607 // Get motor current
sayzyas 0:19075177391c 608 if( speed != 0 )
sayzyas 0:19075177391c 609 {
sayzyas 0:19075177391c 610 if( direction == 0 )
sayzyas 0:19075177391c 611 {
sayzyas 0:19075177391c 612 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_CTILT, MOTOR_DIR_FWD);
sayzyas 0:19075177391c 613 }
sayzyas 0:19075177391c 614 else
sayzyas 0:19075177391c 615 {
sayzyas 0:19075177391c 616 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_CTILT, MOTOR_DIR_RVS);
sayzyas 0:19075177391c 617 }
sayzyas 0:19075177391c 618 // Get information
sayzyas 0:19075177391c 619 winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
sayzyas 0:19075177391c 620 DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
sayzyas 0:19075177391c 621 if( i2cAcs.flg_motor_lock == true )
sayzyas 0:19075177391c 622 {
sayzyas 0:19075177391c 623 DEBUG_PRINT_L3("Bd0> ### CAMERA PAN LOCK !!! ###\r\n");
sayzyas 0:19075177391c 624 mtrAcs.cmdControl( "XX_CTLT", 7, 0 );
sayzyas 0:19075177391c 625 flg_CTLT_motor_stop = true;
sayzyas 0:19075177391c 626 winchPosition = 9999; // motor lock
sayzyas 0:19075177391c 627 }
sayzyas 0:19075177391c 628 }
sayzyas 0:19075177391c 629 // Send motor lock status to handy controller
sayzyas 0:19075177391c 630 if( direction != 2 )
sayzyas 0:19075177391c 631 {
sayzyas 0:19075177391c 632 sprintf( sbuf, "%04d", winchPosition );
sayzyas 0:19075177391c 633 sbuf[4] = '\0';
sayzyas 0:19075177391c 634 tcp_client.send_all(sbuf, sizeof(sbuf));
sayzyas 0:19075177391c 635 }
sayzyas 0:19075177391c 636 led.led_off(3);
sayzyas 0:19075177391c 637 }
sayzyas 0:19075177391c 638 //------------------------------------
sayzyas 0:19075177391c 639 // Cmd: "CLRF" : B1/B2DP RF Crawler
sayzyas 0:19075177391c 640 //------------------------------------
sayzyas 0:19075177391c 641 else if( !strncmp( rbuf+3, "CLRF", 4 ))
sayzyas 0:19075177391c 642 {
sayzyas 0:19075177391c 643 if( direction == 0 )
sayzyas 0:19075177391c 644 {
sayzyas 0:19075177391c 645 led.led_on(3);
sayzyas 0:19075177391c 646 speed = ( speed * lfsAcs.setValue.crwCtrl.rf_mtr_hspd_r ) / 10;
sayzyas 0:19075177391c 647 }
sayzyas 0:19075177391c 648 else if( direction == 1 )
sayzyas 0:19075177391c 649 {
sayzyas 0:19075177391c 650 led.led_on(3);
sayzyas 0:19075177391c 651 speed = ( speed * lfsAcs.setValue.crwCtrl.rf_mtr_hspd_f ) / -10;
sayzyas 0:19075177391c 652 }
sayzyas 0:19075177391c 653 else // direction == 2 is JS off
sayzyas 0:19075177391c 654 {
sayzyas 0:19075177391c 655 flg_CLRF_motor_stop = false; // flag release when JS off
sayzyas 0:19075177391c 656 speed = 0;
sayzyas 0:19075177391c 657 }
sayzyas 0:19075177391c 658 if( flg_CLRF_motor_stop == false )
sayzyas 0:19075177391c 659 {
sayzyas 0:19075177391c 660 DEBUG_PRINT_L4("Bd0> XX_CLRF [ dir: %d, speed: %d]", direction,speed/10);
sayzyas 0:19075177391c 661 mtrAcs.cmdControl( "XX_CLRF", 7, speed );
sayzyas 0:19075177391c 662 }
sayzyas 0:19075177391c 663 // Pass motor current check for B1B2 Demo Crawler motor
sayzyas 0:19075177391c 664 /*
sayzyas 0:19075177391c 665 // Get motor current
sayzyas 0:19075177391c 666 if( speed != 0 )
sayzyas 0:19075177391c 667 {
sayzyas 0:19075177391c 668 if( direction == 0 )
sayzyas 0:19075177391c 669 {
sayzyas 0:19075177391c 670 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_RFCRW, MOTOR_DIR_FWD);
sayzyas 0:19075177391c 671 }
sayzyas 0:19075177391c 672 else
sayzyas 0:19075177391c 673 {
sayzyas 0:19075177391c 674 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_RFCRW, MOTOR_DIR_RVS);
sayzyas 0:19075177391c 675 }
sayzyas 0:19075177391c 676 // Get information
sayzyas 0:19075177391c 677 winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
sayzyas 0:19075177391c 678 DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
sayzyas 0:19075177391c 679 if( i2cAcs.flg_motor_lock == true )
sayzyas 0:19075177391c 680 {
sayzyas 0:19075177391c 681 DEBUG_PRINT_L3("Bd0> ### RF CRAWLER LOCK !!! ###\r\n");
sayzyas 0:19075177391c 682 mtrAcs.cmdControl( "XX_CLRF", 7, 0 );
sayzyas 0:19075177391c 683 flg_CLRF_motor_stop = true;
sayzyas 0:19075177391c 684 }
sayzyas 0:19075177391c 685 }
sayzyas 0:19075177391c 686 */
sayzyas 0:19075177391c 687 DEBUG_PRINT_L4("\r\n");
sayzyas 0:19075177391c 688 // Don't Send motor lock status to handy controller
sayzyas 0:19075177391c 689 led.led_off(3);
sayzyas 0:19075177391c 690 }
sayzyas 0:19075177391c 691
sayzyas 0:19075177391c 692 //------------------------------------
sayzyas 0:19075177391c 693 // Cmd: "CLLB" : B1/B2DP LB Crawler
sayzyas 0:19075177391c 694 //------------------------------------
sayzyas 0:19075177391c 695 else if( !strncmp( rbuf+3, "CLLB", 4 ))
sayzyas 0:19075177391c 696 {
sayzyas 0:19075177391c 697 if( direction == 0 )
sayzyas 0:19075177391c 698 {
sayzyas 0:19075177391c 699 led.led_on(3);
sayzyas 0:19075177391c 700 speed = ( speed * lfsAcs.setValue.crwCtrl.lb_mtr_hspd_f ) / 10;
sayzyas 0:19075177391c 701 }
sayzyas 0:19075177391c 702 else if( direction == 1 )
sayzyas 0:19075177391c 703 {
sayzyas 0:19075177391c 704 led.led_on(3);
sayzyas 0:19075177391c 705 speed = ( speed * lfsAcs.setValue.crwCtrl.lb_mtr_hspd_r ) / -10;
sayzyas 0:19075177391c 706 }
sayzyas 0:19075177391c 707 else // direction == 2 is JS off
sayzyas 0:19075177391c 708 {
sayzyas 0:19075177391c 709 flg_CLLB_motor_stop = false; // flag release when JS off
sayzyas 0:19075177391c 710 speed = 0;
sayzyas 0:19075177391c 711 }
sayzyas 0:19075177391c 712 DEBUG_PRINT_L4("Bd0> XX_CLLB [%d]\r\n", speed);
sayzyas 0:19075177391c 713 if( flg_CLLB_motor_stop == false )
sayzyas 0:19075177391c 714 {
sayzyas 0:19075177391c 715 mtrAcs.cmdControl( "XX_CLLB", 7, speed );
sayzyas 0:19075177391c 716 }
sayzyas 0:19075177391c 717 // Pass motor current check for B1B2 Demo Crawler motor
sayzyas 0:19075177391c 718 /*
sayzyas 0:19075177391c 719 // Get motor current
sayzyas 0:19075177391c 720 if( speed != 0 )
sayzyas 0:19075177391c 721 {
sayzyas 0:19075177391c 722 if( direction == 0 )
sayzyas 0:19075177391c 723 {
sayzyas 0:19075177391c 724 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_LBCRW, MOTOR_DIR_FWD);
sayzyas 0:19075177391c 725 }
sayzyas 0:19075177391c 726 else
sayzyas 0:19075177391c 727 {
sayzyas 0:19075177391c 728 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_LBCRW, MOTOR_DIR_RVS);
sayzyas 0:19075177391c 729 }
sayzyas 0:19075177391c 730 // Get information
sayzyas 0:19075177391c 731 winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
sayzyas 0:19075177391c 732 DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
sayzyas 0:19075177391c 733 if( i2cAcs.flg_motor_lock == true )
sayzyas 0:19075177391c 734 {
sayzyas 0:19075177391c 735 DEBUG_PRINT_L3("Bd0> ### LB CRAWLER LOCK !!! ###\r\n");
sayzyas 0:19075177391c 736 mtrAcs.cmdControl( "XX_CLLB", 7, 0 );
sayzyas 0:19075177391c 737 flg_CLLB_motor_stop = true;
sayzyas 0:19075177391c 738 }
sayzyas 0:19075177391c 739 }
sayzyas 0:19075177391c 740 */
sayzyas 0:19075177391c 741 /*
sayzyas 0:19075177391c 742 // Send motor lock status to handy controller
sayzyas 0:19075177391c 743 if(( flg_CLRF_motor_stop == true ) || ( flg_CLLB_motor_stop == true ))
sayzyas 0:19075177391c 744 {
sayzyas 0:19075177391c 745 winchPosition = 9999; // motor lock
sayzyas 0:19075177391c 746 }
sayzyas 0:19075177391c 747 if( direction != 2 )
sayzyas 0:19075177391c 748 {
sayzyas 0:19075177391c 749 sprintf( sbuf, "%04d", winchPosition );
sayzyas 0:19075177391c 750 sbuf[4] = '\0';
sayzyas 0:19075177391c 751 tcp_client.send_all(sbuf, sizeof(sbuf));
sayzyas 0:19075177391c 752 }
sayzyas 0:19075177391c 753 */
sayzyas 0:19075177391c 754 DEBUG_PRINT_L4("\r\n");
sayzyas 0:19075177391c 755 led.led_off(3);
sayzyas 0:19075177391c 756 }
sayzyas 0:19075177391c 757 //------------------------------------
sayzyas 0:19075177391c 758 // Cmd: "TFRF" : B1/B2DP RF transform
sayzyas 0:19075177391c 759 //------------------------------------
sayzyas 0:19075177391c 760 else if( !strncmp( rbuf+3, "TFRF", 4 ))
sayzyas 0:19075177391c 761 {
sayzyas 0:19075177391c 762 if( direction == 0 )
sayzyas 0:19075177391c 763 {
sayzyas 0:19075177391c 764 led.led_on(3);
sayzyas 0:19075177391c 765 if( ( lswstop == 1 ) && ( limit_sw_rf == 0 ) )
sayzyas 0:19075177391c 766 {
sayzyas 0:19075177391c 767 DEBUG_PRINT_L3("RF limit sw ON\r\n");
sayzyas 0:19075177391c 768 speed = 0;
sayzyas 0:19075177391c 769 }
sayzyas 0:19075177391c 770 else
sayzyas 0:19075177391c 771 {
sayzyas 0:19075177391c 772 speed = ( speed * lfsAcs.setValue.tfmCtrl.rf_mtr_hspd_f ) / 10;
sayzyas 0:19075177391c 773 }
sayzyas 0:19075177391c 774 }
sayzyas 0:19075177391c 775 else if( direction == 1 ) // reverse
sayzyas 0:19075177391c 776 {
sayzyas 0:19075177391c 777 led.led_on(3);
sayzyas 0:19075177391c 778 if( ( lswstop == 1 ) && ( limit_sw_rf == 0 ) )
sayzyas 0:19075177391c 779 {
sayzyas 0:19075177391c 780 DEBUG_PRINT_L3("RF limit sw ON\r\n");
sayzyas 0:19075177391c 781 speed = 0;
sayzyas 0:19075177391c 782 }
sayzyas 0:19075177391c 783 else
sayzyas 0:19075177391c 784 {
sayzyas 0:19075177391c 785 speed = ( speed * lfsAcs.setValue.tfmCtrl.rf_mtr_hspd_r ) / -10;
sayzyas 0:19075177391c 786 }
sayzyas 0:19075177391c 787 }
sayzyas 0:19075177391c 788 else
sayzyas 0:19075177391c 789 {
sayzyas 0:19075177391c 790 flg_TFRF_motor_stop = false; // flag release when JS off
sayzyas 0:19075177391c 791 speed = 0;
sayzyas 0:19075177391c 792 }
sayzyas 0:19075177391c 793 if( flg_TFRF_motor_stop == false )
sayzyas 0:19075177391c 794 {
sayzyas 0:19075177391c 795 DEBUG_PRINT_L4("Bd0> XX_TFRF [%d]\r\n", speed);
sayzyas 0:19075177391c 796 mtrAcs.cmdControl( "XX_TFRF", 7, speed );
sayzyas 0:19075177391c 797 }
sayzyas 0:19075177391c 798 // Get motor current
sayzyas 0:19075177391c 799 if( speed != 0 )
sayzyas 0:19075177391c 800 {
sayzyas 0:19075177391c 801 if( direction == 0 )
sayzyas 0:19075177391c 802 {
sayzyas 0:19075177391c 803 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_RFTFM, MOTOR_DIR_FWD);
sayzyas 0:19075177391c 804 }
sayzyas 0:19075177391c 805 else
sayzyas 0:19075177391c 806 {
sayzyas 0:19075177391c 807 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_RFTFM, MOTOR_DIR_RVS);
sayzyas 0:19075177391c 808 }
sayzyas 0:19075177391c 809 // Get information
sayzyas 0:19075177391c 810 winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
sayzyas 0:19075177391c 811 DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
sayzyas 0:19075177391c 812 if( i2cAcs.flg_motor_lock == true )
sayzyas 0:19075177391c 813 {
sayzyas 0:19075177391c 814 DEBUG_PRINT_L3("Bd0> ### RF TRANSFORM LOCK !!! ###\r\n");
sayzyas 0:19075177391c 815 mtrAcs.cmdControl( "XX_TFRF", 7, 0 );
sayzyas 0:19075177391c 816 flg_TFRF_motor_stop = true;
sayzyas 0:19075177391c 817 winchPosition = 9999; // motor lock
sayzyas 0:19075177391c 818 // flg_mlock_check = false;
sayzyas 0:19075177391c 819 i2cAcs.i2cClearMLCnt(I2C_ADDRESS_RESOLVER);
sayzyas 0:19075177391c 820 }
sayzyas 0:19075177391c 821 }
sayzyas 0:19075177391c 822 // Send motor lock status to handy controller
sayzyas 0:19075177391c 823 if( direction != 2 )
sayzyas 0:19075177391c 824 {
sayzyas 0:19075177391c 825 sprintf( sbuf, "%04d", winchPosition );
sayzyas 0:19075177391c 826 sbuf[4] = '\0';
sayzyas 0:19075177391c 827 tcp_client.send_all(sbuf, sizeof(sbuf));
sayzyas 0:19075177391c 828 }
sayzyas 0:19075177391c 829 led.led_off(3);
sayzyas 0:19075177391c 830 }
sayzyas 0:19075177391c 831
sayzyas 0:19075177391c 832 //------------------------------------
sayzyas 0:19075177391c 833 // Cmd: "TFLB" : B1/B2DP LB transform
sayzyas 0:19075177391c 834 //------------------------------------
sayzyas 0:19075177391c 835 else if( !strncmp( rbuf+3, "TFLB", 4 ))
sayzyas 0:19075177391c 836 {
sayzyas 0:19075177391c 837 if( direction == 0 )
sayzyas 0:19075177391c 838 {
sayzyas 0:19075177391c 839 led.led_on(3);
sayzyas 0:19075177391c 840 if( ( lswstop == 1 ) && ( limit_sw_lb == 0 ) )
sayzyas 0:19075177391c 841 {
sayzyas 0:19075177391c 842 DEBUG_PRINT_L3("LB limit sw ON\r\n");
sayzyas 0:19075177391c 843 speed = 0;
sayzyas 0:19075177391c 844 }
sayzyas 0:19075177391c 845 else
sayzyas 0:19075177391c 846 {
sayzyas 0:19075177391c 847 speed = ( speed * lfsAcs.setValue.tfmCtrl.lb_mtr_hspd_f ) / 10;
sayzyas 0:19075177391c 848 }
sayzyas 0:19075177391c 849 }
sayzyas 0:19075177391c 850 else if( direction == 1 )
sayzyas 0:19075177391c 851 {
sayzyas 0:19075177391c 852 led.led_on(3);
sayzyas 0:19075177391c 853 if( ( lswstop == 1 ) && ( limit_sw_lb == 0 ) )
sayzyas 0:19075177391c 854 {
sayzyas 0:19075177391c 855 DEBUG_PRINT_L3("LB limit sw ON\r\n");
sayzyas 0:19075177391c 856 speed = 0;
sayzyas 0:19075177391c 857 }
sayzyas 0:19075177391c 858 else
sayzyas 0:19075177391c 859 {
sayzyas 0:19075177391c 860 speed = ( speed * lfsAcs.setValue.tfmCtrl.lb_mtr_hspd_r ) / -10;
sayzyas 0:19075177391c 861 }
sayzyas 0:19075177391c 862 }
sayzyas 0:19075177391c 863 else
sayzyas 0:19075177391c 864 {
sayzyas 0:19075177391c 865 flg_TFLB_motor_stop = false; // flag release when JS off
sayzyas 0:19075177391c 866 speed = 0;
sayzyas 0:19075177391c 867 }
sayzyas 0:19075177391c 868 if( flg_TFLB_motor_stop == false )
sayzyas 0:19075177391c 869 {
sayzyas 0:19075177391c 870 DEBUG_PRINT_L4("Bd0> XX_TFLB [%d]\r\n", speed);
sayzyas 0:19075177391c 871 mtrAcs.cmdControl( "XX_TFLB", 7, speed );
sayzyas 0:19075177391c 872 }
sayzyas 0:19075177391c 873 if( speed != 0 )
sayzyas 0:19075177391c 874 {
sayzyas 0:19075177391c 875 if( direction == 0 )
sayzyas 0:19075177391c 876 {
sayzyas 0:19075177391c 877 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_LBTFM, MOTOR_DIR_FWD);
sayzyas 0:19075177391c 878 }
sayzyas 0:19075177391c 879 else
sayzyas 0:19075177391c 880 {
sayzyas 0:19075177391c 881 i2cAcs.i2cGetMotorCurrent(I2C_ADDRESS_RESOLVER, MOTOR_LBTFM, MOTOR_DIR_RVS);
sayzyas 0:19075177391c 882 }
sayzyas 0:19075177391c 883 // Read information
sayzyas 0:19075177391c 884 winchPosition = i2cAcs.i2cReadInformation( I2C_ADDRESS_RESOLVER, 0 );
sayzyas 0:19075177391c 885 DEBUG_PRINT_L3( " --> POSITION [%04d]\r\n", winchPosition );
sayzyas 0:19075177391c 886 if( i2cAcs.flg_motor_lock == true )
sayzyas 0:19075177391c 887 {
sayzyas 0:19075177391c 888 DEBUG_PRINT_L3("Bd0> ### LB TRANSFORM LOCK !!! ###\r\n");
sayzyas 0:19075177391c 889 mtrAcs.cmdControl( "XX_TFLB", 7, 0 );
sayzyas 0:19075177391c 890 flg_TFLB_motor_stop = true;
sayzyas 0:19075177391c 891 winchPosition = 9999; // motor lock
sayzyas 0:19075177391c 892 i2cAcs.i2cClearMLCnt(I2C_ADDRESS_RESOLVER);
sayzyas 0:19075177391c 893 // flg_mlock_check = false;
sayzyas 0:19075177391c 894 }
sayzyas 0:19075177391c 895 }
sayzyas 0:19075177391c 896 // Send motor lock status to handy controller
sayzyas 0:19075177391c 897 if( direction != 2 )
sayzyas 0:19075177391c 898 {
sayzyas 0:19075177391c 899 sprintf( sbuf, "%04d", winchPosition );
sayzyas 0:19075177391c 900 sbuf[4] = '\0';
sayzyas 0:19075177391c 901 tcp_client.send_all(sbuf, sizeof(sbuf));
sayzyas 0:19075177391c 902 }
sayzyas 0:19075177391c 903
sayzyas 0:19075177391c 904 led.led_off(3);
sayzyas 0:19075177391c 905 }
sayzyas 0:19075177391c 906 //------------------------------------
sayzyas 0:19075177391c 907 // All motor stop
sayzyas 0:19075177391c 908 //------------------------------------
sayzyas 0:19075177391c 909 else
sayzyas 0:19075177391c 910 {
sayzyas 0:19075177391c 911 i2cAcs.flg_motor_lock = false; // no lock
sayzyas 0:19075177391c 912 mtrAcs.cmdControl( "XX_TFRF", 7, 0 );
sayzyas 0:19075177391c 913 mtrAcs.cmdControl( "XX_TFLB", 7, 0 );
sayzyas 0:19075177391c 914 mtrAcs.cmdControl( "XX_CLRF", 7, 0 );
sayzyas 0:19075177391c 915 mtrAcs.cmdControl( "XX_CLLB", 7, 0 );
sayzyas 0:19075177391c 916 mtrAcs.cmdControl( "XX_WICH", 7, 0 );
sayzyas 0:19075177391c 917 mtrAcs.cmdControl( "XX_CPAN", 7, 0 );
sayzyas 0:19075177391c 918 mtrAcs.cmdControl( "XX_CTLT", 7, 0 );
sayzyas 0:19075177391c 919 // i2cAcs.i2cClearMLCnt(I2C_ADDRESS_RESOLVER);
sayzyas 0:19075177391c 920 }
sayzyas 0:19075177391c 921 }
sayzyas 0:19075177391c 922 //Thread::wait(20); // <== never insert this !
sayzyas 0:19075177391c 923 }
sayzyas 0:19075177391c 924 }
sayzyas 0:19075177391c 925 tcp_client.close();
sayzyas 0:19075177391c 926 }
sayzyas 0:19075177391c 927 }
sayzyas 0:19075177391c 928 }
sayzyas 0:19075177391c 929 else{
sayzyas 0:19075177391c 930 led.led_main_on();
sayzyas 0:19075177391c 931 wMusic.bz_error(5);
sayzyas 0:19075177391c 932 while( true ){
sayzyas 0:19075177391c 933 led.led_error();
sayzyas 0:19075177391c 934 led.led_main_blink(5);
sayzyas 0:19075177391c 935 DEBUG_PRINT_L0("Bd0> ***ERROR*** Eternat connect Fali\r\n");
sayzyas 0:19075177391c 936 DEBUG_PRINT_L0("Bd0> This programis booting in Stand alone mode.\r\n");
sayzyas 0:19075177391c 937 Thread::wait(1000);
sayzyas 0:19075177391c 938 }
sayzyas 0:19075177391c 939 }
sayzyas 0:19075177391c 940 }
sayzyas 0:19075177391c 941 else{
sayzyas 0:19075177391c 942 led.led_main_on();
sayzyas 0:19075177391c 943 wMusic.bz_error(5);
sayzyas 0:19075177391c 944 while( true ){
sayzyas 0:19075177391c 945 led.led_error();
sayzyas 0:19075177391c 946 led.led_main_blink(5);
sayzyas 0:19075177391c 947 DEBUG_PRINT_L0("Bd0> *** ERROR*** Eternat init Fail\r\n");
sayzyas 0:19075177391c 948 DEBUG_PRINT_L0("Bd0> This program is booting in Stand alone mode.\r\n");
sayzyas 0:19075177391c 949 Thread::wait(1000);
sayzyas 0:19075177391c 950 }
sayzyas 0:19075177391c 951 }
sayzyas 0:19075177391c 952 led.led_main_off();
sayzyas 0:19075177391c 953 led.led_off(2);
sayzyas 0:19075177391c 954 }