File content as of revision 3:4a7dea2da5b5:

/**********************************************************
 * Project: B2 (1F-1)
 * Title:   CrExp B2 Motor Ctrl Main
 * Target:  LPCXpresso824-Max
 *          or ( LPC800Max )    
 * Author:  sayzyas
 * Date(Latest update) 2015.06.17(Wed)
 * --------------------------------------------------------
 *
 *           LPCXpresso 824-MAX
 *          +---------USB---------+
 *          |                     |
 *          |                     |
 *          |                     | 
 *          |                     |
 *          |                     | SCL  P0_10 D15
 *          |   ##   ###     ##   | SDA  P0_11 D14
 *          |  #  # #   #   # #   |            AVDD
 * N/A      |  #  #     #   # #   |            GND
 * +3V3     |   ##     #   #  #   | SCK  P0_24 D13         
 * NRST     |  #  #   #    #  #   | MISO P0_25 D12
 * +3V3     |  #  #  #  # ######  | MOSI P0_26 D11 -> I/O Ex Bd
 * +5V      |   ##  #####     #   | SSEL P0_15 D10 -> I/O Ex Bd
 * GND      |                     |      P0_27 D9  -> I/O Ex Bd
 * GND      |                     |      P0_13 D8  -> I/O Ex Bd
 * N/A      |                     |      P0_17 D7 
 *          |                     |      P0_16 D6
 * A0 P0_6  |                     | PWM  P0_28 D5
 * A1 P0_14 |                     | PWM  P0_18 D4
 * A2 P0_23 |                     | PWM  P0_12 D3
 * A3 P0_22 |                     | PWM  P0_19 D2
 * A4 P0_21 |                     | TX   P0_4  D1 -> UART Tx
 * A5 P0_20 |                     | RX   P0_0  D0 -> UART Rx
 *          +---------------------+
 *
 ***************************************/
 /*
    2016.02.06
    Added Limit switch detect control in RoboteQ motor control task.
 */
 #define ___CURRENT_CHECK___

#include "mbed.h"
#include "rtos.h"
#include "common.h"



// Hardware setting
Serial pc(USBTX, USBRX);        // tx, rx
Serial sdc2130(P0_4, P0_0);     // Communicate with RpboteQ Driver by tx, rx

/* #### Don't use #### */
/* LED indicator for motor current indicate */
//DigitalOut myled0(P0_6);  // My LED0
//DigitalOut myled1(P0_14); // My LED1
DigitalOut myled2(P0_23); // My LED2
DigitalOut myled3(P0_22); // My LED3

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);

/* Analog input setting */
AnalogIn   motor1_speed_adjin(A0);
AnalogIn   motor2_speed_adjin(A1);

AnalogIn   motor1_current_in(A5);
AnalogIn   motor2_current_in(A4);

/* Digital In */
//DigitalIn limit_sw_rf(P0_24);    // Limit switch (RF-h) for Transform controller only
//DigitalIn limit_sw_lb(P0_25);    // Limit switch (LB-I) for Transform controller only
//2016.05.18 Limit switch input port change to opposite.  
DigitalIn limit_sw_rf(P0_25);    // Limit switch (RF-h) for Transform controller only
DigitalIn limit_sw_lb(P0_24);    // Limit switch (LB-I) for Transform controller only


Mutex stdio_mutex; 

bool        flg_lsw_valid = false;

/* ** Worning *****************************
 * M1 side is not compatible with Arduino IO
 * exp board, so use following pin and modify 
 * or re-connect signal of IO exp board. */
//DigitalOut motor1_dir(P0_24); //D8
//PwmOut motor1_out(P0_25);
//DigitalOut motor1_out(P0_25); //D9

// M2 side is compatible with Arduino IO exp board.
//DigitalOut motor2_out(P0_15); //D10
//DigitalOut motor2_dir(P0_26); //D11
//PwmOut motor2_out(P0_15);

I2CSlave slave(P0_11, P0_10); //I2C SDA SCL

//QEI wheel( P0_15, P0_26, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );

// ----------------------------------------------------------------
// Prototype
//
void led_onoff(void);
void led_demo(void);
void myled_control( int action );
int read_motor_current( uint32_t motor_no );
void current_read_task(void const *);
void motor1_ctrl( uint32_t onoff, uint32_t dir );
void motor2_ctrl( uint32_t onoff, uint32_t dir );
void RoboteQM1DrvCtrl_task( void const *);
void RoboteQM2DrvCtrl_task( void const *);
void motor_control_task(void const *);
void set_init_motor_current(uint32_t motor_no, float motor_current, int cnt);
float calb_motor_current(uint32_t motor_no, float motor_current);

