safetystopfunction.cpp

00001 /**
00002   ******************************************************************************
00003   * @file    SafetyStopFunction.cpp
00004   * @author  RBRO/PJ-IU
00005   * @version V1.0.0
00006   * @date    day-month-2017
00007   * @brief   This file contains the class definition for the safety stop activation
00008   *          methods.
00009   ******************************************************************************
00010  */
00011 
00012 #include <SafetyStop/safetystopfunction.hpp>
00013 
00014 
00015 /**
00016  * @brief Construct a new CSafetyStopFunction::CSafetyStopFunction object
00017  * 
00018  * @param f_senLeftFront sensor mounteed on left front part
00019  * @param f_senMidLeftFront sensor mounteed on mid left front part
00020  * @param f_senCenterFront sensor mounted on middle front part
00021  * @param f_senMidRightFront sensor mounteed on mid right front part
00022  * @param f_senRightFront sensor mounteed on right front part
00023  */
00024 CSafetyStopFunction::CSafetyStopFunction(
00025                       C_IR_DistanceSensorDriver& f_senLeftFront
00026                     , C_IR_DistanceSensorDriver& f_senMidLeftFront
00027                     , C_IR_DistanceSensorDriver& f_senCenterFront
00028                     , C_IR_DistanceSensorDriver& f_senMidRightFront
00029                     , C_IR_DistanceSensorDriver& f_senRightFront
00030                     // , C_IR_DistanceSensorDriver& f_senLeftBack
00031                     // , C_IR_DistanceSensorDriver& f_senRightBack
00032                     )
00033     : m_active(false) 
00034     ,m_speed(0)
00035     , m_angle(0)
00036     , m_senLeftFront(f_senLeftFront)
00037     , m_senMidLeftFront(f_senMidLeftFront)
00038     , m_senCenterFront(f_senCenterFront)
00039     , m_senMidRightFront(f_senMidRightFront)
00040     , m_senRightFront(f_senRightFront)
00041     // , m_senLeftBach(f_senLeftBack)
00042     // , m_senRightBach(f_senRightBack)
00043 {
00044 }
00045 
00046 /** @brief  Class destructor
00047  *
00048  *  Destructor method
00049  */
00050 CSafetyStopFunction::~CSafetyStopFunction()
00051 {}
00052 
00053 /** @brief  Function for activating safety stop
00054  * 
00055  *  @param  f_speed
00056  *  @param f_angle
00057  */
00058 bool CSafetyStopFunction::isSafetyStopActive(float f_speed, float f_angle){
00059     // Verification the state of the functionality
00060     if(!this->m_active){
00061         // Inactive -> no need to brake
00062         return false;
00063     }
00064     // Get current speed
00065     m_speed = f_speed;
00066     // Get current angle
00067     m_angle = f_angle;
00068     // Value indicating whether brake should be activated
00069     bool ret_val  =false;
00070     // dyunamic threshold for activating brake
00071     float threshold;
00072     // If moving forward take into account only front sensors
00073     if (m_speed > 0)
00074     {
00075         threshold = m_speed*5-(1/m_speed)*10;
00076         if ((m_senLeftFront.getValue() <= threshold) ||
00077             (m_senMidLeftFront.getValue() <= threshold) ||
00078             (m_senCenterFront.getValue() <= threshold) || 
00079             (m_senMidRightFront.getValue() <= threshold) || 
00080             (m_senRightFront.getValue() <= threshold)  )
00081         {
00082             ret_val = true;
00083         }
00084     }
00085     // If moving forward take into account only front sensors
00086     // Uncomment next if clause only if back sensors are being mounted
00087     // else if (m_speed < 0)
00088     //         {
00089     //             threshold = m_speed*8+(1/m_speed)*10;
00090     //             if ( (m_senLeftBach.getDistance() <= threshold*(-1)) || (m_senRightBach.getDistance() <= threshold*(-1))  )
00091     //             {
00092     //                 ret_val = true;
00093     //             }
00094     //         }
00095 
00096     // Return value indicating whether brake should be activated
00097     return ret_val;
00098 }
00099 
00100 /** @brief  Serial callback method
00101   *
00102   * Serial callback attaching serial callback to CSafetyStopFunction object
00103   *
00104   * @param  obj                 CSafetyStopFunction controller object
00105   * @param  a                   string to read data from
00106   * @param b                   string to write data to
00107   * 
00108   */
00109 void CSafetyStopFunction::staticSerialCallback(void* obj,char const * a, char * b)
00110 {
00111     CSafetyStopFunction* self = static_cast<CSafetyStopFunction*>(obj);
00112     self->serialCallback(a,b);
00113 }
00114 
00115 /** @brief  Serial callback actions
00116   *
00117   * Serial callback method setting controller to values received
00118   *
00119   * @param  a                   string to read data from
00120   * @param b                   string to write data to
00121   * 
00122   */
00123 void CSafetyStopFunction::serialCallback(char const * a, char * b){
00124         int l_isActivate=0;
00125         uint32_t l_res= sscanf(a,"%d",&l_isActivate);
00126         if(l_res==1){
00127             m_active=(l_isActivate>=1);
00128             sprintf(b,"ack;;");
00129         }else{
00130             sprintf(b,"sintax error;;");
00131         }
00132     }
00133 
00134 
00135