z ysaito / Mbed 2 deprecated Pro_B2_1ZNR_CrExpB2_MotorCtrl_LPC824MAX

2018.08.06

Dependencies:   mbed-rtos mbed

1_main.cpp@3:4a7dea2da5b5, 2018-08-06 (annotated)

Committer:
sayzyas
Date:
Mon Aug 06 08:29:14 2018 +0000
Revision:
3:4a7dea2da5b5
Parent:
2:aeff576d5013 
2018.08.06;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:653609b2a5cf 1 /**********************************************************
sayzyas 0:653609b2a5cf 2 * Project: B2 (1F-1)
sayzyas 0:653609b2a5cf 3 * Title: CrExp B2 Motor Ctrl Main
sayzyas 0:653609b2a5cf 4 * Target: LPCXpresso824-Max
sayzyas 0:653609b2a5cf 5 * or ( LPC800Max )
sayzyas 0:653609b2a5cf 6 * Author: sayzyas
sayzyas 0:653609b2a5cf 7 * Date(Latest update) 2015.06.17(Wed)
sayzyas 0:653609b2a5cf 8 * --------------------------------------------------------
sayzyas 0:653609b2a5cf 9 *
sayzyas 0:653609b2a5cf 10 * LPCXpresso 824-MAX
sayzyas 0:653609b2a5cf 11 * +---------USB---------+
sayzyas 0:653609b2a5cf 12 * | |
sayzyas 0:653609b2a5cf 13 * | |
sayzyas 0:653609b2a5cf 14 * | |
sayzyas 0:653609b2a5cf 15 * | |
sayzyas 0:653609b2a5cf 16 * | | SCL P0_10 D15
sayzyas 0:653609b2a5cf 17 * | ## ### ## | SDA P0_11 D14
sayzyas 0:653609b2a5cf 18 * | # # # # # # | AVDD
sayzyas 0:653609b2a5cf 19 * N/A | # # # # # | GND
sayzyas 0:653609b2a5cf 20 * +3V3 | ## # # # | SCK P0_24 D13
sayzyas 0:653609b2a5cf 21 * NRST | # # # # # | MISO P0_25 D12
sayzyas 0:653609b2a5cf 22 * +3V3 | # # # # ###### | MOSI P0_26 D11 -> I/O Ex Bd
sayzyas 0:653609b2a5cf 23 * +5V | ## ##### # | SSEL P0_15 D10 -> I/O Ex Bd
sayzyas 0:653609b2a5cf 24 * GND | | P0_27 D9 -> I/O Ex Bd
sayzyas 0:653609b2a5cf 25 * GND | | P0_13 D8 -> I/O Ex Bd
sayzyas 0:653609b2a5cf 26 * N/A | | P0_17 D7
sayzyas 0:653609b2a5cf 27 * | | P0_16 D6
sayzyas 0:653609b2a5cf 28 * A0 P0_6 | | PWM P0_28 D5
sayzyas 0:653609b2a5cf 29 * A1 P0_14 | | PWM P0_18 D4
sayzyas 0:653609b2a5cf 30 * A2 P0_23 | | PWM P0_12 D3
sayzyas 0:653609b2a5cf 31 * A3 P0_22 | | PWM P0_19 D2
sayzyas 0:653609b2a5cf 32 * A4 P0_21 | | TX P0_4 D1 -> UART Tx
sayzyas 0:653609b2a5cf 33 * A5 P0_20 | | RX P0_0 D0 -> UART Rx
sayzyas 0:653609b2a5cf 34 * +---------------------+
sayzyas 0:653609b2a5cf 35 *
sayzyas 0:653609b2a5cf 36 ***************************************/
sayzyas 0:653609b2a5cf 37 /*
sayzyas 0:653609b2a5cf 38 2016.02.06
sayzyas 0:653609b2a5cf 39 Added Limit switch detect control in RoboteQ motor control task.
sayzyas 0:653609b2a5cf 40 */
sayzyas 0:653609b2a5cf 41 #define ___CURRENT_CHECK___
sayzyas 0:653609b2a5cf 42
sayzyas 0:653609b2a5cf 43 #include "mbed.h"
sayzyas 0:653609b2a5cf 44 #include "rtos.h"
sayzyas 0:653609b2a5cf 45 #include "common.h"
sayzyas 0:653609b2a5cf 46
sayzyas 0:653609b2a5cf 47
sayzyas 0:653609b2a5cf 48
sayzyas 0:653609b2a5cf 49 // Hardware setting
sayzyas 0:653609b2a5cf 50 Serial pc(USBTX, USBRX); // tx, rx
sayzyas 0:653609b2a5cf 51 Serial sdc2130(P0_4, P0_0); // Communicate with RpboteQ Driver by tx, rx
sayzyas 0:653609b2a5cf 52
sayzyas 0:653609b2a5cf 53 /* #### Don't use #### */
sayzyas 0:653609b2a5cf 54 /* LED indicator for motor current indicate */
sayzyas 2:aeff576d5013 55 //DigitalOut myled0(P0_6); // My LED0
sayzyas 2:aeff576d5013 56 //DigitalOut myled1(P0_14); // My LED1
sayzyas 0:653609b2a5cf 57 DigitalOut myled2(P0_23); // My LED2
sayzyas 0:653609b2a5cf 58 DigitalOut myled3(P0_22); // My LED3
sayzyas 0:653609b2a5cf 59
sayzyas 0:653609b2a5cf 60 DigitalOut led1(LED1);
sayzyas 0:653609b2a5cf 61 DigitalOut led2(LED2);
sayzyas 0:653609b2a5cf 62 DigitalOut led3(LED3);
sayzyas 0:653609b2a5cf 63
sayzyas 2:aeff576d5013 64 /* Analog input setting */
sayzyas 2:aeff576d5013 65 AnalogIn motor1_speed_adjin(A0);
sayzyas 2:aeff576d5013 66 AnalogIn motor2_speed_adjin(A1);
sayzyas 2:aeff576d5013 67
sayzyas 2:aeff576d5013 68 AnalogIn motor1_current_in(A5);
sayzyas 2:aeff576d5013 69 AnalogIn motor2_current_in(A4);
sayzyas 0:653609b2a5cf 70
sayzyas 0:653609b2a5cf 71 /* Digital In */
sayzyas 2:aeff576d5013 72 //DigitalIn limit_sw_rf(P0_24); // Limit switch (RF-h) for Transform controller only
sayzyas 2:aeff576d5013 73 //DigitalIn limit_sw_lb(P0_25); // Limit switch (LB-I) for Transform controller only
sayzyas 2:aeff576d5013 74 //2016.05.18 Limit switch input port change to opposite.
sayzyas 2:aeff576d5013 75 DigitalIn limit_sw_rf(P0_25); // Limit switch (RF-h) for Transform controller only
sayzyas 2:aeff576d5013 76 DigitalIn limit_sw_lb(P0_24); // Limit switch (LB-I) for Transform controller only
sayzyas 2:aeff576d5013 77
sayzyas 0:653609b2a5cf 78
sayzyas 1:c3508bfe65b6 79 Mutex stdio_mutex;
sayzyas 0:653609b2a5cf 80
sayzyas 1:c3508bfe65b6 81 bool flg_lsw_valid = false;
sayzyas 0:653609b2a5cf 82
sayzyas 0:653609b2a5cf 83 /* ** Worning *****************************
sayzyas 0:653609b2a5cf 84 * M1 side is not compatible with Arduino IO
sayzyas 0:653609b2a5cf 85 * exp board, so use following pin and modify
sayzyas 0:653609b2a5cf 86 * or re-connect signal of IO exp board. */
sayzyas 0:653609b2a5cf 87 //DigitalOut motor1_dir(P0_24); //D8
sayzyas 0:653609b2a5cf 88 //PwmOut motor1_out(P0_25);
sayzyas 0:653609b2a5cf 89 //DigitalOut motor1_out(P0_25); //D9
sayzyas 0:653609b2a5cf 90
sayzyas 0:653609b2a5cf 91 // M2 side is compatible with Arduino IO exp board.
sayzyas 0:653609b2a5cf 92 //DigitalOut motor2_out(P0_15); //D10
sayzyas 0:653609b2a5cf 93 //DigitalOut motor2_dir(P0_26); //D11
sayzyas 0:653609b2a5cf 94 //PwmOut motor2_out(P0_15);
sayzyas 0:653609b2a5cf 95
sayzyas 0:653609b2a5cf 96 I2CSlave slave(P0_11, P0_10); //I2C SDA SCL
sayzyas 0:653609b2a5cf 97
sayzyas 0:653609b2a5cf 98 //QEI wheel( P0_15, P0_26, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
sayzyas 0:653609b2a5cf 99
sayzyas 0:653609b2a5cf 100 // ----------------------------------------------------------------
sayzyas 0:653609b2a5cf 101 // Prototype
sayzyas 0:653609b2a5cf 102 //
sayzyas 0:653609b2a5cf 103 void led_onoff(void);
sayzyas 0:653609b2a5cf 104 void led_demo(void);
sayzyas 0:653609b2a5cf 105 void myled_control( int action );
sayzyas 0:653609b2a5cf 106 int read_motor_current( uint32_t motor_no );
sayzyas 0:653609b2a5cf 107 void current_read_task(void const *);
sayzyas 0:653609b2a5cf 108 void motor1_ctrl( uint32_t onoff, uint32_t dir );
sayzyas 0:653609b2a5cf 109 void motor2_ctrl( uint32_t onoff, uint32_t dir );
sayzyas 0:653609b2a5cf 110 void RoboteQM1DrvCtrl_task( void const *);
sayzyas 0:653609b2a5cf 111 void RoboteQM2DrvCtrl_task( void const *);
sayzyas 0:653609b2a5cf 112 void motor_control_task(void const *);
sayzyas 0:653609b2a5cf 113 void set_init_motor_current(uint32_t motor_no, float motor_current, int cnt);
sayzyas 0:653609b2a5cf 114 float calb_motor_current(uint32_t motor_no, float motor_current);
sayzyas 0:653609b2a5cf 115
sayzyas 0:653609b2a5cf 116 // ----------------------------------------------------------------
sayzyas 0:653609b2a5cf 117 // Sub Routine
sayzyas 0:653609b2a5cf 118 //
sayzyas 0:653609b2a5cf 119 void led_onoff(){
sayzyas 0:653609b2a5cf 120 led1 = 1;
sayzyas 0:653609b2a5cf 121 wait(0.5);
sayzyas 0:653609b2a5cf 122 led1 = 0; // off
sayzyas 0:653609b2a5cf 123 wait(0.5);
sayzyas 0:653609b2a5cf 124
sayzyas 0:653609b2a5cf 125 }
sayzyas 0:653609b2a5cf 126 // LED demo
sayzyas 0:653609b2a5cf 127 void led_demo(){
sayzyas 0:653609b2a5cf 128 int i;
sayzyas 0:653609b2a5cf 129 for( i = 0; i < 5; i++ ) {
sayzyas 0:653609b2a5cf 130 led1 = LED_ON;
sayzyas 0:653609b2a5cf 131 led2 = LED_OFF;
sayzyas 0:653609b2a5cf 132 led3 = LED_OFF;
sayzyas 0:653609b2a5cf 133 wait_ms(50);
sayzyas 0:653609b2a5cf 134 led1 = LED_OFF;
sayzyas 0:653609b2a5cf 135 led2 = LED_OFF;
sayzyas 0:653609b2a5cf 136 led3 = LED_OFF;
sayzyas 0:653609b2a5cf 137 wait_ms(50);
sayzyas 0:653609b2a5cf 138 led1 = LED_OFF;
sayzyas 0:653609b2a5cf 139 led2 = LED_ON;
sayzyas 0:653609b2a5cf 140 led3 = LED_OFF;
sayzyas 0:653609b2a5cf 141 wait_ms(50);
sayzyas 0:653609b2a5cf 142 led1 = LED_OFF;
sayzyas 0:653609b2a5cf 143 led2 = LED_OFF;
sayzyas 0:653609b2a5cf 144 led3 = LED_OFF;
sayzyas 0:653609b2a5cf 145 wait_ms(50);
sayzyas 0:653609b2a5cf 146 led1 = LED_OFF;
sayzyas 0:653609b2a5cf 147 led2 = LED_OFF;
sayzyas 0:653609b2a5cf 148 led3 = LED_ON;
sayzyas 0:653609b2a5cf 149 wait_ms(50);
sayzyas 0:653609b2a5cf 150 led1 = LED_OFF;
sayzyas 0:653609b2a5cf 151 led2 = LED_OFF;
sayzyas 0:653609b2a5cf 152 led3 = LED_OFF;
sayzyas 0:653609b2a5cf 153 wait_ms(50);
sayzyas 0:653609b2a5cf 154 }
sayzyas 0:653609b2a5cf 155 wait_ms(3000);
sayzyas 0:653609b2a5cf 156 }
sayzyas 0:653609b2a5cf 157
sayzyas 0:653609b2a5cf 158 void myled_control( int action ){
sayzyas 0:653609b2a5cf 159
sayzyas 0:653609b2a5cf 160 for( int i = 0; i < 10; i++ ){
sayzyas 2:aeff576d5013 161 // myled0 = 1;
sayzyas 2:aeff576d5013 162 // myled1 = 1;
sayzyas 0:653609b2a5cf 163 myled2 = 1;
sayzyas 0:653609b2a5cf 164 myled3 = 1;
sayzyas 0:653609b2a5cf 165 wait(0.2);
sayzyas 2:aeff576d5013 166 // myled0 = 0; // off
sayzyas 2:aeff576d5013 167 // myled1 = 0; // off
sayzyas 0:653609b2a5cf 168 myled2 = 0; // off
sayzyas 0:653609b2a5cf 169 myled3 = 0; // off
sayzyas 0:653609b2a5cf 170 wait(0.2);
sayzyas 0:653609b2a5cf 171 }
sayzyas 0:653609b2a5cf 172 }
sayzyas 0:653609b2a5cf 173
sayzyas 2:aeff576d5013 174 uint32_t motor1_lock_count = 0;
sayzyas 2:aeff576d5013 175 uint32_t motor2_lock_count = 0;
sayzyas 0:653609b2a5cf 176 volatile bool flg_motor1_lockcup = false;
sayzyas 0:653609b2a5cf 177 volatile bool flg_motor2_lockcup = false;
sayzyas 0:653609b2a5cf 178 volatile uint32_t flg_motor1_lock = 0;
sayzyas 0:653609b2a5cf 179 volatile uint32_t flg_motor2_lock = 0;
sayzyas 0:653609b2a5cf 180
sayzyas 0:653609b2a5cf 181 int i2c_saddress = 0;
sayzyas 0:653609b2a5cf 182
sayzyas 0:653609b2a5cf 183 uint8_t send_data[7];
sayzyas 0:653609b2a5cf 184
sayzyas 0:653609b2a5cf 185 float motor1_current_center_value;
sayzyas 0:653609b2a5cf 186 float motor2_current_center_value;
sayzyas 0:653609b2a5cf 187 float motor3_current_center_value;
sayzyas 0:653609b2a5cf 188
sayzyas 0:653609b2a5cf 189 uint8_t motor1_speed;
sayzyas 0:653609b2a5cf 190 uint8_t motor2_speed;
sayzyas 0:653609b2a5cf 191
sayzyas 2:aeff576d5013 192 uint16_t motor1_speed_adjdata;
sayzyas 2:aeff576d5013 193 uint16_t motor2_speed_adjdata;
sayzyas 2:aeff576d5013 194
sayzyas 0:653609b2a5cf 195 // --------------------------------------------------------------
sayzyas 0:653609b2a5cf 196 // Motor Current Calibration Function
sayzyas 0:653609b2a5cf 197 // Arg: Motor Number 1 - 3
sayzyas 0:653609b2a5cf 198 // --------------------------------------------------------------
sayzyas 0:653609b2a5cf 199 void set_init_motor_current(uint32_t motor_no, float motor_current, int cnt){
sayzyas 0:653609b2a5cf 200
sayzyas 0:653609b2a5cf 201 for( int i = 0; i < cnt; i++ ){
sayzyas 0:653609b2a5cf 202 if( motor_no == 1 ){
sayzyas 0:653609b2a5cf 203 motor1_current_center_value = motor_current;
sayzyas 0:653609b2a5cf 204 }
sayzyas 0:653609b2a5cf 205 else if( motor_no == 2 ){
sayzyas 0:653609b2a5cf 206 motor2_current_center_value = motor_current;
sayzyas 0:653609b2a5cf 207 }
sayzyas 0:653609b2a5cf 208 else if( motor_no == 3 ){
sayzyas 0:653609b2a5cf 209 motor2_current_center_value = motor_current;
sayzyas 0:653609b2a5cf 210 }
sayzyas 0:653609b2a5cf 211 }
sayzyas 0:653609b2a5cf 212 }
sayzyas 0:653609b2a5cf 213
sayzyas 0:653609b2a5cf 214 // --------------------------------------------------------------
sayzyas 0:653609b2a5cf 215 // Motor Current Calibration Function
sayzyas 0:653609b2a5cf 216 // Arg: Motor Number 1 - 3
sayzyas 0:653609b2a5cf 217 // --------------------------------------------------------------
sayzyas 0:653609b2a5cf 218 float calb_motor_current(uint32_t motor_no, float motor_current){
sayzyas 0:653609b2a5cf 219 float rts;
sayzyas 0:653609b2a5cf 220
sayzyas 0:653609b2a5cf 221 if( motor_no == 1 ){
sayzyas 0:653609b2a5cf 222 motor1_current_center_value += motor_current;
sayzyas 0:653609b2a5cf 223 motor1_current_center_value /= 2;
sayzyas 0:653609b2a5cf 224 rts = motor1_current_center_value;
sayzyas 0:653609b2a5cf 225 }
sayzyas 0:653609b2a5cf 226 else if( motor_no == 2 ){
sayzyas 0:653609b2a5cf 227 motor2_current_center_value += motor_current;
sayzyas 0:653609b2a5cf 228 motor2_current_center_value /= 2;
sayzyas 0:653609b2a5cf 229 rts = motor2_current_center_value;
sayzyas 0:653609b2a5cf 230 }
sayzyas 0:653609b2a5cf 231 else if( motor_no == 3 ){
sayzyas 0:653609b2a5cf 232 motor3_current_center_value += motor_current;
sayzyas 0:653609b2a5cf 233 motor3_current_center_value /= 2;
sayzyas 0:653609b2a5cf 234 rts = motor3_current_center_value;
sayzyas 0:653609b2a5cf 235 }
sayzyas 1:c3508bfe65b6 236 else rts = 0;
sayzyas 0:653609b2a5cf 237 return rts;
sayzyas 0:653609b2a5cf 238 }
sayzyas 0:653609b2a5cf 239
sayzyas 1:c3508bfe65b6 240 float motor1_current_fwd_thd;
sayzyas 1:c3508bfe65b6 241 float motor1_current_rvs_thd;
sayzyas 1:c3508bfe65b6 242 float motor2_current_fwd_thd;
sayzyas 1:c3508bfe65b6 243 float motor2_current_rvs_thd;
sayzyas 0:653609b2a5cf 244 // --------------------------------------------------------------
sayzyas 0:653609b2a5cf 245 // Motor Current Read Function
sayzyas 0:653609b2a5cf 246 // Arg: Motor Number 1 - 3
sayzyas 0:653609b2a5cf 247 // --------------------------------------------------------------
sayzyas 0:653609b2a5cf 248 int read_motor_current( char motor_no, char motor_dir ){
sayzyas 1:c3508bfe65b6 249
sayzyas 1:c3508bfe65b6 250 float motor_current;
sayzyas 1:c3508bfe65b6 251 int i;
sayzyas 1:c3508bfe65b6 252 int mc_abs_pct = 0;
sayzyas 1:c3508bfe65b6 253 int mc_Threshold = 0;
sayzyas 0:653609b2a5cf 254
sayzyas 0:653609b2a5cf 255 if (motor_no == MOTOR_1 ){
sayzyas 2:aeff576d5013 256 motor_current = motor1_current_in.read()*100.0f;
sayzyas 0:653609b2a5cf 257 mc_abs_pct = abs((int)((motor_current - motor1_current_center_value)*100.0f));
sayzyas 1:c3508bfe65b6 258 DEBUG_DSPMCNT("Bd1> M1:%lf/%lf= [%03d%%] th:%3.2f< <%3.2f\t", motor_current, motor1_current_center_value, mc_abs_pct, motor1_current_rvs_thd, motor1_current_fwd_thd );
sayzyas 1:c3508bfe65b6 259 // DEBUG_DSPMCNT("Bd1> M1 motor_current center value : %lf%%\r\n", motor1_current_center_value );
sayzyas 1:c3508bfe65b6 260 // DEBUG_DSPMCNT("Bd1> M1 motor_current abs percentage : %d%%\r\n", mc_abs_pct );
sayzyas 1:c3508bfe65b6 261 for( i = 0; i < (mc_abs_pct/10); i++){
sayzyas 1:c3508bfe65b6 262 DEBUG_DSPMCNT(">");
sayzyas 1:c3508bfe65b6 263 }
sayzyas 1:c3508bfe65b6 264 DEBUG_DSPMCNT("\r\n");
sayzyas 1:c3508bfe65b6 265
sayzyas 0:653609b2a5cf 266 if( motor_dir == MOTOR_FWD ){
sayzyas 1:c3508bfe65b6 267 mc_Threshold = (int)motor1_current_fwd_thd;
sayzyas 0:653609b2a5cf 268 DEBUG_PRINT_L1("Bd1> Upper threshold: %d\r\n", mc_Threshold);
sayzyas 0:653609b2a5cf 269 }
sayzyas 0:653609b2a5cf 270 else{
sayzyas 1:c3508bfe65b6 271 mc_Threshold = (int)motor1_current_rvs_thd;
sayzyas 0:653609b2a5cf 272 DEBUG_PRINT_L1("Bd1> Lower threshold: %d\r\n", mc_Threshold);
sayzyas 0:653609b2a5cf 273 }
sayzyas 0:653609b2a5cf 274 if( mc_abs_pct > mc_Threshold ){
sayzyas 0:653609b2a5cf 275 DEBUG_PRINT_L1("Bd1> **** MC1 Over the Limit [%d] ****\r\n", motor1_lock_count );
sayzyas 0:653609b2a5cf 276 motor1_lock_count += 1;
sayzyas 0:653609b2a5cf 277 if( motor1_lock_count >= MC_LOCK_COUNT ){
sayzyas 0:653609b2a5cf 278 stdio_mutex.lock(); // Mutex Lock
sayzyas 0:653609b2a5cf 279 flg_motor1_lock = 1;
sayzyas 0:653609b2a5cf 280 stdio_mutex.unlock(); // Mutex Release
sayzyas 1:c3508bfe65b6 281 DEBUG_DSPMCNT("Bd1> #### MOTOR1 LOCK ! #### (%d)\r\n", flg_motor1_lock);
sayzyas 0:653609b2a5cf 282 }
sayzyas 0:653609b2a5cf 283 }
sayzyas 0:653609b2a5cf 284 else{
sayzyas 0:653609b2a5cf 285 DEBUG_PRINT_L1("Bd1> Pass\r\n");
sayzyas 0:653609b2a5cf 286 if( motor1_lock_count > 0 ) motor1_lock_count -= 1;
sayzyas 0:653609b2a5cf 287 else motor1_lock_count = 0;
sayzyas 0:653609b2a5cf 288 }
sayzyas 0:653609b2a5cf 289 }
sayzyas 0:653609b2a5cf 290 else if (motor_no == MOTOR_2 ){
sayzyas 2:aeff576d5013 291 motor_current = motor2_current_in.read()*100.0f;
sayzyas 0:653609b2a5cf 292 mc_abs_pct = abs((int)((motor_current - motor2_current_center_value)*100.0f));
sayzyas 1:c3508bfe65b6 293 DEBUG_DSPMCNT("Bd1> M2:%lf/%lf= [%03d%%] th:%3.2f< <%3.2f\t", motor_current, motor2_current_center_value, mc_abs_pct, motor2_current_rvs_thd, motor2_current_fwd_thd );
sayzyas 1:c3508bfe65b6 294 for( i = 0; i < (mc_abs_pct/10); i++){
sayzyas 1:c3508bfe65b6 295 DEBUG_DSPMCNT("=");
sayzyas 1:c3508bfe65b6 296 }
sayzyas 1:c3508bfe65b6 297 DEBUG_DSPMCNT("\r\n");
sayzyas 1:c3508bfe65b6 298
sayzyas 0:653609b2a5cf 299 if( motor_dir == MOTOR_FWD ){
sayzyas 1:c3508bfe65b6 300 mc_Threshold = (int)motor2_current_fwd_thd;
sayzyas 0:653609b2a5cf 301 }
sayzyas 0:653609b2a5cf 302 else{
sayzyas 1:c3508bfe65b6 303 mc_Threshold = (int)motor2_current_rvs_thd;
sayzyas 0:653609b2a5cf 304 }
sayzyas 0:653609b2a5cf 305 if( mc_abs_pct > mc_Threshold ){
sayzyas 0:653609b2a5cf 306 DEBUG_PRINT_L1("Bd1> **** MC2 Over the Limit [%d] ****\r\n", motor2_lock_count );
sayzyas 0:653609b2a5cf 307 motor2_lock_count += 1;
sayzyas 0:653609b2a5cf 308 if( motor2_lock_count >= MC_LOCK_COUNT ){
sayzyas 0:653609b2a5cf 309 stdio_mutex.lock(); // Mutex Lock
sayzyas 0:653609b2a5cf 310 flg_motor2_lock = 1;
sayzyas 0:653609b2a5cf 311 stdio_mutex.unlock(); // Mutex Release
sayzyas 1:c3508bfe65b6 312 DEBUG_DSPMCNT("Bd1> #### MOTOR2 LOCK ! #### (%d)\r\n", flg_motor2_lock);
sayzyas 0:653609b2a5cf 313 }
sayzyas 0:653609b2a5cf 314 }
sayzyas 0:653609b2a5cf 315 else{
sayzyas 0:653609b2a5cf 316 if( motor2_lock_count > 0 ) motor2_lock_count -= 1;
sayzyas 0:653609b2a5cf 317 else motor2_lock_count = 0;
sayzyas 0:653609b2a5cf 318 }
sayzyas 0:653609b2a5cf 319 }
sayzyas 0:653609b2a5cf 320 return mc_abs_pct;
sayzyas 0:653609b2a5cf 321 }
sayzyas 0:653609b2a5cf 322
sayzyas 0:653609b2a5cf 323
sayzyas 0:653609b2a5cf 324 Timer t;
sayzyas 0:653609b2a5cf 325
sayzyas 0:653609b2a5cf 326 uint8_t motor_control_flag1 = 0x00;
sayzyas 0:653609b2a5cf 327 uint8_t motor_control_flag2 = 0x00;
sayzyas 0:653609b2a5cf 328
sayzyas 0:653609b2a5cf 329 // --------------------------------------------------------------------------
sayzyas 0:653609b2a5cf 330 // Motor Control Function
sayzyas 0:653609b2a5cf 331 // Motor on off by microcontroller
sayzyas 0:653609b2a5cf 332 // motor1_out : motor 1 on /off
sayzyas 0:653609b2a5cf 333 // motor1_dir : motor 1 direction
sayzyas 0:653609b2a5cf 334 // motor2_out : motor 2 on / off
sayzyas 0:653609b2a5cf 335 // motor2_dir : motor 2 direction
sayzyas 0:653609b2a5cf 336 // --------------------------------------------------------------------------
sayzyas 0:653609b2a5cf 337 // "motor_control_flag" bit function
sayzyas 0:653609b2a5cf 338 // bit
sayzyas 0:653609b2a5cf 339 // ------
sayzyas 0:653609b2a5cf 340 // 7 : motor number : upper nibble
sayzyas 0:653609b2a5cf 341 // 6 : 0 : no motor
sayzyas 0:653609b2a5cf 342 // 5 : 0 - 15 : motor number
sayzyas 0:653609b2a5cf 343 // 4 :
sayzyas 0:653609b2a5cf 344 // ------
sayzyas 0:653609b2a5cf 345 // 3 : motor on/off, 0=off 1=on
sayzyas 0:653609b2a5cf 346 // 2 :
sayzyas 0:653609b2a5cf 347 // 1 :
sayzyas 0:653609b2a5cf 348 // 0 : motor direction, 0=fwd 1=rev
sayzyas 0:653609b2a5cf 349 void motor1_ctrl(
sayzyas 0:653609b2a5cf 350 uint32_t onoff, // Motor ON/OFF
sayzyas 0:653609b2a5cf 351 uint32_t dir // Motor rotation direction
sayzyas 0:653609b2a5cf 352 ){
sayzyas 0:653609b2a5cf 353 // DEBUG_PRINT( "Motor No.%02d, [onoff:%d], [dir:%d]\r\n", motor_no, onoff, dir );
sayzyas 0:653609b2a5cf 354 led1 = !onoff;
sayzyas 0:653609b2a5cf 355 motor_control_flag1 &= 0x0F;
sayzyas 0:653609b2a5cf 356 motor_control_flag1 |= 0x10;
sayzyas 0:653609b2a5cf 357 if( dir == FLG_MOTOR_DIR_FWD ){
sayzyas 0:653609b2a5cf 358 // motor1_dir = 0; // 0:Fwd, 1:Rvs
sayzyas 0:653609b2a5cf 359 motor_control_flag1 &= 0xFE;
sayzyas 0:653609b2a5cf 360 }
sayzyas 0:653609b2a5cf 361 else if( dir == FLG_MOTOR_DIR_RVS ){
sayzyas 0:653609b2a5cf 362 // motor1_dir = 1; // 0:Fwd, 1:Rvs
sayzyas 0:653609b2a5cf 363 motor_control_flag1 |= 0x01;
sayzyas 0:653609b2a5cf 364 }
sayzyas 0:653609b2a5cf 365 // stdio_mutex.lock(); // Mutex Lock
sayzyas 0:653609b2a5cf 366 if(( !flg_motor1_lock )&&( onoff )){ // Case of motor unlock by timer
sayzyas 0:653609b2a5cf 367 // motor1_out = 1; // Motor1 on
sayzyas 0:653609b2a5cf 368 motor_control_flag1 |= 0x08;
sayzyas 0:653609b2a5cf 369 }
sayzyas 0:653609b2a5cf 370 else{
sayzyas 0:653609b2a5cf 371 // motor1_out = 0; // Motor1 off
sayzyas 0:653609b2a5cf 372 motor_control_flag1 &= 0xF7;
sayzyas 0:653609b2a5cf 373 }
sayzyas 0:653609b2a5cf 374 // stdio_mutex.unlock(); // Mutex Release
sayzyas 0:653609b2a5cf 375 }
sayzyas 0:653609b2a5cf 376
sayzyas 0:653609b2a5cf 377 void motor2_ctrl(
sayzyas 0:653609b2a5cf 378 uint32_t onoff, // Motor ON/OFF
sayzyas 0:653609b2a5cf 379 uint32_t dir // Motor rotaion direction
sayzyas 0:653609b2a5cf 380 ){
sayzyas 0:653609b2a5cf 381 // DEBUG_PRINT( "Motor No.%02d, [onoff:%d], [dir:%d]\r\n", motor_no, onoff, dir );
sayzyas 0:653609b2a5cf 382 led1 = !onoff;
sayzyas 0:653609b2a5cf 383 motor_control_flag2 &= 0x0F;
sayzyas 0:653609b2a5cf 384 motor_control_flag2 |= 0x20;
sayzyas 0:653609b2a5cf 385 if( dir == FLG_MOTOR_DIR_FWD ){
sayzyas 0:653609b2a5cf 386 // motor2_dir = 0; // 0:Fwd, 1:Rvs
sayzyas 0:653609b2a5cf 387 motor_control_flag2 &= 0xFE;
sayzyas 0:653609b2a5cf 388 }
sayzyas 0:653609b2a5cf 389 else if( dir == FLG_MOTOR_DIR_RVS ){
sayzyas 0:653609b2a5cf 390 // motor2_dir = 1; // 0:Fwd, 1:Rvs
sayzyas 0:653609b2a5cf 391 motor_control_flag2 |= 0x01;
sayzyas 0:653609b2a5cf 392 }
sayzyas 0:653609b2a5cf 393 // stdio_mutex.lock(); // Mutex Lock
sayzyas 0:653609b2a5cf 394 if(( !flg_motor2_lock )&&( onoff )){ // Case of motor unlock by timer
sayzyas 0:653609b2a5cf 395 // motor2_out = 1; // Motor2 on
sayzyas 0:653609b2a5cf 396 motor_control_flag2 |= 0x08;
sayzyas 0:653609b2a5cf 397 }
sayzyas 0:653609b2a5cf 398 else{
sayzyas 0:653609b2a5cf 399 // motor2_out = 0; // Motor2 off
sayzyas 0:653609b2a5cf 400 motor_control_flag2 &= 0xF7;
sayzyas 0:653609b2a5cf 401 }
sayzyas 0:653609b2a5cf 402 // stdio_mutex.unlock(); // Mutex Release
sayzyas 0:653609b2a5cf 403 }
sayzyas 0:653609b2a5cf 404
sayzyas 0:653609b2a5cf 405
sayzyas 0:653609b2a5cf 406 int limitSw_Sts = 0;
sayzyas 0:653609b2a5cf 407 // -----------------------------------------------------
sayzyas 0:653609b2a5cf 408 // Motor COntrol Task: loop forever !
sayzyas 0:653609b2a5cf 409 // For RoboteQ motor controller
sayzyas 0:653609b2a5cf 410 // Note. Motor control command should always send to
sayzyas 0:653609b2a5cf 411 // RoboteQ controllet, so following should be
sayzyas 0:653609b2a5cf 412 // done by thread.
sayzyas 0:653609b2a5cf 413 // -----------------------------------------------------
sayzyas 0:653609b2a5cf 414 // motor_control_flag bit function
sayzyas 0:653609b2a5cf 415 // bit
sayzyas 0:653609b2a5cf 416 // ------
sayzyas 0:653609b2a5cf 417 // 7 : motor number : upper nibble
sayzyas 0:653609b2a5cf 418 // 6 : 0 : no motor
sayzyas 0:653609b2a5cf 419 // 5 : 0 - 15 : motor number
sayzyas 0:653609b2a5cf 420 // 4 :
sayzyas 0:653609b2a5cf 421 // ------
sayzyas 0:653609b2a5cf 422 // 3 : motor on/off, 0=off 1=on
sayzyas 0:653609b2a5cf 423 // 2 :
sayzyas 0:653609b2a5cf 424 // 1 :
sayzyas 0:653609b2a5cf 425 // 0 : motor direction, 0=fwd 1=rev
sayzyas 0:653609b2a5cf 426 void RoboteQM1DrvCtrl_task( void const *){
sayzyas 0:653609b2a5cf 427 while(1){
sayzyas 1:c3508bfe65b6 428
sayzyas 1:c3508bfe65b6 429
sayzyas 1:c3508bfe65b6 430 if( flg_motor1_lock ){
sayzyas 2:aeff576d5013 431 DEBUG_PRINT_L1("Bd1> M1 LOCK\r\n");
sayzyas 0:653609b2a5cf 432 sdc2130.printf( "!G 1 0\r" ); // Motor off
sayzyas 0:653609b2a5cf 433 }
sayzyas 0:653609b2a5cf 434 else{
sayzyas 0:653609b2a5cf 435 // DEBUG_PRINT("Motor Speed : %d\r\n", motor1_speed);
sayzyas 0:653609b2a5cf 436 if(( motor_control_flag1 & 0x08 )){ // motor on
sayzyas 0:653609b2a5cf 437 if(!(motor_control_flag1 & 0x01)){ // motor dir fwd
sayzyas 0:653609b2a5cf 438 if((motor_control_flag1 & 0xF0) == 0x10){ // motor 1
sayzyas 0:653609b2a5cf 439 DEBUG_PRINT_L1("Bd1> RoboteQ <-- M1 FWD (%d)\r\n", motor1_speed );
sayzyas 0:653609b2a5cf 440 stdio_mutex.lock(); // Mutex Lock
sayzyas 0:653609b2a5cf 441 // sdc2130.printf( "!AC 1 10000" );
sayzyas 0:653609b2a5cf 442 sdc2130.printf( "!G 1 %d\r", motor1_speed*10 ); // Motor on reverse 100.0%
sayzyas 0:653609b2a5cf 443 // sdc2130.printf( "!G 1 %d\r", motor1_speed*10 ); // Motor on reverse 100.0%
sayzyas 0:653609b2a5cf 444 limitSw_Sts &= 0xFE;
sayzyas 0:653609b2a5cf 445 stdio_mutex.unlock(); // Mutex Lock
sayzyas 0:653609b2a5cf 446 }
sayzyas 0:653609b2a5cf 447 }
sayzyas 0:653609b2a5cf 448 else{ // motor dir rev
sayzyas 0:653609b2a5cf 449 if((motor_control_flag1 & 0xF0) == 0x10){ // motor 1
sayzyas 0:653609b2a5cf 450 stdio_mutex.lock(); // Mutex Lock
sayzyas 1:c3508bfe65b6 451 if(( limit_sw_rf == 0 )&&(flg_lsw_valid == true )){
sayzyas 0:653609b2a5cf 452 DEBUG_PRINT_L1("Limit switch ON (M1 reverse) \r\n");
sayzyas 0:653609b2a5cf 453 DEBUG_PRINT_L4("Bd1> RoboteQ <-- M1 RVS (0)\r\n");
sayzyas 0:653609b2a5cf 454 sdc2130.printf("!G 1 0\r"); // Motor off
sayzyas 0:653609b2a5cf 455 limitSw_Sts |= 0x01;
sayzyas 0:653609b2a5cf 456 }
sayzyas 0:653609b2a5cf 457 else{
sayzyas 0:653609b2a5cf 458 DEBUG_PRINT_L1("Bd1> RoboteQ <-- M1 RVS (%d)\r\n", motor1_speed*-1 );
sayzyas 0:653609b2a5cf 459 sdc2130.printf( "!G 1 %d\r", (motor1_speed*-1)*10 ); // Motor on reverse 100.0%
sayzyas 0:653609b2a5cf 460 limitSw_Sts &= 0xFE;
sayzyas 0:653609b2a5cf 461 }
sayzyas 0:653609b2a5cf 462 stdio_mutex.unlock(); // Mutex Lock
sayzyas 0:653609b2a5cf 463 }
sayzyas 0:653609b2a5cf 464 }
sayzyas 0:653609b2a5cf 465 }
sayzyas 0:653609b2a5cf 466 else{
sayzyas 2:aeff576d5013 467 // DEBUG_PRINT_L1("Bd1> M1 OFF\r\n");
sayzyas 0:653609b2a5cf 468 sdc2130.printf( "!G 1 0\r" ); // Motor off
sayzyas 1:c3508bfe65b6 469 // Thread::wait(30);
sayzyas 2:aeff576d5013 470 // calb_motor_current(1, motor1_current_in.read()*100.0f);
sayzyas 0:653609b2a5cf 471 }
sayzyas 0:653609b2a5cf 472 }
sayzyas 0:653609b2a5cf 473 Thread::wait(7);
sayzyas 0:653609b2a5cf 474 }
sayzyas 0:653609b2a5cf 475 }
sayzyas 0:653609b2a5cf 476
sayzyas 0:653609b2a5cf 477 void RoboteQM2DrvCtrl_task( void const *){
sayzyas 0:653609b2a5cf 478 while(1){
sayzyas 0:653609b2a5cf 479 if( flg_motor2_lock ){
sayzyas 2:aeff576d5013 480 DEBUG_PRINT_L1("Bd1> M2 LOCK\r\n");
sayzyas 0:653609b2a5cf 481 sdc2130.printf( "!G 2 0\r" ); // Motor off
sayzyas 0:653609b2a5cf 482 }
sayzyas 0:653609b2a5cf 483 else{
sayzyas 0:653609b2a5cf 484 // DEBUG_PRINT("Motor Speed : %d\r\n", motor1_speed);
sayzyas 0:653609b2a5cf 485 if(( motor_control_flag2 & 0x08 )){ // motor on
sayzyas 0:653609b2a5cf 486 if(!(motor_control_flag2 & 0x01)){ // motor dir fwd
sayzyas 0:653609b2a5cf 487 if((motor_control_flag2 & 0xF0) == 0x20){ // motor 2
sayzyas 0:653609b2a5cf 488 DEBUG_PRINT_L1("Bd1> RoboteQ <-- M2 FWD (%d)\r\n", motor2_speed );
sayzyas 0:653609b2a5cf 489 // sdc2130.printf( "!G 2 1000\r" ); // Motor on reverse 100.0%
sayzyas 0:653609b2a5cf 490 stdio_mutex.lock(); // Mutex Lock
sayzyas 0:653609b2a5cf 491 sdc2130.printf( "!G 2 %d\r", motor2_speed*10 ); // Motor on reverse 100.0%
sayzyas 0:653609b2a5cf 492 // sdc2130.printf( "!G 2 %d\r", motor2_speed*10 ); // Motor on reverse 100.0%
sayzyas 0:653609b2a5cf 493 limitSw_Sts &= 0xFD;
sayzyas 0:653609b2a5cf 494 stdio_mutex.unlock(); // Mutex Lock
sayzyas 0:653609b2a5cf 495 }
sayzyas 0:653609b2a5cf 496 }
sayzyas 0:653609b2a5cf 497 else{ // motor dir rev
sayzyas 0:653609b2a5cf 498 if((motor_control_flag2 & 0xF0) == 0x20){ // motor 2
sayzyas 0:653609b2a5cf 499 // sdc2130.printf( "!G 2 -1000\r" ); // Motor on reverse 100.0%
sayzyas 0:653609b2a5cf 500 stdio_mutex.lock(); // Mutex Lock
sayzyas 1:c3508bfe65b6 501 if(( limit_sw_lb == 0 )&&(flg_lsw_valid == true )){
sayzyas 0:653609b2a5cf 502 DEBUG_PRINT_L1("Limit switch ON (M2 reverse) \r\n");
sayzyas 0:653609b2a5cf 503 DEBUG_PRINT_L4("Bd1> RoboteQ <-- M2 RVS (0)\r\n");
sayzyas 0:653609b2a5cf 504 sdc2130.printf("!G 2 0\r"); // Motor off
sayzyas 0:653609b2a5cf 505 limitSw_Sts |= 0x02;
sayzyas 0:653609b2a5cf 506 }
sayzyas 0:653609b2a5cf 507 else{
sayzyas 0:653609b2a5cf 508 DEBUG_PRINT_L1("Bd1> RoboteQ <-- M2 RVS (%d)\r\n", motor2_speed*-1);
sayzyas 0:653609b2a5cf 509 sdc2130.printf("!G 2 %d\r", (motor2_speed*-1)*10); // Motor on reverse 100.0%
sayzyas 0:653609b2a5cf 510 limitSw_Sts &= 0xFD;
sayzyas 0:653609b2a5cf 511 }
sayzyas 0:653609b2a5cf 512 stdio_mutex.unlock(); // Mutex Lock
sayzyas 0:653609b2a5cf 513 }
sayzyas 0:653609b2a5cf 514 }
sayzyas 0:653609b2a5cf 515 }
sayzyas 0:653609b2a5cf 516 else{
sayzyas 2:aeff576d5013 517 // DEBUG_PRINT_L1("Bd1> M2 OFF\r\n");
sayzyas 0:653609b2a5cf 518 sdc2130.printf( "!G 2 0\r" ); // Motor off
sayzyas 1:c3508bfe65b6 519 // Thread::wait(30);
sayzyas 2:aeff576d5013 520 // calb_motor_current(2, motor2_current_in.read()*100.0f);
sayzyas 0:653609b2a5cf 521 }
sayzyas 0:653609b2a5cf 522 }
sayzyas 0:653609b2a5cf 523 Thread::wait(1);
sayzyas 0:653609b2a5cf 524 }
sayzyas 0:653609b2a5cf 525 }
sayzyas 0:653609b2a5cf 526
sayzyas 0:653609b2a5cf 527 int motor1_current_pct;
sayzyas 0:653609b2a5cf 528 int motor2_current_pct;
sayzyas 0:653609b2a5cf 529 // ========================================================================
sayzyas 0:653609b2a5cf 530 // Thread: Motor Control Task
sayzyas 0:653609b2a5cf 531 // ========================================================================
sayzyas 0:653609b2a5cf 532 void motor_control_task(void const *) {
sayzyas 0:653609b2a5cf 533
sayzyas 1:c3508bfe65b6 534 char buf[14]; // command buffer
sayzyas 1:c3508bfe65b6 535
sayzyas 2:aeff576d5013 536 char res_msg2[6];
sayzyas 0:653609b2a5cf 537
sayzyas 0:653609b2a5cf 538 slave.address(i2c_saddress);
sayzyas 0:653609b2a5cf 539
sayzyas 0:653609b2a5cf 540 /*
sayzyas 0:653609b2a5cf 541 * I2C Access
sayzyas 0:653609b2a5cf 542 */
sayzyas 0:653609b2a5cf 543 while(1) {
sayzyas 0:653609b2a5cf 544 int i = slave.receive();
sayzyas 0:653609b2a5cf 545 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:653609b2a5cf 546
sayzyas 0:653609b2a5cf 547 // DEBUG_PRINT_L3("Bd1> Slave Received\r\n");
sayzyas 0:653609b2a5cf 548 switch (i) {
sayzyas 0:653609b2a5cf 549 case I2CSlave::NoData:
sayzyas 0:653609b2a5cf 550 // DEBUG_PRINT_L3("Bd1> NoData(the slave has not been addressed)\r\n");
sayzyas 0:653609b2a5cf 551 // motor_current_pct = read_motor_current(2);
sayzyas 0:653609b2a5cf 552 break;
sayzyas 0:653609b2a5cf 553 case I2CSlave::ReadAddressed:
sayzyas 2:aeff576d5013 554 // Send Motor Current here to host controller.
sayzyas 2:aeff576d5013 555 res_msg2[0] = (char)(motor1_current_pct & 0xFF);
sayzyas 2:aeff576d5013 556 res_msg2[1] = (char)(motor2_current_pct & 0xFF);
sayzyas 2:aeff576d5013 557 res_msg2[3] = (char)(limitSw_Sts);
sayzyas 2:aeff576d5013 558 if( flg_motor1_lockcup == true ){
sayzyas 2:aeff576d5013 559 res_msg2[4] = '1'; // Motor 1 Lock !!
sayzyas 2:aeff576d5013 560 }
sayzyas 2:aeff576d5013 561 else if( flg_motor2_lockcup == true ){
sayzyas 2:aeff576d5013 562 res_msg2[4] = '2'; // Motor 2 Lock !!
sayzyas 2:aeff576d5013 563 }
sayzyas 2:aeff576d5013 564 else{
sayzyas 2:aeff576d5013 565 res_msg2[4] = 'S';
sayzyas 2:aeff576d5013 566 }
sayzyas 2:aeff576d5013 567 res_msg2[5] = '\0';
sayzyas 1:c3508bfe65b6 568
sayzyas 2:aeff576d5013 569 DEBUG_PRINT_L3("\t\t motor1_current_pct = %d\r\n", motor1_current_pct );
sayzyas 2:aeff576d5013 570 DEBUG_PRINT_L3("\t\t motor2_current_pct = %d\r\n", motor2_current_pct );
sayzyas 2:aeff576d5013 571 DEBUG_PRINT_L3("\t\t imit sw = 0x%02x\r\n", limitSw_Sts );
sayzyas 2:aeff576d5013 572 if( flg_motor1_lockcup == true ){
sayzyas 2:aeff576d5013 573 res_msg2[2] = 0x88;
sayzyas 2:aeff576d5013 574 DEBUG_PRINT_L3(" 0: [%d]\r\n", res_msg2[0] );
sayzyas 2:aeff576d5013 575 DEBUG_PRINT_L3(" 1: [%d]\r\n", res_msg2[1] );
sayzyas 2:aeff576d5013 576 DEBUG_PRINT_L3(" 2: [0x%02x]\r\n", res_msg2[2] );
sayzyas 2:aeff576d5013 577 DEBUG_PRINT_L3(" 3: [%d]\r\n", res_msg2[3] );
sayzyas 2:aeff576d5013 578 DEBUG_PRINT_L3(" 4: [%c]\r\n", res_msg2[4] );
sayzyas 2:aeff576d5013 579 }
sayzyas 2:aeff576d5013 580 else{
sayzyas 2:aeff576d5013 581 res_msg2[2] = 0x00;
sayzyas 2:aeff576d5013 582 }
sayzyas 2:aeff576d5013 583 slave.write(res_msg2, 6); // Includes null char
sayzyas 2:aeff576d5013 584 break;
sayzyas 0:653609b2a5cf 585 case I2CSlave::WriteGeneral:
sayzyas 0:653609b2a5cf 586 // DEBUG_PRINT_L3("Bd1> the master is writing to all slave\r\n");
sayzyas 0:653609b2a5cf 587 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:653609b2a5cf 588 // DEBUG_PRINT_L3("Bd1> Read G: %s\r\n", buf);
sayzyas 0:653609b2a5cf 589 break;
sayzyas 0:653609b2a5cf 590 case I2CSlave::WriteAddressed:
sayzyas 1:c3508bfe65b6 591 motor1_current_fwd_thd = (float)(((buf[I2C_CP_M1_FWD_CNTTH_U]<<8)&0xFF00)|(buf[I2C_CP_M1_FWD_CNTTH_L]&0xFF)); // <-- New!
sayzyas 1:c3508bfe65b6 592 motor1_current_rvs_thd = (float)(((buf[I2C_CP_M1_RVS_CNTTH_U]<<8)&0xFF00)|(buf[I2C_CP_M1_RVS_CNTTH_L]&0xFF)); // <-- New!
sayzyas 1:c3508bfe65b6 593 motor2_current_fwd_thd = (float)(((buf[I2C_CP_M2_FWD_CNTTH_U]<<8)&0xFF00)|(buf[I2C_CP_M2_FWD_CNTTH_L]&0xFF)); // <-- New!
sayzyas 1:c3508bfe65b6 594 motor2_current_rvs_thd = (float)(((buf[I2C_CP_M2_RVS_CNTTH_U]<<8)&0xFF00)|(buf[I2C_CP_M2_RVS_CNTTH_L]&0xFF)); // <-- New!
sayzyas 2:aeff576d5013 595 // Set Motor Speed here: Speed is set by Main Controller via I2C command right now.
sayzyas 2:aeff576d5013 596 // In case of Winch motor controller, Speed is set by analog data from A0 and A1 <-- It's for evaluation
sayzyas 2:aeff576d5013 597 // for devalopment the "Fix position falling winch".
sayzyas 2:aeff576d5013 598 if( buf[I2C_CP_M1_DIR] == MOTOR_STP )
sayzyas 2:aeff576d5013 599 {
sayzyas 2:aeff576d5013 600 motor1_speed = 0;
sayzyas 2:aeff576d5013 601 }
sayzyas 2:aeff576d5013 602 else
sayzyas 2:aeff576d5013 603 {
sayzyas 2:aeff576d5013 604 if(( buf[I2C_CP_M1_DIR] == MOTOR_FWD )||( buf[I2C_CP_M1_DIR] == MOTOR_RVS ))
sayzyas 2:aeff576d5013 605 {
sayzyas 2:aeff576d5013 606 motor1_speed = buf[I2C_CP_M1_SPEED];
sayzyas 2:aeff576d5013 607 }
sayzyas 2:aeff576d5013 608 }
sayzyas 2:aeff576d5013 609 if( buf[I2C_CP_M2_DIR] == MOTOR_STP )
sayzyas 2:aeff576d5013 610 {
sayzyas 2:aeff576d5013 611 motor2_speed = 0;
sayzyas 2:aeff576d5013 612 }
sayzyas 2:aeff576d5013 613 else
sayzyas 2:aeff576d5013 614 {
sayzyas 2:aeff576d5013 615 if(( buf[I2C_CP_M2_DIR] == MOTOR_FWD )||( buf[I2C_CP_M2_DIR] == MOTOR_RVS ))
sayzyas 2:aeff576d5013 616 {
sayzyas 2:aeff576d5013 617 motor2_speed = buf[I2C_CP_M2_SPEED];
sayzyas 2:aeff576d5013 618 }
sayzyas 2:aeff576d5013 619 }
sayzyas 2:aeff576d5013 620 // DEBUG_PRINT_L1( "Bd1> GOT MOTOR SPEED: M1[ %d ] ", motor1_speed );
sayzyas 2:aeff576d5013 621 // DEBUG_PRINT_L1( "M2[ %d ] \r\n", motor2_speed );
sayzyas 2:aeff576d5013 622
sayzyas 1:c3508bfe65b6 623 if( buf[I2C_CP_COMMAND] == 'V' ){
sayzyas 1:c3508bfe65b6 624 flg_lsw_valid = true;
sayzyas 0:653609b2a5cf 625 }
sayzyas 1:c3508bfe65b6 626 else{
sayzyas 1:c3508bfe65b6 627 flg_lsw_valid = false;
sayzyas 0:653609b2a5cf 628 }
sayzyas 1:c3508bfe65b6 629
sayzyas 1:c3508bfe65b6 630 if( buf[I2C_CP_M2_DIR] == MOTOR_STP ){
sayzyas 0:653609b2a5cf 631 //DEBUG_PRINT_L1("M2 Off_1\r\n");
sayzyas 0:653609b2a5cf 632 motor2_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
sayzyas 0:653609b2a5cf 633 t.stop();
sayzyas 0:653609b2a5cf 634 motor2_lock_count = 0;
sayzyas 0:653609b2a5cf 635 motor2_current_pct = 0;
sayzyas 0:653609b2a5cf 636 flg_motor2_lockcup = false;
sayzyas 0:653609b2a5cf 637 stdio_mutex.lock();
sayzyas 0:653609b2a5cf 638 flg_motor2_lock = 0;
sayzyas 0:653609b2a5cf 639 stdio_mutex.unlock();
sayzyas 0:653609b2a5cf 640 }
sayzyas 1:c3508bfe65b6 641 else if(( buf[I2C_CP_M2_DIR] == MOTOR_FWD )&&(flg_motor2_lockcup == false)){ // MOTOR2 FORWARD
sayzyas 0:653609b2a5cf 642 motor2_ctrl( FLG_MOTOR_ON, FLG_MOTOR_DIR_FWD );
sayzyas 0:653609b2a5cf 643 motor2_current_pct = read_motor_current( MOTOR_2, MOTOR_FWD );
sayzyas 0:653609b2a5cf 644 }
sayzyas 1:c3508bfe65b6 645 else if(( buf[I2C_CP_M2_DIR] == MOTOR_RVS )&&(flg_motor2_lockcup == false)){ // MOTOR2 REVERSE
sayzyas 0:653609b2a5cf 646 motor2_ctrl( FLG_MOTOR_ON, FLG_MOTOR_DIR_RVS );
sayzyas 0:653609b2a5cf 647 motor2_current_pct = read_motor_current( MOTOR_2, MOTOR_RVS );
sayzyas 0:653609b2a5cf 648 }
sayzyas 2:aeff576d5013 649 // 2016.11.07: following section commented out ! :
sayzyas 2:aeff576d5013 650 // If this section is velidate, then suddnly motor stopped because motor on
sayzyas 2:aeff576d5013 651 // flag is cleared. this is bug !!
sayzyas 2:aeff576d5013 652 /*
sayzyas 0:653609b2a5cf 653 else{
sayzyas 0:653609b2a5cf 654 //DEBUG_PRINT_L1("M2 Off_2\r\n");
sayzyas 0:653609b2a5cf 655 motor2_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
sayzyas 0:653609b2a5cf 656 motor2_lock_count = 0;
sayzyas 0:653609b2a5cf 657 motor2_current_pct = 0;
sayzyas 0:653609b2a5cf 658 t.stop();
sayzyas 0:653609b2a5cf 659 stdio_mutex.lock();
sayzyas 0:653609b2a5cf 660 flg_motor2_lock = 0;
sayzyas 0:653609b2a5cf 661 stdio_mutex.unlock();
sayzyas 0:653609b2a5cf 662 }
sayzyas 2:aeff576d5013 663 */
sayzyas 1:c3508bfe65b6 664 if( buf[I2C_CP_M1_DIR] == MOTOR_STP ){
sayzyas 0:653609b2a5cf 665 //DEBUG_PRINT_L1("M1 Off_1\r\n");
sayzyas 0:653609b2a5cf 666 motor1_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
sayzyas 0:653609b2a5cf 667 t.stop();
sayzyas 0:653609b2a5cf 668 motor1_current_pct = 0;
sayzyas 0:653609b2a5cf 669 motor1_lock_count = 0;
sayzyas 0:653609b2a5cf 670 flg_motor1_lockcup = false;
sayzyas 0:653609b2a5cf 671 stdio_mutex.lock();
sayzyas 0:653609b2a5cf 672 flg_motor1_lock = 0;
sayzyas 0:653609b2a5cf 673 stdio_mutex.unlock();
sayzyas 0:653609b2a5cf 674 }
sayzyas 1:c3508bfe65b6 675 else if(( buf[I2C_CP_M1_DIR] == MOTOR_FWD)&&(flg_motor1_lockcup == false)){ // MOTOR1 FORWARD
sayzyas 0:653609b2a5cf 676 motor1_ctrl( FLG_MOTOR_ON, FLG_MOTOR_DIR_FWD );
sayzyas 0:653609b2a5cf 677 motor1_current_pct = read_motor_current( MOTOR_1, MOTOR_FWD );
sayzyas 0:653609b2a5cf 678 }
sayzyas 1:c3508bfe65b6 679 else if(( buf[I2C_CP_M1_DIR] == MOTOR_RVS )&&(flg_motor1_lockcup == false)){ // MOTOR1 REVERSE
sayzyas 0:653609b2a5cf 680 motor1_ctrl( FLG_MOTOR_ON, FLG_MOTOR_DIR_RVS );
sayzyas 0:653609b2a5cf 681 motor1_current_pct = read_motor_current( MOTOR_1, MOTOR_RVS );
sayzyas 0:653609b2a5cf 682 }
sayzyas 2:aeff576d5013 683 // 2016.11.07: following section commented out ! :
sayzyas 2:aeff576d5013 684 // If this section is velidate, then suddnly motor stopped because motor on
sayzyas 2:aeff576d5013 685 // flag is cleared. this is bug !!
sayzyas 2:aeff576d5013 686 /* else{
sayzyas 0:653609b2a5cf 687 motor1_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
sayzyas 0:653609b2a5cf 688 motor1_current_pct = 0;
sayzyas 0:653609b2a5cf 689 motor1_lock_count = 0;
sayzyas 0:653609b2a5cf 690 t.stop();
sayzyas 0:653609b2a5cf 691 stdio_mutex.lock();
sayzyas 1:c3508bfe65b6 692 // pc.printf("%%%%%%%%\r\n");
sayzyas 0:653609b2a5cf 693 flg_motor1_lock = 0;
sayzyas 0:653609b2a5cf 694 stdio_mutex.unlock();
sayzyas 0:653609b2a5cf 695 }
sayzyas 2:aeff576d5013 696 */
sayzyas 2:aeff576d5013 697 break;
sayzyas 0:653609b2a5cf 698 }
sayzyas 1:c3508bfe65b6 699 for(int i = 0; i < NumberOfI2CCommand; i++) buf[i] = 0; // Clear buffer
sayzyas 0:653609b2a5cf 700
sayzyas 0:653609b2a5cf 701
sayzyas 0:653609b2a5cf 702 stdio_mutex.lock();
sayzyas 0:653609b2a5cf 703
sayzyas 2:aeff576d5013 704 #ifdef __SEPARATE_TYPR__
sayzyas 2:aeff576d5013 705 if ( flg_motor1_lock == 1 ){
sayzyas 2:aeff576d5013 706 t.start();
sayzyas 2:aeff576d5013 707 DEBUG_PRINT_L1( "Bd1> MOTOR1 lock time start\r\n" );
sayzyas 2:aeff576d5013 708 if( t.read_ms() > 100 ){ // 1sec
sayzyas 2:aeff576d5013 709 DEBUG_PRINT_L1("Bd1> MOTOR1 STOP lock with time up: %d \r\n", flg_motor1_lock );
sayzyas 2:aeff576d5013 710 // DEBUG_PRINT_L1("Bd1> MOTOR2 STOP lock with time up: %d \r\n", flg_motor2_lock );
sayzyas 2:aeff576d5013 711 motor1_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
sayzyas 2:aeff576d5013 712 // motor2_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
sayzyas 2:aeff576d5013 713 flg_motor1_lockcup = true;
sayzyas 2:aeff576d5013 714 // flg_motor2_lockcup = true;
sayzyas 2:aeff576d5013 715 t.stop();
sayzyas 2:aeff576d5013 716 }
sayzyas 2:aeff576d5013 717 }
sayzyas 2:aeff576d5013 718
sayzyas 2:aeff576d5013 719 if ( flg_motor2_lock == 1 ){
sayzyas 2:aeff576d5013 720 t.start();
sayzyas 2:aeff576d5013 721 DEBUG_PRINT_L1( "Bd1> MOTOR2 lock time start\r\n" );
sayzyas 2:aeff576d5013 722 if( t.read_ms() > 100 ){ // 1sec
sayzyas 2:aeff576d5013 723 // DEBUG_PRINT_L1("Bd1> MOTOR1 STOP lock with time up: %d \r\n", flg_motor1_lock );
sayzyas 2:aeff576d5013 724 DEBUG_PRINT_L1("Bd1> MOTOR2 STOP lock with time up: %d \r\n", flg_motor2_lock );
sayzyas 2:aeff576d5013 725 // motor1_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
sayzyas 2:aeff576d5013 726 motor2_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
sayzyas 2:aeff576d5013 727 // flg_motor1_lockcup = true;
sayzyas 2:aeff576d5013 728 flg_motor2_lockcup = true;
sayzyas 2:aeff576d5013 729 t.stop();
sayzyas 2:aeff576d5013 730 }
sayzyas 2:aeff576d5013 731 }
sayzyas 2:aeff576d5013 732 #else
sayzyas 2:aeff576d5013 733 /*
sayzyas 2:aeff576d5013 734 Motor Lock detection and flag on part
sayzyas 2:aeff576d5013 735 -------------------------------------
sayzyas 2:aeff576d5013 736 Currently, in case of each motor lock, then both motor will stop
sayzyas 2:aeff576d5013 737 and both flag on.
sayzyas 2:aeff576d5013 738 Therefore, PC S/W can detect motor lock only if only one side flag check.
sayzyas 2:aeff576d5013 739 */
sayzyas 0:653609b2a5cf 740 if (( flg_motor1_lock == 1 )||( flg_motor2_lock == 1 )){
sayzyas 0:653609b2a5cf 741 t.start();
sayzyas 2:aeff576d5013 742 DEBUG_PRINT_L1( "Bd1> MOTOR2 lock time start\r\n" );
sayzyas 0:653609b2a5cf 743 if( t.read_ms() > 100 ){ // 1sec
sayzyas 0:653609b2a5cf 744 DEBUG_PRINT_L1("Bd1> MOTOR1 STOP lock with time up: %d \r\n", flg_motor1_lock );
sayzyas 0:653609b2a5cf 745 DEBUG_PRINT_L1("Bd1> MOTOR2 STOP lock with time up: %d \r\n", flg_motor2_lock );
sayzyas 2:aeff576d5013 746 motor1_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
sayzyas 0:653609b2a5cf 747 motor2_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
sayzyas 0:653609b2a5cf 748 flg_motor1_lockcup = true;
sayzyas 0:653609b2a5cf 749 flg_motor2_lockcup = true;
sayzyas 0:653609b2a5cf 750 t.stop();
sayzyas 0:653609b2a5cf 751 }
sayzyas 2:aeff576d5013 752 }
sayzyas 2:aeff576d5013 753 #endif
sayzyas 2:aeff576d5013 754
sayzyas 0:653609b2a5cf 755 stdio_mutex.unlock();
sayzyas 2:aeff576d5013 756 // calb_motor_current(1, motor1_current_in.read()*100.0f);
sayzyas 2:aeff576d5013 757 // calb_motor_current(2, motor2_current_in.read()*100.0f);
sayzyas 0:653609b2a5cf 758 Thread::wait(1);
sayzyas 0:653609b2a5cf 759 }
sayzyas 0:653609b2a5cf 760 }
sayzyas 0:653609b2a5cf 761
sayzyas 0:653609b2a5cf 762 // Data recieve and display
sayzyas 0:653609b2a5cf 763 void callback() {
sayzyas 0:653609b2a5cf 764 char c;
sayzyas 0:653609b2a5cf 765 // Note: you need to actually read from the serial to clear the RX interrup
sayzyas 0:653609b2a5cf 766 c = sdc2130.getc();
sayzyas 0:653609b2a5cf 767 // if( c != '+')
sayzyas 0:653609b2a5cf 768 // DEBUG_PRINT_L1("%c\r\n", c);
sayzyas 0:653609b2a5cf 769 // DEBUG_PRINT("%c\r\n", sdc2130.getc()); // <-- may be this is correct ???
sayzyas 0:653609b2a5cf 770 }
sayzyas 0:653609b2a5cf 771
sayzyas 0:653609b2a5cf 772
sayzyas 0:653609b2a5cf 773 int main() {
sayzyas 0:653609b2a5cf 774
sayzyas 0:653609b2a5cf 775 int32_t counter = 0;
sayzyas 0:653609b2a5cf 776
sayzyas 0:653609b2a5cf 777 pc.baud(115200);
sayzyas 0:653609b2a5cf 778 sdc2130.baud(115200);
sayzyas 0:653609b2a5cf 779
sayzyas 0:653609b2a5cf 780 limit_sw_rf.mode( PullUp ); // use internal pullup
sayzyas 0:653609b2a5cf 781 limit_sw_lb.mode( PullUp ); // use internal pullup
sayzyas 0:653609b2a5cf 782
sayzyas 0:653609b2a5cf 783 DEBUG_PRINT_L0("\r\n");
sayzyas 0:653609b2a5cf 784 DEBUG_PRINT_L0("Bd1> +--------------------------------------------------------------\r\n");
sayzyas 0:653609b2a5cf 785 DEBUG_PRINT_L0("Bd1> | Project: B2 Crawler Explorer for 1F-1 PCV internal inspection\r\n");
sayzyas 0:653609b2a5cf 786 DEBUG_PRINT_L0("Bd1> |---------\r\n");
sayzyas 0:653609b2a5cf 787 DEBUG_PRINT_L0("Bd1> | This is: Each Motor Controller of Main Controller\r\n");
sayzyas 0:653609b2a5cf 788 DEBUG_PRINT_L0("Bd1> | Letest update: %s\r\n", LatestUpDate);
sayzyas 0:653609b2a5cf 789 DEBUG_PRINT_L0("Bd1> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 0:653609b2a5cf 790 DEBUG_PRINT_L0("Bd1> | Author: %s\r\n", Author);
sayzyas 0:653609b2a5cf 791 DEBUG_PRINT_L0("Bd1> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
sayzyas 0:653609b2a5cf 792 DEBUG_PRINT_L0("Bd1> | -------------------------------------------------\r\n");
sayzyas 0:653609b2a5cf 793 DEBUG_PRINT_L0("Bd1> | RoboteQ motor control thread added\r\n");
sayzyas 0:653609b2a5cf 794 DEBUG_PRINT_L0("Bd1> | 2016.03.16 Limit switch control added\r\n");
sayzyas 0:653609b2a5cf 795 DEBUG_PRINT_L0("Bd1> +--------------------------------------------------------------\r\n");
sayzyas 0:653609b2a5cf 796 DEBUG_PRINT_L0("Bd1> \r\ninitializing ... \r\n");
sayzyas 0:653609b2a5cf 797 // All output off
sayzyas 0:653609b2a5cf 798 // motor1_out = 0;
sayzyas 0:653609b2a5cf 799 // motor2_out = 0;
sayzyas 0:653609b2a5cf 800
sayzyas 3:4a7dea2da5b5 801 i2c_saddress = I2C_ADDRESS_WINCH; // 0x10
sayzyas 2:aeff576d5013 802 //i2c_saddress = I2C_ADDRESS_TRANSFORM; // 0x08
sayzyas 3:4a7dea2da5b5 803 //i2c_saddress = I2C_ADDRESS_CRAWLER; // 0x04
sayzyas 0:653609b2a5cf 804
sayzyas 0:653609b2a5cf 805 sdc2130.attach(&callback);
sayzyas 0:653609b2a5cf 806 sdc2130.printf( "^ECHOF 1\r" );
sayzyas 0:653609b2a5cf 807 sdc2130.printf( "^ECHOF 2\r" );
sayzyas 0:653609b2a5cf 808
sayzyas 0:653609b2a5cf 809 led_demo();
sayzyas 0:653609b2a5cf 810
sayzyas 0:653609b2a5cf 811 DEBUG_PRINT_L0("Bd1> > Board type: 0x%02x\r\n",i2c_saddress );
sayzyas 0:653609b2a5cf 812
sayzyas 0:653609b2a5cf 813 // Set the initial value of motor current
sayzyas 0:653609b2a5cf 814 DEBUG_PRINT_L0("\r\nBd1> Read and set the initial value of motor current ... \r\n");
sayzyas 2:aeff576d5013 815 set_init_motor_current(1, motor1_current_in.read()*100.0f, 1000);
sayzyas 2:aeff576d5013 816 set_init_motor_current(2, motor2_current_in.read()*100.0f, 1000);
sayzyas 0:653609b2a5cf 817
sayzyas 0:653609b2a5cf 818 DEBUG_PRINT_L0( "Bd1> *****************************************\r\n" );
sayzyas 0:653609b2a5cf 819 DEBUG_PRINT_L0( "Bd1> Init motor current1: %3.4f%\r\n", motor1_current_center_value );
sayzyas 0:653609b2a5cf 820 DEBUG_PRINT_L0( "Bd1> Init motor current2: %3.4f%\r\n", motor2_current_center_value );
sayzyas 0:653609b2a5cf 821 DEBUG_PRINT_L0( "Bd1> *****************************************\r\n" );
sayzyas 0:653609b2a5cf 822
sayzyas 1:c3508bfe65b6 823 // Thread (
sayzyas 1:c3508bfe65b6 824 // void(*task)(void const *argument),
sayzyas 1:c3508bfe65b6 825 // void *argument=NULL,
sayzyas 1:c3508bfe65b6 826 // osPriority priority=osPriorityNormal,
sayzyas 1:c3508bfe65b6 827 // uint32_t stack_size=DEFAULT_STACK_SIZE,
sayzyas 1:c3508bfe65b6 828 // unsigned char *stack_pointer=NULL
sayzyas 1:c3508bfe65b6 829 //)
sayzyas 1:c3508bfe65b6 830
sayzyas 1:c3508bfe65b6 831 // -----------------------------------------------------------------
sayzyas 1:c3508bfe65b6 832 // In case of multi stack using , should increase stack size.
sayzyas 1:c3508bfe65b6 833 // 2016.04.07
sayzyas 1:c3508bfe65b6 834 // -----------------------------------------------------------------
sayzyas 0:653609b2a5cf 835 DEBUG_PRINT_L0("Bd1> Start RoboteQ motor driver task ... \r\n");
sayzyas 1:c3508bfe65b6 836 Thread motorDrvControlTask1(RoboteQM1DrvCtrl_task, NULL, osPriorityNormal, 1024);
sayzyas 1:c3508bfe65b6 837 Thread motorDrvControlTask2(RoboteQM2DrvCtrl_task, NULL, osPriorityNormal, 1024); //
sayzyas 0:653609b2a5cf 838 DEBUG_PRINT_L0("Bd1> Start motor control taslk (main thread) ... \r\n");
sayzyas 1:c3508bfe65b6 839 Thread motorControlTask(motor_control_task, NULL, osPriorityNormal, 1024);
sayzyas 0:653609b2a5cf 840 DEBUG_PRINT_L0("Bd1> Initializing completed\r\n");
sayzyas 0:653609b2a5cf 841
sayzyas 0:653609b2a5cf 842 // Reset the motor controller;
sayzyas 0:653609b2a5cf 843 /*
sayzyas 0:653609b2a5cf 844 sdc2130.printf( "%RESET 32164987\r" );
sayzyas 0:653609b2a5cf 845 sdc2130.printf( "%RESET 32164987\r" );
sayzyas 0:653609b2a5cf 846 sdc2130.printf( "%RESET 32164987\r" );
sayzyas 0:653609b2a5cf 847 sdc2130.printf( "%RESET 32164987\r" );
sayzyas 0:653609b2a5cf 848 sdc2130.printf( "%RESET 32164987\r" );
sayzyas 0:653609b2a5cf 849 */
sayzyas 0:653609b2a5cf 850 sdc2130.printf( "^ECHOF 1\r" );
sayzyas 0:653609b2a5cf 851 sdc2130.printf( "^ECHOF 2\r" );
sayzyas 0:653609b2a5cf 852
sayzyas 0:653609b2a5cf 853 while(1) {
sayzyas 0:653609b2a5cf 854 Thread::wait(20);
sayzyas 0:653609b2a5cf 855 // sdc2130.printf( "?V\r" );
sayzyas 0:653609b2a5cf 856 // sdc2130.printf( "^ECHOF 1\r" );
sayzyas 0:653609b2a5cf 857 // sdc2130.printf( "^ECHOF 2\r" );
sayzyas 0:653609b2a5cf 858 counter++;
sayzyas 0:653609b2a5cf 859 if( counter >= 25 ){
sayzyas 0:653609b2a5cf 860 led2 = !led2;
sayzyas 0:653609b2a5cf 861 counter = 0;
sayzyas 0:653609b2a5cf 862
sayzyas 0:653609b2a5cf 863 if( limit_sw_rf == 0 ){
sayzyas 0:653609b2a5cf 864 limitSw_Sts |= 0x01;
sayzyas 0:653609b2a5cf 865 }
sayzyas 0:653609b2a5cf 866 else{
sayzyas 0:653609b2a5cf 867 limitSw_Sts &= 0xFE;
sayzyas 0:653609b2a5cf 868 }
sayzyas 0:653609b2a5cf 869
sayzyas 0:653609b2a5cf 870 if( limit_sw_lb == 0 ){
sayzyas 0:653609b2a5cf 871 limitSw_Sts |= 0x02;
sayzyas 0:653609b2a5cf 872 }
sayzyas 0:653609b2a5cf 873 else{
sayzyas 0:653609b2a5cf 874 limitSw_Sts &= 0xFD;
sayzyas 0:653609b2a5cf 875 }
sayzyas 0:653609b2a5cf 876
sayzyas 0:653609b2a5cf 877 }
sayzyas 0:653609b2a5cf 878 Thread::wait(5);
sayzyas 0:653609b2a5cf 879 }
sayzyas 0:653609b2a5cf 880 }