// ----------------------------------------------------------------
// Sub Routine
//
void led_onoff(){
    led1 = 1;
    wait(0.5);
    led1 = 0;   // off
    wait(0.5);
    
}
// LED demo
void led_demo(){
    int i;
    for( i = 0; i < 5; i++ ) {
        led1 = LED_ON;
        led2 = LED_OFF;
        led3 = LED_OFF;
        wait_ms(50);
        led1 = LED_OFF;
        led2 = LED_OFF;
        led3 = LED_OFF;
        wait_ms(50);
        led1 = LED_OFF;
        led2 = LED_ON;
        led3 = LED_OFF;
        wait_ms(50);
        led1 = LED_OFF;
        led2 = LED_OFF;
        led3 = LED_OFF;
        wait_ms(50);
        led1 = LED_OFF;
        led2 = LED_OFF;
        led3 = LED_ON;
        wait_ms(50);
        led1 = LED_OFF;
        led2 = LED_OFF;
        led3 = LED_OFF;
        wait_ms(50);
    }
    wait_ms(3000);
}

void myled_control( int action ){
    
    for( int i = 0; i < 10; i++ ){
    //    myled0 = 1;
    //    myled1 = 1;
        myled2 = 1;
        myled3 = 1;
        wait(0.2);
    //    myled0 = 0;   // off
    //    myled1 = 0;   // off
        myled2 = 0;   // off
        myled3 = 0;   // off
        wait(0.2);
   }          
}

uint32_t   motor1_lock_count = 0;
uint32_t   motor2_lock_count = 0;
volatile bool flg_motor1_lockcup = false;
volatile bool flg_motor2_lockcup = false;
volatile uint32_t flg_motor1_lock = 0;
volatile uint32_t flg_motor2_lock = 0;

int i2c_saddress = 0;

uint8_t send_data[7];

float motor1_current_center_value;
float motor2_current_center_value;
float motor3_current_center_value;

uint8_t motor1_speed;
uint8_t motor2_speed;

uint16_t motor1_speed_adjdata;
uint16_t motor2_speed_adjdata;

// --------------------------------------------------------------
// Motor Current Calibration Function
//   Arg: Motor Number 1 - 3
// --------------------------------------------------------------
void set_init_motor_current(uint32_t motor_no, float motor_current, int cnt){

    for( int i = 0; i < cnt; i++ ){
        if( motor_no == 1 ){
            motor1_current_center_value = motor_current;
        }
        else if( motor_no == 2 ){
            motor2_current_center_value = motor_current;
        } 
        else if( motor_no == 3 ){
            motor2_current_center_value = motor_current;
        } 
    }    
}

// --------------------------------------------------------------
// Motor Current Calibration Function
//   Arg: Motor Number 1 - 3
// --------------------------------------------------------------
float calb_motor_current(uint32_t motor_no, float motor_current){
    float rts;
    
    if( motor_no == 1 ){
        motor1_current_center_value += motor_current;
        motor1_current_center_value /= 2;
        rts = motor1_current_center_value;
    }
    else if( motor_no == 2 ){
        motor2_current_center_value += motor_current;
        motor2_current_center_value /= 2;
        rts = motor2_current_center_value;
    } 
    else if( motor_no == 3 ){
        motor3_current_center_value += motor_current;
        motor3_current_center_value /= 2;
        rts = motor3_current_center_value;
    }
    else rts = 0;
    return rts;
}

