![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
a
src/SafetyStop/safetystopfunction.cpp
- Committer:
- alexpirciu
- Date:
- 2019-03-28
- Revision:
- 1:ceee5a608e7c
File content as of revision 1:ceee5a608e7c:
/** ****************************************************************************** * @file SafetyStopFunction.cpp * @author RBRO/PJ-IU * @version V1.0.0 * @date day-month-2017 * @brief This file contains the class definition for the safety stop activation * methods. ****************************************************************************** */ #include <SafetyStop/safetystopfunction.hpp> /** * @brief Construct a new CSafetyStopFunction::CSafetyStopFunction object * * @param f_senLeftFront sensor mounteed on left front part * @param f_senMidLeftFront sensor mounteed on mid left front part * @param f_senCenterFront sensor mounted on middle front part * @param f_senMidRightFront sensor mounteed on mid right front part * @param f_senRightFront sensor mounteed on right front part */ CSafetyStopFunction::CSafetyStopFunction( C_IR_DistanceSensorDriver& f_senLeftFront , C_IR_DistanceSensorDriver& f_senMidLeftFront , C_IR_DistanceSensorDriver& f_senCenterFront , C_IR_DistanceSensorDriver& f_senMidRightFront , C_IR_DistanceSensorDriver& f_senRightFront // , C_IR_DistanceSensorDriver& f_senLeftBack // , C_IR_DistanceSensorDriver& f_senRightBack ) : m_active(false) ,m_speed(0) , m_angle(0) , m_senLeftFront(f_senLeftFront) , m_senMidLeftFront(f_senMidLeftFront) , m_senCenterFront(f_senCenterFront) , m_senMidRightFront(f_senMidRightFront) , m_senRightFront(f_senRightFront) // , m_senLeftBach(f_senLeftBack) // , m_senRightBach(f_senRightBack) { } /** @brief Class destructor * * Destructor method */ CSafetyStopFunction::~CSafetyStopFunction() {} /** @brief Function for activating safety stop * * @param f_speed * @param f_angle */ bool CSafetyStopFunction::isSafetyStopActive(float f_speed, float f_angle){ // Verification the state of the functionality if(!this->m_active){ // Inactive -> no need to brake return false; } // Get current speed m_speed = f_speed; // Get current angle m_angle = f_angle; // Value indicating whether brake should be activated bool ret_val =false; // dyunamic threshold for activating brake float threshold; // If moving forward take into account only front sensors if (m_speed > 0) { threshold = m_speed*5-(1/m_speed)*10; if ((m_senLeftFront.getValue() <= threshold) || (m_senMidLeftFront.getValue() <= threshold) || (m_senCenterFront.getValue() <= threshold) || (m_senMidRightFront.getValue() <= threshold) || (m_senRightFront.getValue() <= threshold) ) { ret_val = true; } } // If moving forward take into account only front sensors // Uncomment next if clause only if back sensors are being mounted // else if (m_speed < 0) // { // threshold = m_speed*8+(1/m_speed)*10; // if ( (m_senLeftBach.getDistance() <= threshold*(-1)) || (m_senRightBach.getDistance() <= threshold*(-1)) ) // { // ret_val = true; // } // } // Return value indicating whether brake should be activated return ret_val; } /** @brief Serial callback method * * Serial callback attaching serial callback to CSafetyStopFunction object * * @param obj CSafetyStopFunction controller object * @param a string to read data from * @param b string to write data to * */ void CSafetyStopFunction::staticSerialCallback(void* obj,char const * a, char * b) { CSafetyStopFunction* self = static_cast<CSafetyStopFunction*>(obj); self->serialCallback(a,b); } /** @brief Serial callback actions * * Serial callback method setting controller to values received * * @param a string to read data from * @param b string to write data to * */ void CSafetyStopFunction::serialCallback(char const * a, char * b){ int l_isActivate=0; uint32_t l_res= sscanf(a,"%d",&l_isActivate); if(l_res==1){ m_active=(l_isActivate>=1); sprintf(b,"ack;;"); }else{ sprintf(b,"sintax error;;"); } }