File content as of revision 0:b3376afd10d8:

#include "mbed.h"
#include "rtos.h"
#include "stdio.h"
#include "common.h"
//#include "com_func.h"
#include "i2cAccess.h"

I2C i2c(p9, p10);       // I2C SDA, SCL is good

i2cAccess::i2cAccess()
{
    sp_wdram_f = 100;     // motor speed
    sp_wdram_r = 100;     // motor speed     
    sp_wcabl_f = 100;     // motor speed     
    sp_wcabl_r = 100;     // motor speed  
    flg_motor_lock = false;
}

bool i2cAccess::i2c_read( int address, const char* data, int length)
{
;
}
    
bool i2cAccess::i2c_write( int address, const char* data, int length )
{
    int rts;
    rts = i2c.write(address, data, length);
    if( rts == 0 ) // i2c write success = 0
    {
        return true;
    }
    else // non 0 is failure
    {
        return false;
    }
}

// --------------------------------------------------------------------------------------
//  I2C read motor current
// --------------------------------------------------------------------------------------
int16_t i2cAccess::i2cReadMotorCurrent(
    int32_t i2c_addr,   // i2c address
    int motor_id,       // Motor CAN ID
    int motor_no,       // Motor number: 0 or 1
    int motor_dir,      // Motor direction 0:Fwd, 1:Rvs
    int threshold       // Motor current threshold
){

    int motor_current = 100;
    
    // read motor current several times and judgemant.

    if(true)
    {
        return 0;   // motor unlock
    }
    else
    {
        return -1;  // motor lock
    }
}

/*
// ============================================================
// Read motor current
// ============================================================
int read_motorCurrent(
    int motor_id,   // Motor CAN ID 
    int motor_no,   // Motor Number ( 1 or 2 )
    int motor_dir   // Motor direction 
){

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

    if( motor_id == MCTR_CANID_PANTILTWCH )
    {
        if (motor_no == MOTOR_2 ){
            //motor_current = mcnt_panwdm.read()*100.0f;
            mc_abs_pct = abs((int)((motor_current - motor1_current_center_value)*100.0f));
            DEBUG_PRINT_L1("Bd0> M1:%lf/%lf= [%03d%%] th:%3.2f< <%3.2f\t", motor_current, motor1_current_center_value, mc_abs_pct, motor1_current_rvs_thd, motor1_current_fwd_thd );

            for( i = 0; i < (mc_abs_pct/10); i++){
                DEBUG_PRINT_L1(">");
            }
            DEBUG_PRINT_L4("\r\n");
        
            if( motor_dir == MOTOR_FWD ){
                mc_Threshold = (int)motor1_current_fwd_thd;
                    DEBUG_PRINT_L1("Bd0> Upper threshold: %d\r\n", mc_Threshold);
            }
            else{
                mc_Threshold = (int)motor1_current_rvs_thd;
                DEBUG_PRINT_L1("Bd0> Lower threshold: %d\r\n", mc_Threshold);
            }
            if( mc_abs_pct > mc_Threshold ){
                DEBUG_PRINT_L1("Bd0> **** MC1 Over the Limit [%d] ****\r\n", motor1_lock_count );
                motor_pantiltwch_1_lock_count += 1;
                if( motor_pantiltwch_1_lock_count >= MC_LOCK_COUNT ){
                    stdio_mutex.lock();     // Mutex Lock
                    flg_motor_pantiltwch_1_lock = 1;
                    stdio_mutex.unlock();   // Mutex Release
                    DEBUG_PRINT_L1("Bd0> #### MOTOR1 LOCK ! #### (%d)\r\n", flg_motor_pantiltwch_1_lock);
                }
            }
            else{
                DEBUG_PRINT_L1("Bd0> Pass\r\n");
                if( motor_pantiltwch_1_lock_count > 0 ) flg_motor_pantiltwch_1_lock -= 1;
                else motor_pantiltwch_1_lock_count = 0;
            }
        }
    }
    else if( motor_id == MCTR_CANID_TFM )
    {
    }
    else if( motor_id == MCTR_CANID_CRW )
    {
    }
    return mc_abs_pct;
}
*/


//    int16_t     sp_wdram_f;     // motor speed
//    int16_t     sp_wdram_r;     // motor speed     
//    int16_t     sp_wcabl_f;     // motor speed     
//    int16_t     sp_wcabl_r;     // motor speed   
bool i2cAccess::i2cReadParameters(
    int32_t i2c_addr    // i2c address
){
   char        I2C_data[12];
   /*
     [00]: 0x12
     [01]: motot speed dram motor forward (CW) upper
     [02]: motot speed dram motor forward (CW) lower
     [03]: motot speed dram motor reverse (CCW) upper
     [04]: motot speed dram motor reverse (CCW) lower
     [05]: motot speed cable motor forward (CW) upper
     [06]: motot speed cable motor forward (CW) lower
     [07]: motot speed cable motor forward (CCW) upper
     [08]: motot speed cable motor forward (CCW) lower
     [09]: winch dropped ammount upper 
     [10]: winch dropped ammount lower
     [11]: 0x34
   */
    Thread::wait(5);
    i2c.read(i2c_addr, I2C_data, 12); // Read

    if(( I2C_data[0] == 0x12 )&&( I2C_data[11] == 0x34 ))
    {
        sp_wdram_f = (I2C_data[2] << 8) | I2C_data[1]; 
        if( sp_wdram_f <= 3 ) 
            sp_wdram_f = 0;
        else if( sp_wdram_f >= 97 )  
            sp_wdram_f = 100;
            
        sp_wdram_r = (I2C_data[4] << 8) | I2C_data[3]; 
        if( sp_wdram_r <= 3 ) 
            sp_wdram_r = 0;
        else if( sp_wdram_r >= 97 )  
            sp_wdram_r = 100;
        
        sp_wcabl_f = (I2C_data[6] << 8) | I2C_data[5]; 
        if( sp_wcabl_f <= 3 ) 
            sp_wcabl_f = 0;
        else if( sp_wcabl_f >= 97 )  
            sp_wcabl_f = 100;
        
        sp_wcabl_r = (I2C_data[8] << 8) | I2C_data[7];
        if( sp_wcabl_r <= 3 ) 
            sp_wcabl_r = 0;
        else if( sp_wcabl_r >= 97 )  
            sp_wcabl_r = 100;

        drop_ammount = (I2C_data[10] << 8) | I2C_data[9];
            
        return true;
    }
    else
    {
        return false;
    }
}