float motor1_current_fwd_thd;
float motor1_current_rvs_thd;
float motor2_current_fwd_thd;
float motor2_current_rvs_thd;
// --------------------------------------------------------------
// Motor Current Read Function
//   Arg: Motor Number 1 - 3
// --------------------------------------------------------------
int read_motor_current( char motor_no, char motor_dir ){

    float   motor_current;
    int     i;
    int     mc_abs_pct = 0;
    int     mc_Threshold = 0;

    if (motor_no == MOTOR_1 ){      
        motor_current = motor1_current_in.read()*100.0f;
        mc_abs_pct = abs((int)((motor_current - motor1_current_center_value)*100.0f));
        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 );
     //   DEBUG_DSPMCNT("Bd1> M1 motor_current center value   : %lf%%\r\n", motor1_current_center_value );
     //   DEBUG_DSPMCNT("Bd1> M1 motor_current abs percentage : %d%%\r\n", mc_abs_pct );
        for( i = 0; i < (mc_abs_pct/10); i++){
            DEBUG_DSPMCNT(">");
        }
        DEBUG_DSPMCNT("\r\n");

        if( motor_dir == MOTOR_FWD ){
            mc_Threshold = (int)motor1_current_fwd_thd;
            DEBUG_PRINT_L1("Bd1> Upper threshold: %d\r\n", mc_Threshold);
        }
        else{
            mc_Threshold = (int)motor1_current_rvs_thd;
            DEBUG_PRINT_L1("Bd1> Lower threshold: %d\r\n", mc_Threshold);
        }
        if( mc_abs_pct > mc_Threshold ){
            DEBUG_PRINT_L1("Bd1> **** MC1 Over the Limit [%d] ****\r\n", motor1_lock_count );
            motor1_lock_count += 1;
            if( motor1_lock_count >= MC_LOCK_COUNT ){
                stdio_mutex.lock();     // Mutex Lock
                flg_motor1_lock = 1;
                stdio_mutex.unlock();   // Mutex Release
                DEBUG_DSPMCNT("Bd1> #### MOTOR1 LOCK ! #### (%d)\r\n", flg_motor1_lock);
            }
        }
        else{
            DEBUG_PRINT_L1("Bd1> Pass\r\n");
            if( motor1_lock_count > 0 ) motor1_lock_count -= 1;
            else motor1_lock_count = 0;
        }
    }
    else if (motor_no == MOTOR_2 ){      
        motor_current = motor2_current_in.read()*100.0f;
        mc_abs_pct = abs((int)((motor_current - motor2_current_center_value)*100.0f));
        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 );
        for( i = 0; i < (mc_abs_pct/10); i++){
            DEBUG_DSPMCNT("=");
        }
        DEBUG_DSPMCNT("\r\n");
        
        if( motor_dir == MOTOR_FWD ){
            mc_Threshold = (int)motor2_current_fwd_thd;
        }
        else{
            mc_Threshold = (int)motor2_current_rvs_thd;
        }       
        if( mc_abs_pct > mc_Threshold ){
            DEBUG_PRINT_L1("Bd1> **** MC2 Over the Limit [%d] ****\r\n", motor2_lock_count );
            motor2_lock_count += 1;
            if( motor2_lock_count >= MC_LOCK_COUNT ){
                stdio_mutex.lock();     // Mutex Lock
                flg_motor2_lock = 1;
                stdio_mutex.unlock();   // Mutex Release
                DEBUG_DSPMCNT("Bd1> #### MOTOR2 LOCK ! #### (%d)\r\n", flg_motor2_lock);
            }
        }
        else{
            if( motor2_lock_count > 0 ) motor2_lock_count -= 1;
            else motor2_lock_count = 0;
        }
    }
    return mc_abs_pct;
}


Timer t;

uint8_t motor_control_flag1 = 0x00;
uint8_t motor_control_flag2 = 0x00;

// --------------------------------------------------------------------------
// Motor Control Function
//  Motor on off by microcontroller
//  motor1_out : motor 1 on /off
//  motor1_dir : motor 1 direction
//  motor2_out : motor 2 on / off
//  motor2_dir : motor 2 direction
// --------------------------------------------------------------------------
// "motor_control_flag" bit function 
//  bit
//  ------
//   7  : motor number : upper nibble
//   6  :  0 : no motor
//   5  :  0 - 15 : motor number
//   4  : 
//  ------ 
//   3  : motor on/off, 0=off 1=on 
//   2  : 
//   1  : 
//   0  : motor direction, 0=fwd 1=rev  
void motor1_ctrl( 
    uint32_t onoff,         // Motor ON/OFF
    uint32_t dir            // Motor rotation direction
){
//    DEBUG_PRINT( "Motor No.%02d, [onoff:%d], [dir:%d]\r\n", motor_no, onoff, dir );
    led1 = !onoff;
    motor_control_flag1 &= 0x0F;
    motor_control_flag1 |= 0x10;
    if( dir == FLG_MOTOR_DIR_FWD ){
    //    motor1_dir = 0;   // 0:Fwd, 1:Rvs
        motor_control_flag1 &= 0xFE;
    }
    else if( dir == FLG_MOTOR_DIR_RVS ){
    //    motor1_dir = 1;   // 0:Fwd, 1:Rvs  
        motor_control_flag1 |= 0x01;
    }
//    stdio_mutex.lock();     // Mutex Lock
    if(( !flg_motor1_lock )&&( onoff )){ // Case of motor unlock by timer
    //    motor1_out = 1; // Motor1 on
        motor_control_flag1 |= 0x08;
    }
    else{ 
    //    motor1_out = 0; // Motor1 off
        motor_control_flag1 &= 0xF7;
    }
//    stdio_mutex.unlock();   // Mutex Release
}

