z ysaito
/
CPro_DemoB1B2_4_ResAnaCtrl
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
mcchk.cpp
00001 #include "mbed.h" 00002 #include "common.h" 00003 #include <math.h> 00004 #include "mcchk.h" 00005 00006 00007 // Motor current 00008 AnalogIn mctr3_m2c(A5); // LB crawler 00009 AnalogIn mctr3_m1c(A4); // RF crawler 00010 AnalogIn mctr2_m2c(A3); // LB transform 00011 AnalogIn mctr2_m1c(A2); // RF Transform 00012 AnalogIn mctr1_m2c(A1); // Tilt 00013 AnalogIn mctr1_m1c(A0); // B1: Pan, B2:Winch 00014 00015 00016 mcchk::mcchk() 00017 { 00018 cnt_mclock_LBCRW_f = 0; // motor lock counter 00019 cnt_mclock_LBCRW_r = 0; // motor lock counter 00020 cnt_mclock_RFCRW_f = 0; // motor lock counter 00021 cnt_mclock_RFCRW_r = 0; // motor lock counter 00022 cnt_mclock_LBTFM_f = 0; // motor lock counter 00023 cnt_mclock_LBTFM_r = 0; // motor lock counter 00024 cnt_mclock_RFTFM_f = 0; // motor lock counter 00025 cnt_mclock_RFTFM_r = 0; // motor lock counter 00026 cnt_mclock_CMPAN_f = 0; // motor lock counter 00027 cnt_mclock_CMPAN_r = 0; // motor lock counter 00028 cnt_mclock_CTILT_f = 0; // motor lock counter 00029 cnt_mclock_CTILT_r = 0; // motor lock counter 00030 cnt_mclock_WINCH_f = 0; // motor lock counter 00031 cnt_mclock_WINCH_r = 0; // motor lock counter 00032 } 00033 00034 00035 // -------------------------------------------------------------- 00036 // Motor Current Calibration Function 00037 // Arg: Motor Number 1 - 3 00038 // -------------------------------------------------------------- 00039 void mcchk::set_init_mc( int cnt ) 00040 { 00041 00042 m_LBCRW_center_value = 0; 00043 m_RFCRW_center_value = 0; 00044 m_LBTFM_center_value = 0; 00045 m_RFTFM_center_value = 0; 00046 m_WINCH_center_value = 0; 00047 m_CMPAN_center_value = 0; 00048 m_CTILT_center_value = 0; 00049 00050 for( int i = 0; i < cnt; i++ ) 00051 { 00052 m_LBCRW_center_value += mctr3_m1c.read() * 100.0f; 00053 m_RFCRW_center_value += mctr3_m2c.read() * 100.0f; 00054 m_LBTFM_center_value += mctr2_m1c.read() * 100.0f; 00055 m_RFTFM_center_value += mctr2_m2c.read() * 100.0f; 00056 m_WINCH_center_value += mctr1_m1c.read() * 100.0f; 00057 m_CMPAN_center_value += mctr1_m1c.read() * 100.0f; 00058 m_CTILT_center_value += mctr1_m2c.read() * 100.0f; 00059 } 00060 00061 m_LBCRW_center_value /= cnt; 00062 m_RFCRW_center_value /= cnt; 00063 m_LBTFM_center_value /= cnt; 00064 m_RFTFM_center_value /= cnt; 00065 m_WINCH_center_value /= cnt; 00066 m_CMPAN_center_value /= cnt; 00067 m_CTILT_center_value /= cnt; 00068 } 00069 00070 00071 float mcchk::rd_motorCurrent( int no ) 00072 { 00073 if( no == 11 ) return mctr1_m1c.read()*100.0f; 00074 else if( no == 12 ) return mctr1_m2c.read()*100.0f; 00075 else if( no == 21 ) return mctr2_m1c.read()*100.0f; 00076 else if( no == 22 ) return mctr2_m2c.read()*100.0f; 00077 else if( no == 31 ) return mctr3_m1c.read()*100.0f; 00078 else if( no == 32 ) return mctr3_m2c.read()*100.0f; 00079 return 0; 00080 } 00081 00082 00083 bool mcchk::rdnchk_motorCurrent( 00084 int8_t motor_number, 00085 int8_t motor_dir, 00086 int8_t motor_lock_chk_cnt 00087 ){ 00088 bool mcchk = false; 00089 bool rts = false; 00090 00091 // Check RF-Crawler 00092 if( motor_number == MOTOR_RFCRW ) 00093 { 00094 if( motor_dir == MOTOR_DIR_FWD ) 00095 { 00096 mcchk = chk_motor_lock( mctr3_m1c.read()*100.0f, m_RFCRW_center_value, mc_th_RFCRW_f); 00097 if( mcchk == true ) 00098 { 00099 cnt_mclock_RFCRW_f += 1; 00100 } 00101 else 00102 { if( cnt_mclock_RFCRW_f > 0 ) 00103 { 00104 cnt_mclock_RFCRW_f -= 1; 00105 } 00106 else 00107 { 00108 cnt_mclock_RFCRW_f = 0; 00109 } 00110 } 00111 if( cnt_mclock_RFCRW_f > motor_lock_chk_cnt ) 00112 { 00113 flg_mclock_RFCRW = 1; 00114 } 00115 else 00116 { 00117 flg_mclock_RFCRW = 0; 00118 rts = false; // NO LOCK 00119 } 00120 } 00121 else 00122 { 00123 mcchk = chk_motor_lock( mctr3_m1c.read()*100.0f, m_RFCRW_center_value, mc_th_RFCRW_r); 00124 if( mcchk == true ) 00125 { 00126 cnt_mclock_RFCRW_r += 1; 00127 } 00128 else 00129 { if( cnt_mclock_RFCRW_r > 0 ) 00130 { 00131 cnt_mclock_RFCRW_r -= 1; 00132 } 00133 else 00134 { 00135 cnt_mclock_RFCRW_r = 0; 00136 } 00137 } 00138 if( cnt_mclock_RFCRW_r > motor_lock_chk_cnt ) 00139 { 00140 flg_mclock_RFCRW = 1; 00141 rts = true; // LOCK 00142 } 00143 else 00144 { 00145 flg_mclock_RFCRW = 0; 00146 rts = false; // NO LOCK 00147 } 00148 } 00149 } 00150 // Check LB-Crawler 00151 else if( motor_number == MOTOR_LBCRW ) 00152 { 00153 if( motor_dir == MOTOR_DIR_FWD ) 00154 { 00155 mcchk = chk_motor_lock( mctr3_m2c.read()*100.0f, m_LBCRW_center_value, mc_th_LBCRW_f); 00156 if( mcchk == true ) 00157 { 00158 cnt_mclock_LBCRW_f += 1; 00159 } 00160 else 00161 { if( cnt_mclock_LBCRW_f > 0 ) 00162 { 00163 cnt_mclock_LBCRW_f -= 1; 00164 } 00165 else 00166 { 00167 cnt_mclock_LBCRW_f = 0; 00168 } 00169 } 00170 if( cnt_mclock_LBCRW_f > motor_lock_chk_cnt ) 00171 { 00172 flg_mclock_LBCRW = 1; 00173 rts = true; // LOCK 00174 } 00175 else 00176 { 00177 flg_mclock_LBCRW = 0; 00178 rts = false; // LOCK 00179 } 00180 } 00181 else 00182 { 00183 mcchk = chk_motor_lock( mctr3_m2c.read()*100.0f, m_LBCRW_center_value, mc_th_LBCRW_r); 00184 if( mcchk == true ) 00185 { 00186 cnt_mclock_LBCRW_r += 1; 00187 } 00188 else 00189 { if( cnt_mclock_LBCRW_r > 0 ) 00190 { 00191 cnt_mclock_LBCRW_r -= 1; 00192 } 00193 else 00194 { 00195 cnt_mclock_LBCRW_r= 0; 00196 } 00197 } 00198 if( cnt_mclock_LBCRW_r > motor_lock_chk_cnt ) 00199 { 00200 flg_mclock_LBCRW = 1; 00201 rts = true; // LOCK 00202 } 00203 else 00204 { 00205 flg_mclock_LBCRW = 0; 00206 rts = false; // LOCK 00207 } 00208 } 00209 } 00210 // Check RF-Crawler 00211 else if( motor_number == MOTOR_RFTFM ) 00212 { 00213 if( motor_dir == MOTOR_DIR_FWD ) 00214 { 00215 mcchk = chk_motor_lock( mctr2_m1c.read()*100.0f, m_RFTFM_center_value, mc_th_RFTFM_f); 00216 if( mcchk == true ) 00217 { 00218 cnt_mclock_RFTFM_f += 1; 00219 } 00220 else 00221 { if( cnt_mclock_RFTFM_f > 0 ) 00222 { 00223 cnt_mclock_RFTFM_f -= 1; 00224 } 00225 else 00226 { 00227 cnt_mclock_RFTFM_f = 0; 00228 } 00229 } 00230 if( cnt_mclock_RFTFM_f > motor_lock_chk_cnt ) 00231 { 00232 flg_mclock_RFTFM = 1; 00233 rts = true; // LOCK 00234 } 00235 else 00236 { 00237 flg_mclock_RFTFM = 0; 00238 rts = false; // LOCK 00239 } 00240 } 00241 else 00242 { 00243 rts = chk_motor_lock( mctr2_m1c.read()*100.0f, m_RFTFM_center_value, mc_th_RFTFM_r); 00244 if( rts == true ) 00245 { 00246 cnt_mclock_RFTFM_r += 1; 00247 } 00248 else 00249 { if( cnt_mclock_RFTFM_r > 0 ) 00250 { 00251 cnt_mclock_RFTFM_r -= 1; 00252 } 00253 else 00254 { 00255 cnt_mclock_RFTFM_r = 0; 00256 } 00257 } 00258 if( cnt_mclock_RFTFM_r > motor_lock_chk_cnt ) 00259 { 00260 flg_mclock_RFTFM = 1; 00261 rts = true; // LOCK 00262 } 00263 else 00264 { 00265 flg_mclock_RFTFM = 0; 00266 rts = false; // LOCK 00267 } 00268 } 00269 } 00270 // Check LB-Transform 00271 else if( motor_number == MOTOR_LBTFM ) 00272 { 00273 if( motor_dir == MOTOR_DIR_FWD ) 00274 { 00275 mcchk = chk_motor_lock( mctr2_m2c.read()*100.0f, m_LBTFM_center_value, mc_th_LBTFM_f); 00276 if( mcchk == true ) 00277 { 00278 cnt_mclock_LBTFM_f += 1; 00279 } 00280 else 00281 { if( cnt_mclock_LBTFM_f > 0 ) 00282 { 00283 cnt_mclock_LBTFM_f -= 1; 00284 } 00285 else 00286 { 00287 cnt_mclock_LBTFM_f = 0; 00288 } 00289 } 00290 if( cnt_mclock_LBTFM_f > motor_lock_chk_cnt ) 00291 { 00292 flg_mclock_LBTFM = 1; 00293 rts = true; // LOCK 00294 } 00295 else 00296 { 00297 flg_mclock_LBTFM = 0; 00298 rts = false; // LOCK 00299 } 00300 } 00301 else 00302 { 00303 mcchk = chk_motor_lock( mctr2_m2c.read()*100.0f, m_LBTFM_center_value, mc_th_LBTFM_r); 00304 if( mcchk == true ) 00305 { 00306 cnt_mclock_LBTFM_r += 1; 00307 } 00308 else 00309 { if( cnt_mclock_LBTFM_r > 0 ) 00310 { 00311 cnt_mclock_LBTFM_r -= 1; 00312 } 00313 else 00314 { 00315 cnt_mclock_LBTFM_r = 0; 00316 } 00317 } 00318 if( cnt_mclock_LBTFM_r > motor_lock_chk_cnt ) 00319 { 00320 flg_mclock_LBTFM = 1; 00321 rts = true; // LOCK 00322 } 00323 else 00324 { 00325 flg_mclock_LBTFM = 0; 00326 rts = false; // LOCK 00327 } 00328 } 00329 } 00330 00331 // Check Winch 00332 else if( motor_number == MOTOR_WINCH ) 00333 { 00334 if( motor_dir == MOTOR_DIR_FWD ) 00335 { 00336 mcchk= chk_motor_lock( mctr1_m1c.read()*100.0f, m_WINCH_center_value, mc_th_WINCH_f); 00337 if( mcchk == true ) 00338 { 00339 cnt_mclock_WINCH_f += 1; 00340 } 00341 else 00342 { if( cnt_mclock_WINCH_f > 0 ) 00343 { 00344 cnt_mclock_WINCH_f -= 1; 00345 } 00346 else 00347 { 00348 cnt_mclock_WINCH_f = 0; 00349 } 00350 00351 } 00352 if( cnt_mclock_WINCH_f > motor_lock_chk_cnt ) 00353 { 00354 flg_mclock_WINCH = 1; 00355 rts = true; // LOCK 00356 } 00357 else 00358 { 00359 flg_mclock_WINCH = 0; 00360 rts = false; // LOCK 00361 } 00362 } 00363 else 00364 { 00365 mcchk = chk_motor_lock( mctr1_m1c.read()*100.0f, m_WINCH_center_value, mc_th_WINCH_r); 00366 if( mcchk == true ) 00367 { 00368 cnt_mclock_WINCH_r += 1; 00369 } 00370 else 00371 { if( cnt_mclock_WINCH_r > 0 ) 00372 { 00373 cnt_mclock_WINCH_r -= 1; 00374 } 00375 else 00376 { 00377 cnt_mclock_WINCH_r = 0; 00378 } 00379 } 00380 if( cnt_mclock_WINCH_r > motor_lock_chk_cnt ) 00381 { 00382 flg_mclock_WINCH = 1; 00383 rts = true; // LOCK 00384 } 00385 else 00386 { 00387 flg_mclock_WINCH = 0; 00388 rts = false; // LOCK 00389 } 00390 } 00391 } 00392 // Check Tilt 00393 else if( motor_number == MOTOR_CMPAN ) 00394 { 00395 if( motor_dir == MOTOR_DIR_FWD ) 00396 { 00397 mcchk = chk_motor_lock( mctr1_m1c.read()*100.0f, m_CMPAN_center_value, mc_th_CMPAN_f); 00398 if( mcchk == true ) 00399 { 00400 cnt_mclock_CMPAN_f += 1; 00401 } 00402 else 00403 { if( cnt_mclock_CMPAN_f > 0 ) 00404 { 00405 cnt_mclock_CMPAN_f -= 1; 00406 } 00407 else 00408 { 00409 cnt_mclock_CMPAN_f = 0; 00410 } 00411 00412 } 00413 if( cnt_mclock_CMPAN_f > motor_lock_chk_cnt ) 00414 { 00415 flg_mclock_CMPAN = 1; 00416 rts = true; // LOCK 00417 } 00418 else 00419 { 00420 flg_mclock_CMPAN = 0; 00421 rts = false; // LOCK 00422 } 00423 } 00424 else 00425 { 00426 mcchk = chk_motor_lock( mctr1_m1c.read()*100.0f, m_CMPAN_center_value, mc_th_CMPAN_r); 00427 if( mcchk == true ) 00428 { 00429 cnt_mclock_CMPAN_r += 1; 00430 } 00431 else 00432 { if( cnt_mclock_CMPAN_r > 0 ) 00433 { 00434 cnt_mclock_CMPAN_r -= 1; 00435 } 00436 else 00437 { 00438 cnt_mclock_CMPAN_r = 0; 00439 } 00440 } 00441 if( cnt_mclock_CMPAN_r > motor_lock_chk_cnt ) 00442 { 00443 flg_mclock_CMPAN = 1; 00444 rts = true; // LOCK 00445 } 00446 else 00447 { 00448 flg_mclock_CMPAN = 0; 00449 rts = false; // LOCK 00450 } 00451 } 00452 } 00453 // Check Tilt 00454 else if( motor_number == MOTOR_CTILT ) 00455 { 00456 if( motor_dir == MOTOR_DIR_FWD ) 00457 { 00458 mcchk = chk_motor_lock( mctr1_m2c.read()*100.0f, m_CTILT_center_value, mc_th_CTILT_f); 00459 if( mcchk == true ) 00460 { 00461 cnt_mclock_CTILT_f += 1; 00462 } 00463 else 00464 { if( cnt_mclock_CTILT_f > 0 ) 00465 { 00466 cnt_mclock_CTILT_f -= 1; 00467 } 00468 else 00469 { 00470 cnt_mclock_CTILT_f = 0; 00471 } 00472 } 00473 if( cnt_mclock_CTILT_f > motor_lock_chk_cnt ) 00474 { 00475 flg_mclock_CTILT = 1; 00476 rts = true; // LOCK 00477 } 00478 else 00479 { 00480 flg_mclock_CTILT = 0; 00481 rts = false; // LOCK 00482 } 00483 } 00484 else 00485 { 00486 mcchk = chk_motor_lock( mctr1_m2c.read()*100.0f, m_CTILT_center_value, mc_th_CTILT_r); 00487 if( mcchk == true ) 00488 { 00489 cnt_mclock_CTILT_r += 1; 00490 } 00491 else 00492 { if( cnt_mclock_CTILT_r > 0 ) 00493 { 00494 cnt_mclock_CTILT_r -= 1; 00495 } 00496 else 00497 { 00498 cnt_mclock_CTILT_r = 0; 00499 } 00500 } 00501 if( cnt_mclock_CTILT_r > motor_lock_chk_cnt ) 00502 { 00503 flg_mclock_CTILT = 1; 00504 rts = true; // LOCK 00505 } 00506 else 00507 { 00508 flg_mclock_CTILT = 0; 00509 rts = false; // LOCK 00510 } 00511 } 00512 } 00513 return rts; 00514 } 00515 00516 bool mcchk::chk_motor_lock( 00517 float motor_current, // motor current 00518 float motor_center_value, // motor current center value 00519 int mc_threshold // motor current threshold 00520 ){ 00521 // int32_t mc_abs_pct; 00522 00523 _cnt_now = motor_current; 00524 _cnt_center = motor_center_value; 00525 _cnt_th = mc_threshold; 00526 00527 mc_abs_pct = abs((int32_t)((motor_current - motor_center_value)*100.0f)); 00528 if( mc_abs_pct > mc_threshold ) 00529 { 00530 return true; // Motor current is over threshold. 00531 } 00532 else 00533 { 00534 return false; // Motor current isn't over threshold. 00535 } 00536 }
Generated on Fri Jul 29 2022 23:31:48 by
1.7.2