File content as of revision 0:73dd48be5ca6:

#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.
    }
}