z ysaito
2018.07.26
Diff: i2cAccess.cpp
- Revision:
- 0:b3376afd10d8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/i2cAccess.cpp Thu Jul 26 00:20:04 2018 +0000
@@ -0,0 +1,309 @@
+#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;
+}