z ysaito / Mbed 2 deprecated CPro_DemoB1B2_0_Can_Class

2018.07.26

Dependencies:   EthernetInterface TextLCD USBDevice USBHost mbed

i2cAccess.cpp@1:fdf87a1a724b, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:26:07 2018 +0000
Revision:
1:fdf87a1a724b
Parent:
0:19075177391c 
2018.07.26

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:19075177391c 1 #include "mbed.h"
sayzyas 0:19075177391c 2 #include "rtos.h"
sayzyas 0:19075177391c 3 #include "stdio.h"
sayzyas 0:19075177391c 4 #include "common.h"
sayzyas 0:19075177391c 5 #include "com_func.h"
sayzyas 0:19075177391c 6 #include "i2cAccess.h"
sayzyas 0:19075177391c 7
sayzyas 0:19075177391c 8 I2C i2c(p9, p10); // I2C SDA, SCL is good
sayzyas 0:19075177391c 9
sayzyas 0:19075177391c 10 i2cAccess::i2cAccess()
sayzyas 0:19075177391c 11 {
sayzyas 0:19075177391c 12 flg_motor_lock = false;
sayzyas 0:19075177391c 13 }
sayzyas 0:19075177391c 14
sayzyas 0:19075177391c 15 bool i2cAccess::i2c_read( int address, const char* data, int length)
sayzyas 0:19075177391c 16 {
sayzyas 0:19075177391c 17 ;
sayzyas 0:19075177391c 18 }
sayzyas 0:19075177391c 19
sayzyas 0:19075177391c 20 bool i2cAccess::i2c_write( int address, const char* data, int length )
sayzyas 0:19075177391c 21 {
sayzyas 0:19075177391c 22 int rts;
sayzyas 0:19075177391c 23 rts = i2c.write(address, data, length);
sayzyas 0:19075177391c 24 if( rts == 0 ) // i2c write success = 0
sayzyas 0:19075177391c 25 {
sayzyas 0:19075177391c 26 return true;
sayzyas 0:19075177391c 27 }
sayzyas 0:19075177391c 28 else // non 0 is failure
sayzyas 0:19075177391c 29 {
sayzyas 0:19075177391c 30 return false;
sayzyas 0:19075177391c 31 }
sayzyas 0:19075177391c 32 }
sayzyas 0:19075177391c 33
sayzyas 0:19075177391c 34 // --------------------------------------------------------------------------------------
sayzyas 0:19075177391c 35 // I2C read motor current
sayzyas 0:19075177391c 36 // --------------------------------------------------------------------------------------
sayzyas 0:19075177391c 37 int16_t i2cAccess::i2cReadMotorCurrent(
sayzyas 0:19075177391c 38 int32_t i2c_addr, // i2c address
sayzyas 0:19075177391c 39 int motor_id, // Motor CAN ID
sayzyas 0:19075177391c 40 int motor_no, // Motor number: 0 or 1
sayzyas 0:19075177391c 41 int motor_dir, // Motor direction 0:Fwd, 1:Rvs
sayzyas 0:19075177391c 42 int threshold // Motor current threshold
sayzyas 0:19075177391c 43 ){
sayzyas 0:19075177391c 44
sayzyas 0:19075177391c 45 int motor_current = 100;
sayzyas 0:19075177391c 46
sayzyas 0:19075177391c 47 // read motor current several times and judgemant.
sayzyas 0:19075177391c 48
sayzyas 0:19075177391c 49 if(true)
sayzyas 0:19075177391c 50 {
sayzyas 0:19075177391c 51 return 0; // motor unlock
sayzyas 0:19075177391c 52 }
sayzyas 0:19075177391c 53 else
sayzyas 0:19075177391c 54 {
sayzyas 0:19075177391c 55 return -1; // motor lock
sayzyas 0:19075177391c 56 }
sayzyas 0:19075177391c 57 }
sayzyas 0:19075177391c 58
sayzyas 0:19075177391c 59 /*
sayzyas 0:19075177391c 60 // ============================================================
sayzyas 0:19075177391c 61 // Read motor current
sayzyas 0:19075177391c 62 // ============================================================
sayzyas 0:19075177391c 63 int read_motorCurrent(
sayzyas 0:19075177391c 64 int motor_id, // Motor CAN ID
sayzyas 0:19075177391c 65 int motor_no, // Motor Number ( 1 or 2 )
sayzyas 0:19075177391c 66 int motor_dir // Motor direction
sayzyas 0:19075177391c 67 ){
sayzyas 0:19075177391c 68
sayzyas 0:19075177391c 69 float motor_current;
sayzyas 0:19075177391c 70 int i;
sayzyas 0:19075177391c 71 int mc_abs_pct = 0;
sayzyas 0:19075177391c 72 int mc_Threshold = 0;
sayzyas 0:19075177391c 73
sayzyas 0:19075177391c 74 if( motor_id == MCTR_CANID_PANTILTWCH )
sayzyas 0:19075177391c 75 {
sayzyas 0:19075177391c 76 if (motor_no == MOTOR_2 ){
sayzyas 0:19075177391c 77 //motor_current = mcnt_panwdm.read()*100.0f;
sayzyas 0:19075177391c 78 mc_abs_pct = abs((int)((motor_current - motor1_current_center_value)*100.0f));
sayzyas 0:19075177391c 79 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 );
sayzyas 0:19075177391c 80
sayzyas 0:19075177391c 81 for( i = 0; i < (mc_abs_pct/10); i++){
sayzyas 0:19075177391c 82 DEBUG_PRINT_L1(">");
sayzyas 0:19075177391c 83 }
sayzyas 0:19075177391c 84 DEBUG_PRINT_L4("\r\n");
sayzyas 0:19075177391c 85
sayzyas 0:19075177391c 86 if( motor_dir == MOTOR_FWD ){
sayzyas 0:19075177391c 87 mc_Threshold = (int)motor1_current_fwd_thd;
sayzyas 0:19075177391c 88 DEBUG_PRINT_L1("Bd0> Upper threshold: %d\r\n", mc_Threshold);
sayzyas 0:19075177391c 89 }
sayzyas 0:19075177391c 90 else{
sayzyas 0:19075177391c 91 mc_Threshold = (int)motor1_current_rvs_thd;
sayzyas 0:19075177391c 92 DEBUG_PRINT_L1("Bd0> Lower threshold: %d\r\n", mc_Threshold);
sayzyas 0:19075177391c 93 }
sayzyas 0:19075177391c 94 if( mc_abs_pct > mc_Threshold ){
sayzyas 0:19075177391c 95 DEBUG_PRINT_L1("Bd0> **** MC1 Over the Limit [%d] ****\r\n", motor1_lock_count );
sayzyas 0:19075177391c 96 motor_pantiltwch_1_lock_count += 1;
sayzyas 0:19075177391c 97 if( motor_pantiltwch_1_lock_count >= MC_LOCK_COUNT ){
sayzyas 0:19075177391c 98 stdio_mutex.lock(); // Mutex Lock
sayzyas 0:19075177391c 99 flg_motor_pantiltwch_1_lock = 1;
sayzyas 0:19075177391c 100 stdio_mutex.unlock(); // Mutex Release
sayzyas 0:19075177391c 101 DEBUG_PRINT_L1("Bd0> #### MOTOR1 LOCK ! #### (%d)\r\n", flg_motor_pantiltwch_1_lock);
sayzyas 0:19075177391c 102 }
sayzyas 0:19075177391c 103 }
sayzyas 0:19075177391c 104 else{
sayzyas 0:19075177391c 105 DEBUG_PRINT_L1("Bd0> Pass\r\n");
sayzyas 0:19075177391c 106 if( motor_pantiltwch_1_lock_count > 0 ) flg_motor_pantiltwch_1_lock -= 1;
sayzyas 0:19075177391c 107 else motor_pantiltwch_1_lock_count = 0;
sayzyas 0:19075177391c 108 }
sayzyas 0:19075177391c 109 }
sayzyas 0:19075177391c 110 }
sayzyas 0:19075177391c 111 else if( motor_id == MCTR_CANID_TFM )
sayzyas 0:19075177391c 112 {
sayzyas 0:19075177391c 113 }
sayzyas 0:19075177391c 114 else if( motor_id == MCTR_CANID_CRW )
sayzyas 0:19075177391c 115 {
sayzyas 0:19075177391c 116 }
sayzyas 0:19075177391c 117 return mc_abs_pct;
sayzyas 0:19075177391c 118 }
sayzyas 0:19075177391c 119 */
sayzyas 0:19075177391c 120
sayzyas 0:19075177391c 121 // --------------------------------------------------------------------------------------
sayzyas 0:19075177391c 122 // I2C read winch current position
sayzyas 0:19075177391c 123 // --------------------------------------------------------------------------------------
sayzyas 0:19075177391c 124 int16_t i2cAccess::i2cReadInformation(
sayzyas 0:19075177391c 125 int32_t i2c_addr, // i2c address
sayzyas 0:19075177391c 126 int winchOffsetValue // Winch position offset value
sayzyas 0:19075177391c 127 ){
sayzyas 0:19075177391c 128 char I2C_data[9];
sayzyas 0:19075177391c 129 int16_t res_position = 0;
sayzyas 0:19075177391c 130
sayzyas 0:19075177391c 131 /*
sayzyas 0:19075177391c 132 [0]: 0x12
sayzyas 0:19075177391c 133 [1]: Position lower
sayzyas 0:19075177391c 134 [2]: Position upper
sayzyas 0:19075177391c 135 [3]: Motor lock flag 1:lock, 0:nolock
sayzyas 0:19075177391c 136 [4]: -
sayzyas 0:19075177391c 137 [5]: -
sayzyas 0:19075177391c 138 [6]: -
sayzyas 0:19075177391c 139 [7]: -
sayzyas 0:19075177391c 140 [8]: 0x34
sayzyas 0:19075177391c 141 */
sayzyas 0:19075177391c 142 Thread::wait(5);
sayzyas 0:19075177391c 143 i2c.read(i2c_addr, I2C_data, 9); // Read
sayzyas 0:19075177391c 144
sayzyas 0:19075177391c 145 if(( I2C_data[0] == 0x12 )&&( I2C_data[8] == 0x34 ))
sayzyas 0:19075177391c 146 {
sayzyas 0:19075177391c 147 res_position = (I2C_data[2] << 8) | I2C_data[1];
sayzyas 0:19075177391c 148 if( res_position == -1 ){
sayzyas 0:19075177391c 149 res_position = 8888;
sayzyas 0:19075177391c 150 }
sayzyas 0:19075177391c 151 else{
sayzyas 0:19075177391c 152 res_position += winchOffsetValue;
sayzyas 0:19075177391c 153 }
sayzyas 0:19075177391c 154 }
sayzyas 0:19075177391c 155 else{
sayzyas 0:19075177391c 156 res_position = 9999;
sayzyas 0:19075177391c 157 }
sayzyas 0:19075177391c 158
sayzyas 0:19075177391c 159 if( I2C_data[3] == 1 )
sayzyas 0:19075177391c 160 {
sayzyas 0:19075177391c 161 flg_motor_lock = true;
sayzyas 0:19075177391c 162 }
sayzyas 0:19075177391c 163 else{
sayzyas 0:19075177391c 164 flg_motor_lock = false;
sayzyas 0:19075177391c 165 }
sayzyas 0:19075177391c 166
sayzyas 0:19075177391c 167 return res_position;
sayzyas 0:19075177391c 168 }
sayzyas 0:19075177391c 169
sayzyas 0:19075177391c 170
sayzyas 0:19075177391c 171 // --------------------------------------------------------------------------------------
sayzyas 0:19075177391c 172 // I2C write acccess: set motor threshold all
sayzyas 0:19075177391c 173 // --------------------------------------------------------------------------------------
sayzyas 0:19075177391c 174 bool i2cAccess::i2cSetMotorThreshold(
sayzyas 0:19075177391c 175 int32_t i2c_addr,
sayzyas 0:19075177391c 176 int8_t moror_number,
sayzyas 0:19075177391c 177 int8_t motor_dir,
sayzyas 0:19075177391c 178 int16_t motor_thresold
sayzyas 0:19075177391c 179 ){
sayzyas 0:19075177391c 180 char sbuf[NumberOfI2CCommand];
sayzyas 0:19075177391c 181 /*
sayzyas 0:19075177391c 182 I2C_CP_COMMAND, // instruction command
sayzyas 0:19075177391c 183 I2C_CP_MOTORNO, // motor number
sayzyas 0:19075177391c 184 I2C_CP_M_DIR, // motor rotation direction
sayzyas 0:19075177391c 185 I2C_CP_M_CNTTH_U, // motor current limit detection threshold upper byte
sayzyas 0:19075177391c 186 I2C_CP_M_CNTTH_L, // motor current limit detection threshold lower byte
sayzyas 0:19075177391c 187 I2C_CP_WDRAM_DIA_UPPER, // winch dram motor diameter upper
sayzyas 0:19075177391c 188 I2C_CP_WDRAM_DIA_LOWER, // winch dram motor diameter lower
sayzyas 0:19075177391c 189 I2C_CP_CCABLE_DIA_UPPER, // cable diameter upper byte
sayzyas 0:19075177391c 190 I2C_CP_CCABLE_DIA_LOWER, // cable diameter lower byte
sayzyas 0:19075177391c 191 I2C_CP_RESOLVER_RESO, // resolver resolution (bit)
sayzyas 0:19075177391c 192 I2C_CP_PRESET_CPOS_UPPER, // preset position upper
sayzyas 0:19075177391c 193 I2C_CP_PRESET_CPOS_LOWER // preset position lower
sayzyas 0:19075177391c 194 */
sayzyas 0:19075177391c 195 sbuf[I2C_CP_COMMAND] = 'T'; // instruction command
sayzyas 0:19075177391c 196 sbuf[I2C_CP_MOTORNO] = moror_number; // motor number
sayzyas 0:19075177391c 197 sbuf[I2C_CP_M_DIR] = motor_dir; // motor rotation direction
sayzyas 0:19075177391c 198 sbuf[I2C_CP_M_CNTTH_U] = ( motor_thresold >> 8 ) & 0xFF;
sayzyas 0:19075177391c 199 sbuf[I2C_CP_M_CNTTH_L] = ( motor_thresold & 0xFF );
sayzyas 0:19075177391c 200
sayzyas 0:19075177391c 201 i2c_write( i2c_addr, sbuf, NumberOfI2CCommand );
sayzyas 0:19075177391c 202 wait_ms(1);
sayzyas 0:19075177391c 203
sayzyas 0:19075177391c 204 return true;
sayzyas 0:19075177391c 205 }
sayzyas 0:19075177391c 206
sayzyas 0:19075177391c 207
sayzyas 0:19075177391c 208 // --------------------------------------------------------------------------------------
sayzyas 0:19075177391c 209 // I2C write acccess: clear motor lock detection countter
sayzyas 0:19075177391c 210 // --------------------------------------------------------------------------------------
sayzyas 0:19075177391c 211 bool i2cAccess::i2cClearMLCnt(
sayzyas 0:19075177391c 212 int32_t i2c_addr
sayzyas 0:19075177391c 213 ){
sayzyas 0:19075177391c 214 char i2c_cmd[NumberOfI2CCommand];
sayzyas 0:19075177391c 215
sayzyas 0:19075177391c 216 i2c_cmd[I2C_CP_COMMAND] = 0x4f; // 'O' instruction command
sayzyas 0:19075177391c 217 i2c_cmd[I2C_CP_MOTORNO] = 0; // instruction command
sayzyas 0:19075177391c 218 i2c_cmd[I2C_CP_M_DIR] = 0; // motor rotation direction
sayzyas 0:19075177391c 219 i2c_write( i2c_addr, i2c_cmd, NumberOfI2CCommand );
sayzyas 0:19075177391c 220
sayzyas 0:19075177391c 221 return true;
sayzyas 0:19075177391c 222 }
sayzyas 0:19075177391c 223
sayzyas 0:19075177391c 224
sayzyas 0:19075177391c 225 // --------------------------------------------------------------------------------------
sayzyas 0:19075177391c 226 // I2C write acccess: get motor current and judgement
sayzyas 0:19075177391c 227 // --------------------------------------------------------------------------------------
sayzyas 0:19075177391c 228 bool i2cAccess::i2cGetMotorCurrent(
sayzyas 0:19075177391c 229 int32_t i2c_addr,
sayzyas 0:19075177391c 230 int8_t moror_number,
sayzyas 0:19075177391c 231 int8_t motor_dir
sayzyas 0:19075177391c 232 ){
sayzyas 0:19075177391c 233 char i2c_cmd[NumberOfI2CCommand];
sayzyas 0:19075177391c 234
sayzyas 0:19075177391c 235 i2c_cmd[I2C_CP_COMMAND] = 'G'; // instruction command
sayzyas 0:19075177391c 236 i2c_cmd[I2C_CP_MOTORNO] = moror_number; // instruction command
sayzyas 0:19075177391c 237 i2c_cmd[I2C_CP_M_DIR] = motor_dir; // motor rotation direction
sayzyas 0:19075177391c 238
sayzyas 0:19075177391c 239 i2c_write( i2c_addr, i2c_cmd, NumberOfI2CCommand );
sayzyas 0:19075177391c 240
sayzyas 0:19075177391c 241 return true;
sayzyas 0:19075177391c 242 }