void motor2_ctrl( 
    uint32_t onoff,     // Motor ON/OFF
    uint32_t dir        // Motor rotaion direction
){
//    DEBUG_PRINT( "Motor No.%02d, [onoff:%d], [dir:%d]\r\n", motor_no, onoff, dir );
    led1 = !onoff;
    motor_control_flag2 &= 0x0F;
    motor_control_flag2 |= 0x20;
    if( dir == FLG_MOTOR_DIR_FWD ){
    //    motor2_dir = 0;   // 0:Fwd, 1:Rvs  
        motor_control_flag2 &= 0xFE;
    }
    else if( dir == FLG_MOTOR_DIR_RVS ){
    //    motor2_dir = 1;   // 0:Fwd, 1:Rvs  
        motor_control_flag2 |= 0x01;
    }
//     stdio_mutex.lock();     // Mutex Lock
    if(( !flg_motor2_lock )&&( onoff )){ // Case of motor unlock by timer
    //    motor2_out = 1; // Motor2 on
        motor_control_flag2 |= 0x08;
    }
    else{
    //    motor2_out = 0; // Motor2 off
        motor_control_flag2 &= 0xF7;
    }
//    stdio_mutex.unlock();   // Mutex Release
}


int limitSw_Sts = 0;
// -----------------------------------------------------
// Motor COntrol Task: loop forever !
//  For RoboteQ motor controller
//  Note. Motor control command should always send to 
//        RoboteQ controllet, so following should be 
//        done by thread.
// -----------------------------------------------------
// motor_control_flag bit function 
//  bit
//  ------
//   7  : motor number : upper nibble
//   6  :  0 : no motor
//   5  :  0 - 15 : motor number
//   4  : 
//  ------ 
//   3  : motor on/off, 0=off 1=on 
//   2  : 
//   1  : 
//   0  : motor direction, 0=fwd 1=rev  
void RoboteQM1DrvCtrl_task( void const *){
    while(1){

        
        if( flg_motor1_lock ){  
            DEBUG_PRINT_L1("Bd1> M1 LOCK\r\n");   
            sdc2130.printf( "!G 1 0\r" );       // Motor off
        }
        else{
        //    DEBUG_PRINT("Motor Speed : %d\r\n", motor1_speed);
            if(( motor_control_flag1 & 0x08 )){ // motor on 
                if(!(motor_control_flag1 & 0x01)){ // motor dir fwd
                    if((motor_control_flag1 & 0xF0) == 0x10){ // motor 1
                        DEBUG_PRINT_L1("Bd1> RoboteQ <-- M1 FWD (%d)\r\n", motor1_speed );   
                        stdio_mutex.lock();     // Mutex Lock 
                    //    sdc2130.printf( "!AC 1 10000" );
                        sdc2130.printf( "!G 1 %d\r", motor1_speed*10 );    // Motor on reverse 100.0% 
                    //    sdc2130.printf( "!G 1 %d\r", motor1_speed*10 );    // Motor on reverse 100.0% 
                        limitSw_Sts &= 0xFE;    
                        stdio_mutex.unlock();     // Mutex Lock
                    }
                }
                else{ // motor dir rev
                    if((motor_control_flag1 & 0xF0) == 0x10){ // motor 1 
                        stdio_mutex.lock();     // Mutex Lock 
                        if(( limit_sw_rf == 0 )&&(flg_lsw_valid == true )){
                            DEBUG_PRINT_L1("Limit switch ON (M1 reverse) \r\n");
                            DEBUG_PRINT_L4("Bd1> RoboteQ <-- M1 RVS (0)\r\n");   
                            sdc2130.printf("!G 1 0\r"); // Motor off
                            limitSw_Sts |= 0x01;
                        }
                        else{
                            DEBUG_PRINT_L1("Bd1> RoboteQ <-- M1 RVS (%d)\r\n", motor1_speed*-1 );   
                            sdc2130.printf( "!G 1 %d\r", (motor1_speed*-1)*10 );    // Motor on reverse 100.0% 
                            limitSw_Sts &= 0xFE;
                        }
                        stdio_mutex.unlock();     // Mutex Lock 
                    }
                }
            }
            else{
            //    DEBUG_PRINT_L1("Bd1> M1 OFF\r\n");   
                sdc2130.printf( "!G 1 0\r" );       // Motor off
            //    Thread::wait(30); 
            //    calb_motor_current(1, motor1_current_in.read()*100.0f); 
            }
        } 
    Thread::wait(7);           
    }
}

