Diff: i2cAccess.cpp

Revision:

0:19075177391c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/i2cAccess.cpp	Thu Jul 26 00:18:49 2018 +0000
@@ -0,0 +1,242 @@
+#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()
+{
+    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;
+}
+*/
+
+// --------------------------------------------------------------------------------------
+//  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;    
+}