// --------------------------------------------------------------------------------------
//  I2C read winch current position
// --------------------------------------------------------------------------------------
int16_t i2cAccess::i2cReadInformation( 
    int32_t i2c_addr,           // i2c address
    int     winchOffsetValue    // Winch position offset value
){
    char        I2C_data[9];
    int16_t     res_position = 0;
    
    /*
        [0]: 0x12
        [1]: Position lower
        [2]: Position upper
        [3]: Motor lock flag 1:lock, 0:nolock
        [4]: -
        [5]: -
        [6]: -
        [7]: -
        [8]: 0x34
    */
    Thread::wait(5);
    i2c.read(i2c_addr, I2C_data, 9); // Read

    if(( I2C_data[0] == 0x12 )&&( I2C_data[8] == 0x34 ))
    {
        res_position = (I2C_data[2] << 8) | I2C_data[1]; 
        if( res_position == -1 ){
            res_position = 8888;
        }
        else{
            res_position += winchOffsetValue;
        }
    }
    else{
        res_position = 9999;
    }
    
    if( I2C_data[3] == 1 )
    {
        flg_motor_lock = true;
    }
    else{
        flg_motor_lock = false;
    }
    
    return res_position;
}


// --------------------------------------------------------------------------------------
//  I2C write acccess: set motor threshold all
// --------------------------------------------------------------------------------------
bool i2cAccess::i2cSetMotorThreshold(
    int32_t     i2c_addr,
    int8_t      moror_number, 
    int8_t      motor_dir,
    int16_t     motor_thresold
){
    char sbuf[NumberOfI2CCommand];
/*    
    I2C_CP_COMMAND,             // instruction command
    I2C_CP_MOTORNO,             // motor number
    I2C_CP_M_DIR,               // motor rotation direction   
    I2C_CP_M_CNTTH_U,       // motor current limit detection threshold upper byte 
    I2C_CP_M_CNTTH_L,       // motor current limit detection threshold lower byte 
    I2C_CP_WDRAM_DIA_UPPER,     // winch dram motor diameter upper   
    I2C_CP_WDRAM_DIA_LOWER,     // winch dram motor diameter lower    
    I2C_CP_CCABLE_DIA_UPPER,    // cable diameter upper byte 
    I2C_CP_CCABLE_DIA_LOWER,    // cable diameter lower byte 
    I2C_CP_RESOLVER_RESO,       // resolver resolution (bit)
    I2C_CP_PRESET_CPOS_UPPER,   // preset position upper
    I2C_CP_PRESET_CPOS_LOWER   // preset position lower 
*/   
    sbuf[I2C_CP_COMMAND] = 'T';              // instruction command
    sbuf[I2C_CP_MOTORNO] = moror_number;     // motor number
    sbuf[I2C_CP_M_DIR] = motor_dir;          // motor rotation direction   
    sbuf[I2C_CP_M_CNTTH_U] = ( motor_thresold >> 8 ) & 0xFF;
    sbuf[I2C_CP_M_CNTTH_L] = ( motor_thresold & 0xFF );
   
    i2c_write( i2c_addr, sbuf, NumberOfI2CCommand );
    wait_ms(1);
    
    return true;    
}


// --------------------------------------------------------------------------------------
//  I2C write acccess: clear motor lock detection countter
// --------------------------------------------------------------------------------------
bool i2cAccess::i2cClearMLCnt(
    int32_t     i2c_addr
){
    char i2c_cmd[NumberOfI2CCommand];
    
    i2c_cmd[I2C_CP_COMMAND] = 0x4f; // 'O' instruction command
    i2c_cmd[I2C_CP_MOTORNO] = 0;     // instruction command
    i2c_cmd[I2C_CP_M_DIR] = 0;          // motor rotation direction   
    i2c_write( i2c_addr, i2c_cmd, NumberOfI2CCommand );
    
    return true; 
}


// --------------------------------------------------------------------------------------
//  I2C write acccess: get motor current and judgement
// --------------------------------------------------------------------------------------
bool i2cAccess::i2cGetMotorCurrent(
    int32_t     i2c_addr, 
    int8_t      moror_number, 
    int8_t      motor_dir
){
    char i2c_cmd[NumberOfI2CCommand];
    
    i2c_cmd[I2C_CP_COMMAND] = 'G';              // instruction command
    i2c_cmd[I2C_CP_MOTORNO] = moror_number;     // instruction command
    i2c_cmd[I2C_CP_M_DIR] = motor_dir;          // motor rotation direction   
   
    i2c_write( i2c_addr, i2c_cmd, NumberOfI2CCommand );
    
    return true;    
}