void RoboteQM2DrvCtrl_task( void const *){
    while(1){
        if( flg_motor2_lock ){ 
            DEBUG_PRINT_L1("Bd1> M2 LOCK\r\n");   
            sdc2130.printf( "!G 2 0\r" );       // Motor off        
        }
        else{
        //    DEBUG_PRINT("Motor Speed : %d\r\n", motor1_speed);
            if(( motor_control_flag2 & 0x08 )){ // motor on 
                if(!(motor_control_flag2 & 0x01)){ // motor dir fwd
                    if((motor_control_flag2 & 0xF0) == 0x20){ // motor 2
                        DEBUG_PRINT_L1("Bd1> RoboteQ <-- M2 FWD (%d)\r\n", motor2_speed );   
                    //    sdc2130.printf( "!G 2 1000\r" );    // Motor on reverse 100.0% 
                        stdio_mutex.lock();     // Mutex Lock 
                        sdc2130.printf( "!G 2 %d\r", motor2_speed*10 );    // Motor on reverse 100.0% 
                    //    sdc2130.printf( "!G 2 %d\r", motor2_speed*10 );    // Motor on reverse 100.0% 
                        limitSw_Sts &= 0xFD;
                        stdio_mutex.unlock();     // Mutex Lock 
                    }
                }
                else{ // motor dir rev
                    if((motor_control_flag2 & 0xF0) == 0x20){ // motor 2
                    //    sdc2130.printf( "!G 2 -1000\r" );    // Motor on reverse 100.0% 
                        stdio_mutex.lock();     // Mutex Lock 
                        if(( limit_sw_lb == 0 )&&(flg_lsw_valid == true )){
                            DEBUG_PRINT_L1("Limit switch ON (M2 reverse) \r\n");
                            DEBUG_PRINT_L4("Bd1> RoboteQ <-- M2 RVS (0)\r\n");   
                            sdc2130.printf("!G 2 0\r");       // Motor off
                            limitSw_Sts |= 0x02;
                        }
                        else{                            
                            DEBUG_PRINT_L1("Bd1> RoboteQ <-- M2 RVS (%d)\r\n", motor2_speed*-1);   
                            sdc2130.printf("!G 2 %d\r", (motor2_speed*-1)*10);    // Motor on reverse 100.0% 
                            limitSw_Sts &= 0xFD;
                        }
                        stdio_mutex.unlock();     // Mutex Lock 
                    }
                }
            }
            else{
            //    DEBUG_PRINT_L1("Bd1> M2 OFF\r\n");   
                sdc2130.printf( "!G 2 0\r" );       // Motor off
           //     Thread::wait(30); 
           //     calb_motor_current(2, motor2_current_in.read()*100.0f); 
            }
        }
        Thread::wait(1);        
    }
}

