z ysaito / Mbed OS APro_B2DTst_0_Can_Class

2018.07.26

Dependencies:   WebSocketClient

i2cAccess.cpp@0:b3376afd10d8, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:20:04 2018 +0000
Revision:
0:b3376afd10d8
2018.07.26

Who changed what in which revision?

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