z ysaito
https://os.mbed.com/users/sayzyas
Dependencies: QEI TextLCD mbed
Diff: mcchk.cpp
- Revision:
- 0:73dd48be5ca6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mcchk.cpp Thu Jul 26 00:20:15 2018 +0000
@@ -0,0 +1,536 @@
+#include "mbed.h"
+#include "common.h"
+#include <math.h>
+#include "mcchk.h"
+
+
+// Motor current
+AnalogIn mctr3_m2c(A5); // LB crawler
+AnalogIn mctr3_m1c(A4); // RF crawler
+AnalogIn mctr2_m2c(A3); // LB transform
+AnalogIn mctr2_m1c(A2); // RF Transform
+AnalogIn mctr1_m2c(A1); // Tilt
+AnalogIn mctr1_m1c(A0); // B1: Pan, B2:Winch
+
+
+mcchk::mcchk()
+{
+ cnt_mclock_LBCRW_f = 0; // motor lock counter
+ cnt_mclock_LBCRW_r = 0; // motor lock counter
+ cnt_mclock_RFCRW_f = 0; // motor lock counter
+ cnt_mclock_RFCRW_r = 0; // motor lock counter
+ cnt_mclock_LBTFM_f = 0; // motor lock counter
+ cnt_mclock_LBTFM_r = 0; // motor lock counter
+ cnt_mclock_RFTFM_f = 0; // motor lock counter
+ cnt_mclock_RFTFM_r = 0; // motor lock counter
+ cnt_mclock_CMPAN_f = 0; // motor lock counter
+ cnt_mclock_CMPAN_r = 0; // motor lock counter
+ cnt_mclock_CTILT_f = 0; // motor lock counter
+ cnt_mclock_CTILT_r = 0; // motor lock counter
+ cnt_mclock_WINCH_f = 0; // motor lock counter
+ cnt_mclock_WINCH_r = 0; // motor lock counter
+}
+
+
+// --------------------------------------------------------------
+// Motor Current Calibration Function
+// Arg: Motor Number 1 - 3
+// --------------------------------------------------------------
+void mcchk::set_init_mc( int cnt )
+{
+
+ m_LBCRW_center_value = 0;
+ m_RFCRW_center_value = 0;
+ m_LBTFM_center_value = 0;
+ m_RFTFM_center_value = 0;
+ m_WINCH_center_value = 0;
+ m_CMPAN_center_value = 0;
+ m_CTILT_center_value = 0;
+
+ for( int i = 0; i < cnt; i++ )
+ {
+ m_LBCRW_center_value += mctr3_m1c.read() * 100.0f;
+ m_RFCRW_center_value += mctr3_m2c.read() * 100.0f;
+ m_LBTFM_center_value += mctr2_m1c.read() * 100.0f;
+ m_RFTFM_center_value += mctr2_m2c.read() * 100.0f;
+ m_WINCH_center_value += mctr1_m1c.read() * 100.0f;
+ m_CMPAN_center_value += mctr1_m1c.read() * 100.0f;
+ m_CTILT_center_value += mctr1_m2c.read() * 100.0f;
+ }
+
+ m_LBCRW_center_value /= cnt;
+ m_RFCRW_center_value /= cnt;
+ m_LBTFM_center_value /= cnt;
+ m_RFTFM_center_value /= cnt;
+ m_WINCH_center_value /= cnt;
+ m_CMPAN_center_value /= cnt;
+ m_CTILT_center_value /= cnt;
+}
+
+
+float mcchk::rd_motorCurrent( int no )
+{
+ if( no == 11 ) return mctr1_m1c.read()*100.0f;
+ else if( no == 12 ) return mctr1_m2c.read()*100.0f;
+ else if( no == 21 ) return mctr2_m1c.read()*100.0f;
+ else if( no == 22 ) return mctr2_m2c.read()*100.0f;
+ else if( no == 31 ) return mctr3_m1c.read()*100.0f;
+ else if( no == 32 ) return mctr3_m2c.read()*100.0f;
+ return 0;
+}
+
+
+bool mcchk::rdnchk_motorCurrent(
+ int8_t motor_number,
+ int8_t motor_dir,
+ int8_t motor_lock_chk_cnt
+){
+ bool mcchk = false;
+ bool rts = false;
+
+ // Check RF-Crawler
+ if( motor_number == MOTOR_RFCRW )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk = chk_motor_lock( mctr3_m1c.read()*100.0f, m_RFCRW_center_value, mc_th_RFCRW_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_RFCRW_f += 1;
+ }
+ else
+ { if( cnt_mclock_RFCRW_f > 0 )
+ {
+ cnt_mclock_RFCRW_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_RFCRW_f = 0;
+ }
+ }
+ if( cnt_mclock_RFCRW_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_RFCRW = 1;
+ }
+ else
+ {
+ flg_mclock_RFCRW = 0;
+ rts = false; // NO LOCK
+ }
+ }
+ else
+ {
+ mcchk = chk_motor_lock( mctr3_m1c.read()*100.0f, m_RFCRW_center_value, mc_th_RFCRW_r);
+ if( mcchk == true )
+ {
+ cnt_mclock_RFCRW_r += 1;
+ }
+ else
+ { if( cnt_mclock_RFCRW_r > 0 )
+ {
+ cnt_mclock_RFCRW_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_RFCRW_r = 0;
+ }
+ }
+ if( cnt_mclock_RFCRW_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_RFCRW = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_RFCRW = 0;
+ rts = false; // NO LOCK
+ }
+ }
+ }
+ // Check LB-Crawler
+ else if( motor_number == MOTOR_LBCRW )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk = chk_motor_lock( mctr3_m2c.read()*100.0f, m_LBCRW_center_value, mc_th_LBCRW_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_LBCRW_f += 1;
+ }
+ else
+ { if( cnt_mclock_LBCRW_f > 0 )
+ {
+ cnt_mclock_LBCRW_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_LBCRW_f = 0;
+ }
+ }
+ if( cnt_mclock_LBCRW_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_LBCRW = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_LBCRW = 0;
+ rts = false; // LOCK
+ }
+ }
+ else
+ {
+ mcchk = chk_motor_lock( mctr3_m2c.read()*100.0f, m_LBCRW_center_value, mc_th_LBCRW_r);
+ if( mcchk == true )
+ {
+ cnt_mclock_LBCRW_r += 1;
+ }
+ else
+ { if( cnt_mclock_LBCRW_r > 0 )
+ {
+ cnt_mclock_LBCRW_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_LBCRW_r= 0;
+ }
+ }
+ if( cnt_mclock_LBCRW_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_LBCRW = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_LBCRW = 0;
+ rts = false; // LOCK
+ }
+ }
+ }
+ // Check RF-Crawler
+ else if( motor_number == MOTOR_RFTFM )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk = chk_motor_lock( mctr2_m1c.read()*100.0f, m_RFTFM_center_value, mc_th_RFTFM_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_RFTFM_f += 1;
+ }
+ else
+ { if( cnt_mclock_RFTFM_f > 0 )
+ {
+ cnt_mclock_RFTFM_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_RFTFM_f = 0;
+ }
+ }
+ if( cnt_mclock_RFTFM_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_RFTFM = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_RFTFM = 0;
+ rts = false; // LOCK
+ }
+ }
+ else
+ {
+ rts = chk_motor_lock( mctr2_m1c.read()*100.0f, m_RFTFM_center_value, mc_th_RFTFM_r);
+ if( rts == true )
+ {
+ cnt_mclock_RFTFM_r += 1;
+ }
+ else
+ { if( cnt_mclock_RFTFM_r > 0 )
+ {
+ cnt_mclock_RFTFM_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_RFTFM_r = 0;
+ }
+ }
+ if( cnt_mclock_RFTFM_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_RFTFM = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_RFTFM = 0;
+ rts = false; // LOCK
+ }
+ }
+ }
+ // Check LB-Transform
+ else if( motor_number == MOTOR_LBTFM )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk = chk_motor_lock( mctr2_m2c.read()*100.0f, m_LBTFM_center_value, mc_th_LBTFM_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_LBTFM_f += 1;
+ }
+ else
+ { if( cnt_mclock_LBTFM_f > 0 )
+ {
+ cnt_mclock_LBTFM_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_LBTFM_f = 0;
+ }
+ }
+ if( cnt_mclock_LBTFM_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_LBTFM = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_LBTFM = 0;
+ rts = false; // LOCK
+ }
+ }
+ else
+ {
+ mcchk = chk_motor_lock( mctr2_m2c.read()*100.0f, m_LBTFM_center_value, mc_th_LBTFM_r);
+ if( mcchk == true )
+ {
+ cnt_mclock_LBTFM_r += 1;
+ }
+ else
+ { if( cnt_mclock_LBTFM_r > 0 )
+ {
+ cnt_mclock_LBTFM_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_LBTFM_r = 0;
+ }
+ }
+ if( cnt_mclock_LBTFM_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_LBTFM = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_LBTFM = 0;
+ rts = false; // LOCK
+ }
+ }
+ }
+
+ // Check Winch
+ else if( motor_number == MOTOR_WINCH )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk= chk_motor_lock( mctr1_m1c.read()*100.0f, m_WINCH_center_value, mc_th_WINCH_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_WINCH_f += 1;
+ }
+ else
+ { if( cnt_mclock_WINCH_f > 0 )
+ {
+ cnt_mclock_WINCH_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_WINCH_f = 0;
+ }
+
+ }
+ if( cnt_mclock_WINCH_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_WINCH = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_WINCH = 0;
+ rts = false; // LOCK
+ }
+ }
+ else
+ {
+ mcchk = chk_motor_lock( mctr1_m1c.read()*100.0f, m_WINCH_center_value, mc_th_WINCH_r);
+ if( mcchk == true )
+ {
+ cnt_mclock_WINCH_r += 1;
+ }
+ else
+ { if( cnt_mclock_WINCH_r > 0 )
+ {
+ cnt_mclock_WINCH_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_WINCH_r = 0;
+ }
+ }
+ if( cnt_mclock_WINCH_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_WINCH = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_WINCH = 0;
+ rts = false; // LOCK
+ }
+ }
+ }
+ // Check Tilt
+ else if( motor_number == MOTOR_CMPAN )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk = chk_motor_lock( mctr1_m1c.read()*100.0f, m_CMPAN_center_value, mc_th_CMPAN_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_CMPAN_f += 1;
+ }
+ else
+ { if( cnt_mclock_CMPAN_f > 0 )
+ {
+ cnt_mclock_CMPAN_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_CMPAN_f = 0;
+ }
+
+ }
+ if( cnt_mclock_CMPAN_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_CMPAN = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_CMPAN = 0;
+ rts = false; // LOCK
+ }
+ }
+ else
+ {
+ mcchk = chk_motor_lock( mctr1_m1c.read()*100.0f, m_CMPAN_center_value, mc_th_CMPAN_r);
+ if( mcchk == true )
+ {
+ cnt_mclock_CMPAN_r += 1;
+ }
+ else
+ { if( cnt_mclock_CMPAN_r > 0 )
+ {
+ cnt_mclock_CMPAN_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_CMPAN_r = 0;
+ }
+ }
+ if( cnt_mclock_CMPAN_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_CMPAN = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_CMPAN = 0;
+ rts = false; // LOCK
+ }
+ }
+ }
+ // Check Tilt
+ else if( motor_number == MOTOR_CTILT )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk = chk_motor_lock( mctr1_m2c.read()*100.0f, m_CTILT_center_value, mc_th_CTILT_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_CTILT_f += 1;
+ }
+ else
+ { if( cnt_mclock_CTILT_f > 0 )
+ {
+ cnt_mclock_CTILT_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_CTILT_f = 0;
+ }
+ }
+ if( cnt_mclock_CTILT_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_CTILT = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_CTILT = 0;
+ rts = false; // LOCK
+ }
+ }
+ else
+ {
+ mcchk = chk_motor_lock( mctr1_m2c.read()*100.0f, m_CTILT_center_value, mc_th_CTILT_r);
+ if( mcchk == true )
+ {
+ cnt_mclock_CTILT_r += 1;
+ }
+ else
+ { if( cnt_mclock_CTILT_r > 0 )
+ {
+ cnt_mclock_CTILT_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_CTILT_r = 0;
+ }
+ }
+ if( cnt_mclock_CTILT_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_CTILT = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_CTILT = 0;
+ rts = false; // LOCK
+ }
+ }
+ }
+ return rts;
+}
+
+bool mcchk::chk_motor_lock(
+ float motor_current, // motor current
+ float motor_center_value, // motor current center value
+ int mc_threshold // motor current threshold
+){
+// int32_t mc_abs_pct;
+
+ _cnt_now = motor_current;
+ _cnt_center = motor_center_value;
+ _cnt_th = mc_threshold;
+
+ mc_abs_pct = abs((int32_t)((motor_current - motor_center_value)*100.0f));
+ if( mc_abs_pct > mc_threshold )
+ {
+ return true; // Motor current is over threshold.
+ }
+ else
+ {
+ return false; // Motor current isn't over threshold.
+ }
+}