int motor1_current_pct;    
int motor2_current_pct;
// ========================================================================
//  Thread: Motor Control Task
// ========================================================================
void motor_control_task(void const *) {
    
    char buf[14];   // command buffer

    char res_msg2[6];
    
    slave.address(i2c_saddress);
     
    /* 
     * I2C Access 
     */
    while(1) {
        int i = slave.receive();
        slave.read(buf, NumberOfI2CCommand);
        
   //     DEBUG_PRINT_L3("Bd1> Slave Received\r\n");
        switch (i) {
            case I2CSlave::NoData:
            //    DEBUG_PRINT_L3("Bd1> NoData(the slave has not been addressed)\r\n");
            //    motor_current_pct = read_motor_current(2);
                break;
            case I2CSlave::ReadAddressed:
                // Send Motor Current here to host controller.
                res_msg2[0] = (char)(motor1_current_pct & 0xFF);
                res_msg2[1] = (char)(motor2_current_pct & 0xFF);
                res_msg2[3] = (char)(limitSw_Sts);
                if( flg_motor1_lockcup == true ){
                    res_msg2[4] = '1';        // Motor 1 Lock !!
                }
                else if( flg_motor2_lockcup == true ){
                    res_msg2[4] = '2';   // Motor 2 Lock !!
                }
                else{
                    res_msg2[4] = 'S';
                }
                res_msg2[5] = '\0';

                DEBUG_PRINT_L3("\t\t motor1_current_pct = %d\r\n", motor1_current_pct );
                DEBUG_PRINT_L3("\t\t motor2_current_pct = %d\r\n", motor2_current_pct );
                DEBUG_PRINT_L3("\t\t imit sw = 0x%02x\r\n", limitSw_Sts );
                if( flg_motor1_lockcup == true ){
                    res_msg2[2] = 0x88;  
                    DEBUG_PRINT_L3("  0: [%d]\r\n", res_msg2[0] );
                    DEBUG_PRINT_L3("  1: [%d]\r\n", res_msg2[1] );
                    DEBUG_PRINT_L3("  2: [0x%02x]\r\n", res_msg2[2] );
                    DEBUG_PRINT_L3("  3: [%d]\r\n", res_msg2[3] );
                    DEBUG_PRINT_L3("  4: [%c]\r\n", res_msg2[4] );
                }
                else{
                    res_msg2[2] = 0x00;  
                }
                slave.write(res_msg2, 6); // Includes null char
                break;
            case I2CSlave::WriteGeneral:
          //      DEBUG_PRINT_L3("Bd1> the master is writing to all slave\r\n"); 
                slave.read(buf, NumberOfI2CCommand);
          //      DEBUG_PRINT_L3("Bd1> Read G: %s\r\n", buf);
                break;
            case I2CSlave::WriteAddressed:
                motor1_current_fwd_thd = (float)(((buf[I2C_CP_M1_FWD_CNTTH_U]<<8)&0xFF00)|(buf[I2C_CP_M1_FWD_CNTTH_L]&0xFF)); // <-- New!
                motor1_current_rvs_thd = (float)(((buf[I2C_CP_M1_RVS_CNTTH_U]<<8)&0xFF00)|(buf[I2C_CP_M1_RVS_CNTTH_L]&0xFF)); // <-- New!
                motor2_current_fwd_thd = (float)(((buf[I2C_CP_M2_FWD_CNTTH_U]<<8)&0xFF00)|(buf[I2C_CP_M2_FWD_CNTTH_L]&0xFF)); // <-- New!
                motor2_current_rvs_thd = (float)(((buf[I2C_CP_M2_RVS_CNTTH_U]<<8)&0xFF00)|(buf[I2C_CP_M2_RVS_CNTTH_L]&0xFF)); // <-- New!
                // Set Motor Speed here: Speed is set by Main Controller via I2C command right now.
                // In case of Winch motor controller, Speed is set by analog data from A0 and A1 <-- It's for evaluation 
                // for devalopment the "Fix position falling winch". 
                if( buf[I2C_CP_M1_DIR] == MOTOR_STP )
                {
                    motor1_speed = 0;
                }
                else
                {
                    if(( buf[I2C_CP_M1_DIR] == MOTOR_FWD )||( buf[I2C_CP_M1_DIR] == MOTOR_RVS ))
                    {
                        motor1_speed = buf[I2C_CP_M1_SPEED];
                    }
                }
                if( buf[I2C_CP_M2_DIR] == MOTOR_STP )
                {
                    motor2_speed = 0;
                }
                else
                {
                    if(( buf[I2C_CP_M2_DIR] == MOTOR_FWD )||( buf[I2C_CP_M2_DIR] == MOTOR_RVS ))
                    {
                        motor2_speed = buf[I2C_CP_M2_SPEED];
                    }
                }
         //       DEBUG_PRINT_L1( "Bd1> GOT MOTOR SPEED: M1[ %d ] ", motor1_speed );
         //       DEBUG_PRINT_L1( "M2[ %d ] \r\n", motor2_speed );
 
                if( buf[I2C_CP_COMMAND] == 'V' ){
                    flg_lsw_valid = true;
                }
                else{
                    flg_lsw_valid = false;
                }
            
                if( buf[I2C_CP_M2_DIR] == MOTOR_STP ){
                    //DEBUG_PRINT_L1("M2 Off_1\r\n");
                    motor2_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
                    t.stop();
                    motor2_lock_count = 0;
                    motor2_current_pct = 0;
                    flg_motor2_lockcup = false;
                    stdio_mutex.lock();
                    flg_motor2_lock = 0;
                    stdio_mutex.unlock();
                }               
                else if(( buf[I2C_CP_M2_DIR] == MOTOR_FWD )&&(flg_motor2_lockcup == false)){ // MOTOR2 FORWARD
                    motor2_ctrl( FLG_MOTOR_ON, FLG_MOTOR_DIR_FWD );
                    motor2_current_pct = read_motor_current( MOTOR_2, MOTOR_FWD );
                }
                else if(( buf[I2C_CP_M2_DIR] == MOTOR_RVS )&&(flg_motor2_lockcup == false)){ // MOTOR2 REVERSE
                    motor2_ctrl( FLG_MOTOR_ON, FLG_MOTOR_DIR_RVS );
                    motor2_current_pct = read_motor_current( MOTOR_2, MOTOR_RVS );
                }
                // 2016.11.07: following section commented out ! : 
                // If this section is velidate, then suddnly motor stopped because motor on
                // flag is cleared. this is bug !!
/*
                else{
                    //DEBUG_PRINT_L1("M2 Off_2\r\n");
                    motor2_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
                    motor2_lock_count = 0;
                    motor2_current_pct = 0;
                    t.stop();
                    stdio_mutex.lock();
                    flg_motor2_lock = 0;
                    stdio_mutex.unlock();
                }
*/             
                if( buf[I2C_CP_M1_DIR] == MOTOR_STP ){
                    //DEBUG_PRINT_L1("M1 Off_1\r\n");
                    motor1_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
                    t.stop();
                    motor1_current_pct = 0;
                    motor1_lock_count = 0;
                    flg_motor1_lockcup = false;
                    stdio_mutex.lock();
                    flg_motor1_lock = 0;
                    stdio_mutex.unlock();
                }
                else if(( buf[I2C_CP_M1_DIR] == MOTOR_FWD)&&(flg_motor1_lockcup == false)){ // MOTOR1 FORWARD
                    motor1_ctrl( FLG_MOTOR_ON, FLG_MOTOR_DIR_FWD );
                    motor1_current_pct = read_motor_current( MOTOR_1, MOTOR_FWD );
                }
                else if(( buf[I2C_CP_M1_DIR] == MOTOR_RVS )&&(flg_motor1_lockcup == false)){ // MOTOR1 REVERSE
                    motor1_ctrl( FLG_MOTOR_ON, FLG_MOTOR_DIR_RVS );
                    motor1_current_pct = read_motor_current( MOTOR_1, MOTOR_RVS );
                }
                // 2016.11.07: following section commented out ! : 
                // If this section is velidate, then suddnly motor stopped because motor on
                // flag is cleared. this is bug !!
/*                else{
                    motor1_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
                    motor1_current_pct = 0;
                    motor1_lock_count = 0;
                    t.stop();
                    stdio_mutex.lock();
                //    pc.printf("%%%%%%%%\r\n");
                    flg_motor1_lock = 0;
                    stdio_mutex.unlock();
                }
*/            
                break;
        }
        for(int i = 0; i < NumberOfI2CCommand; i++) buf[i] = 0;    // Clear buffer
 

        stdio_mutex.lock();

#ifdef __SEPARATE_TYPR__
        if ( flg_motor1_lock == 1 ){
            t.start();
            DEBUG_PRINT_L1( "Bd1> MOTOR1 lock time start\r\n" );
            if( t.read_ms() > 100 ){   // 1sec 
                DEBUG_PRINT_L1("Bd1> MOTOR1 STOP lock with time up: %d \r\n", flg_motor1_lock );
            //    DEBUG_PRINT_L1("Bd1> MOTOR2 STOP lock with time up: %d \r\n", flg_motor2_lock );
                motor1_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
            //    motor2_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
                flg_motor1_lockcup = true;
            //    flg_motor2_lockcup = true;
                t.stop();
            }  
        }
        
        if ( flg_motor2_lock == 1 ){
            t.start();
            DEBUG_PRINT_L1( "Bd1> MOTOR2 lock time start\r\n" );
            if( t.read_ms() > 100 ){   // 1sec 
            //    DEBUG_PRINT_L1("Bd1> MOTOR1 STOP lock with time up: %d \r\n", flg_motor1_lock );
                DEBUG_PRINT_L1("Bd1> MOTOR2 STOP lock with time up: %d \r\n", flg_motor2_lock );
            //    motor1_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
                motor2_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
            //    flg_motor1_lockcup = true;
                flg_motor2_lockcup = true;
                t.stop();
            }  
        }        
#else
        /*
            Motor Lock detection and flag on part
            -------------------------------------
            Currently, in case of each motor lock, then both motor will stop
            and both flag on.
            Therefore, PC S/W can detect motor lock only if only one side flag check.
        */
        if (( flg_motor1_lock == 1 )||( flg_motor2_lock == 1 )){
            t.start();
            DEBUG_PRINT_L1( "Bd1> MOTOR2 lock time start\r\n" );
            if( t.read_ms() > 100 ){   // 1sec 
                DEBUG_PRINT_L1("Bd1> MOTOR1 STOP lock with time up: %d \r\n", flg_motor1_lock );
                DEBUG_PRINT_L1("Bd1> MOTOR2 STOP lock with time up: %d \r\n", flg_motor2_lock );
                motor1_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
                motor2_ctrl( FLG_MOTOR_OFF, FLG_MOTOR_DIR_NONE );
                flg_motor1_lockcup = true;
                flg_motor2_lockcup = true;
                t.stop();
            }  
        }  
#endif
        
        stdio_mutex.unlock();
    //    calb_motor_current(1, motor1_current_in.read()*100.0f); 
    //    calb_motor_current(2, motor2_current_in.read()*100.0f); 
        Thread::wait(1);
    }
}

// Data recieve and display
void callback() {
    char c;
    // Note: you need to actually read from the serial to clear the RX interrup
    c = sdc2130.getc();
//    if( c != '+') 
//    DEBUG_PRINT_L1("%c\r\n", c);
    // DEBUG_PRINT("%c\r\n", sdc2130.getc()); // <-- may be this is correct ???
}


int main() {
    
    int32_t counter = 0;

    pc.baud(115200);
    sdc2130.baud(115200);
    
    limit_sw_rf.mode( PullUp );    // use internal pullup
    limit_sw_lb.mode( PullUp );    // use internal pullup

    DEBUG_PRINT_L0("\r\n");
    DEBUG_PRINT_L0("Bd1> +--------------------------------------------------------------\r\n");
    DEBUG_PRINT_L0("Bd1> | Project: B2 Crawler Explorer for 1F-1 PCV internal inspection\r\n");
    DEBUG_PRINT_L0("Bd1> |---------\r\n");
    DEBUG_PRINT_L0("Bd1> | This is: Each Motor Controller of Main Controller\r\n");
    DEBUG_PRINT_L0("Bd1> |   Letest update: %s\r\n", LatestUpDate);
    DEBUG_PRINT_L0("Bd1> |   Program Revision: %s\r\n", ProgramRevision);
    DEBUG_PRINT_L0("Bd1> |   Author: %s\r\n", Author);
    DEBUG_PRINT_L0("Bd1> |   Copyright(C) 2015 %s Allright Reserved\r\n", Company);
    DEBUG_PRINT_L0("Bd1> |  -------------------------------------------------\r\n");
    DEBUG_PRINT_L0("Bd1> |   RoboteQ motor control thread added\r\n");
    DEBUG_PRINT_L0("Bd1> |   2016.03.16 Limit switch control added\r\n");
    DEBUG_PRINT_L0("Bd1> +--------------------------------------------------------------\r\n");
    DEBUG_PRINT_L0("Bd1> \r\ninitializing ... \r\n");
    // All output off
//    motor1_out = 0;    
//    motor2_out = 0;

    i2c_saddress = I2C_ADDRESS_WINCH;       // 0x10
    //i2c_saddress = I2C_ADDRESS_TRANSFORM;   // 0x08
    //i2c_saddress = I2C_ADDRESS_CRAWLER;     // 0x04
    
    sdc2130.attach(&callback);
    sdc2130.printf( "^ECHOF 1\r" );
    sdc2130.printf( "^ECHOF 2\r" );

    led_demo();

    DEBUG_PRINT_L0("Bd1> > Board type: 0x%02x\r\n",i2c_saddress );
    
    // Set the initial value of motor current
    DEBUG_PRINT_L0("\r\nBd1> Read and set the initial value of motor current ... \r\n");
    set_init_motor_current(1, motor1_current_in.read()*100.0f, 1000);
    set_init_motor_current(2, motor2_current_in.read()*100.0f, 1000);
    
    DEBUG_PRINT_L0( "Bd1> *****************************************\r\n" );
    DEBUG_PRINT_L0( "Bd1> Init motor current1: %3.4f%\r\n", motor1_current_center_value );
    DEBUG_PRINT_L0( "Bd1> Init motor current2: %3.4f%\r\n", motor2_current_center_value );
    DEBUG_PRINT_L0( "Bd1> *****************************************\r\n" );

    // Thread (
    //    void(*task)(void const *argument), 
    //    void *argument=NULL, 
    //    osPriority priority=osPriorityNormal, 
    //    uint32_t stack_size=DEFAULT_STACK_SIZE, 
    //    unsigned char *stack_pointer=NULL
    //)

    // -----------------------------------------------------------------
    // In case of multi stack using , should increase stack size.
    // 2016.04.07
    // -----------------------------------------------------------------
    DEBUG_PRINT_L0("Bd1> Start RoboteQ motor driver task ... \r\n");
    Thread motorDrvControlTask1(RoboteQM1DrvCtrl_task, NULL, osPriorityNormal, 1024); 
    Thread motorDrvControlTask2(RoboteQM2DrvCtrl_task, NULL, osPriorityNormal, 1024); // 
    DEBUG_PRINT_L0("Bd1> Start motor control taslk (main thread) ... \r\n");
    Thread motorControlTask(motor_control_task, NULL, osPriorityNormal, 1024);
    DEBUG_PRINT_L0("Bd1> Initializing completed\r\n");
    
    // Reset the motor controller;
/*
    sdc2130.printf( "%RESET 32164987\r" );
    sdc2130.printf( "%RESET 32164987\r" );
    sdc2130.printf( "%RESET 32164987\r" );
    sdc2130.printf( "%RESET 32164987\r" );
    sdc2130.printf( "%RESET 32164987\r" );
*/
    sdc2130.printf( "^ECHOF 1\r" );
    sdc2130.printf( "^ECHOF 2\r" );
           
    while(1) {
        Thread::wait(20);
    //    sdc2130.printf( "?V\r" );
    //    sdc2130.printf( "^ECHOF 1\r" );
    //    sdc2130.printf( "^ECHOF 2\r" ); 
        counter++;
        if( counter >= 25 ){
            led2 = !led2;
            counter = 0;
 
            if( limit_sw_rf == 0 ){
                limitSw_Sts |= 0x01;
            }
            else{
                limitSw_Sts &= 0xFE;
            }           
            
            if( limit_sw_lb == 0 ){
                limitSw_Sts |= 0x02;
            }           
            else{
                limitSw_Sts &= 0xFD;
            }           
            
        }
        Thread::wait(5);
    }
}