Alex Pirciu / Mbed 2 deprecated BFMC

a

Dependencies:   mbed mbed-rtos

src/SafetyStop/safetystopfunction.cpp@1:ceee5a608e7c, 2019-03-28 (annotated)

Committer:
alexpirciu
Date:
Thu Mar 28 07:44:42 2019 +0000
Revision:
1:ceee5a608e7c
assa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alexpirciu 1:ceee5a608e7c 1 /**
alexpirciu 1:ceee5a608e7c 2 ******************************************************************************
alexpirciu 1:ceee5a608e7c 3 * @file SafetyStopFunction.cpp
alexpirciu 1:ceee5a608e7c 4 * @author RBRO/PJ-IU
alexpirciu 1:ceee5a608e7c 5 * @version V1.0.0
alexpirciu 1:ceee5a608e7c 6 * @date day-month-2017
alexpirciu 1:ceee5a608e7c 7 * @brief This file contains the class definition for the safety stop activation
alexpirciu 1:ceee5a608e7c 8 * methods.
alexpirciu 1:ceee5a608e7c 9 ******************************************************************************
alexpirciu 1:ceee5a608e7c 10 */
alexpirciu 1:ceee5a608e7c 11
alexpirciu 1:ceee5a608e7c 12 #include <SafetyStop/safetystopfunction.hpp>
alexpirciu 1:ceee5a608e7c 13
alexpirciu 1:ceee5a608e7c 14
alexpirciu 1:ceee5a608e7c 15 /**
alexpirciu 1:ceee5a608e7c 16 * @brief Construct a new CSafetyStopFunction::CSafetyStopFunction object
alexpirciu 1:ceee5a608e7c 17 *
alexpirciu 1:ceee5a608e7c 18 * @param f_senLeftFront sensor mounteed on left front part
alexpirciu 1:ceee5a608e7c 19 * @param f_senMidLeftFront sensor mounteed on mid left front part
alexpirciu 1:ceee5a608e7c 20 * @param f_senCenterFront sensor mounted on middle front part
alexpirciu 1:ceee5a608e7c 21 * @param f_senMidRightFront sensor mounteed on mid right front part
alexpirciu 1:ceee5a608e7c 22 * @param f_senRightFront sensor mounteed on right front part
alexpirciu 1:ceee5a608e7c 23 */
alexpirciu 1:ceee5a608e7c 24 CSafetyStopFunction::CSafetyStopFunction(
alexpirciu 1:ceee5a608e7c 25 C_IR_DistanceSensorDriver& f_senLeftFront
alexpirciu 1:ceee5a608e7c 26 , C_IR_DistanceSensorDriver& f_senMidLeftFront
alexpirciu 1:ceee5a608e7c 27 , C_IR_DistanceSensorDriver& f_senCenterFront
alexpirciu 1:ceee5a608e7c 28 , C_IR_DistanceSensorDriver& f_senMidRightFront
alexpirciu 1:ceee5a608e7c 29 , C_IR_DistanceSensorDriver& f_senRightFront
alexpirciu 1:ceee5a608e7c 30 // , C_IR_DistanceSensorDriver& f_senLeftBack
alexpirciu 1:ceee5a608e7c 31 // , C_IR_DistanceSensorDriver& f_senRightBack
alexpirciu 1:ceee5a608e7c 32 )
alexpirciu 1:ceee5a608e7c 33 : m_active(false)
alexpirciu 1:ceee5a608e7c 34 ,m_speed(0)
alexpirciu 1:ceee5a608e7c 35 , m_angle(0)
alexpirciu 1:ceee5a608e7c 36 , m_senLeftFront(f_senLeftFront)
alexpirciu 1:ceee5a608e7c 37 , m_senMidLeftFront(f_senMidLeftFront)
alexpirciu 1:ceee5a608e7c 38 , m_senCenterFront(f_senCenterFront)
alexpirciu 1:ceee5a608e7c 39 , m_senMidRightFront(f_senMidRightFront)
alexpirciu 1:ceee5a608e7c 40 , m_senRightFront(f_senRightFront)
alexpirciu 1:ceee5a608e7c 41 // , m_senLeftBach(f_senLeftBack)
alexpirciu 1:ceee5a608e7c 42 // , m_senRightBach(f_senRightBack)
alexpirciu 1:ceee5a608e7c 43 {
alexpirciu 1:ceee5a608e7c 44 }
alexpirciu 1:ceee5a608e7c 45
alexpirciu 1:ceee5a608e7c 46 /** @brief Class destructor
alexpirciu 1:ceee5a608e7c 47 *
alexpirciu 1:ceee5a608e7c 48 * Destructor method
alexpirciu 1:ceee5a608e7c 49 */
alexpirciu 1:ceee5a608e7c 50 CSafetyStopFunction::~CSafetyStopFunction()
alexpirciu 1:ceee5a608e7c 51 {}
alexpirciu 1:ceee5a608e7c 52
alexpirciu 1:ceee5a608e7c 53 /** @brief Function for activating safety stop
alexpirciu 1:ceee5a608e7c 54 *
alexpirciu 1:ceee5a608e7c 55 * @param f_speed
alexpirciu 1:ceee5a608e7c 56 * @param f_angle
alexpirciu 1:ceee5a608e7c 57 */
alexpirciu 1:ceee5a608e7c 58 bool CSafetyStopFunction::isSafetyStopActive(float f_speed, float f_angle){
alexpirciu 1:ceee5a608e7c 59 // Verification the state of the functionality
alexpirciu 1:ceee5a608e7c 60 if(!this->m_active){
alexpirciu 1:ceee5a608e7c 61 // Inactive -> no need to brake
alexpirciu 1:ceee5a608e7c 62 return false;
alexpirciu 1:ceee5a608e7c 63 }
alexpirciu 1:ceee5a608e7c 64 // Get current speed
alexpirciu 1:ceee5a608e7c 65 m_speed = f_speed;
alexpirciu 1:ceee5a608e7c 66 // Get current angle
alexpirciu 1:ceee5a608e7c 67 m_angle = f_angle;
alexpirciu 1:ceee5a608e7c 68 // Value indicating whether brake should be activated
alexpirciu 1:ceee5a608e7c 69 bool ret_val =false;
alexpirciu 1:ceee5a608e7c 70 // dyunamic threshold for activating brake
alexpirciu 1:ceee5a608e7c 71 float threshold;
alexpirciu 1:ceee5a608e7c 72 // If moving forward take into account only front sensors
alexpirciu 1:ceee5a608e7c 73 if (m_speed > 0)
alexpirciu 1:ceee5a608e7c 74 {
alexpirciu 1:ceee5a608e7c 75 threshold = m_speed*5-(1/m_speed)*10;
alexpirciu 1:ceee5a608e7c 76 if ((m_senLeftFront.getValue() <= threshold) ||
alexpirciu 1:ceee5a608e7c 77 (m_senMidLeftFront.getValue() <= threshold) ||
alexpirciu 1:ceee5a608e7c 78 (m_senCenterFront.getValue() <= threshold) ||
alexpirciu 1:ceee5a608e7c 79 (m_senMidRightFront.getValue() <= threshold) ||
alexpirciu 1:ceee5a608e7c 80 (m_senRightFront.getValue() <= threshold) )
alexpirciu 1:ceee5a608e7c 81 {
alexpirciu 1:ceee5a608e7c 82 ret_val = true;
alexpirciu 1:ceee5a608e7c 83 }
alexpirciu 1:ceee5a608e7c 84 }
alexpirciu 1:ceee5a608e7c 85 // If moving forward take into account only front sensors
alexpirciu 1:ceee5a608e7c 86 // Uncomment next if clause only if back sensors are being mounted
alexpirciu 1:ceee5a608e7c 87 // else if (m_speed < 0)
alexpirciu 1:ceee5a608e7c 88 // {
alexpirciu 1:ceee5a608e7c 89 // threshold = m_speed*8+(1/m_speed)*10;
alexpirciu 1:ceee5a608e7c 90 // if ( (m_senLeftBach.getDistance() <= threshold*(-1)) || (m_senRightBach.getDistance() <= threshold*(-1)) )
alexpirciu 1:ceee5a608e7c 91 // {
alexpirciu 1:ceee5a608e7c 92 // ret_val = true;
alexpirciu 1:ceee5a608e7c 93 // }
alexpirciu 1:ceee5a608e7c 94 // }
alexpirciu 1:ceee5a608e7c 95
alexpirciu 1:ceee5a608e7c 96 // Return value indicating whether brake should be activated
alexpirciu 1:ceee5a608e7c 97 return ret_val;
alexpirciu 1:ceee5a608e7c 98 }
alexpirciu 1:ceee5a608e7c 99
alexpirciu 1:ceee5a608e7c 100 /** @brief Serial callback method
alexpirciu 1:ceee5a608e7c 101 *
alexpirciu 1:ceee5a608e7c 102 * Serial callback attaching serial callback to CSafetyStopFunction object
alexpirciu 1:ceee5a608e7c 103 *
alexpirciu 1:ceee5a608e7c 104 * @param obj CSafetyStopFunction controller object
alexpirciu 1:ceee5a608e7c 105 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 106 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 107 *
alexpirciu 1:ceee5a608e7c 108 */
alexpirciu 1:ceee5a608e7c 109 void CSafetyStopFunction::staticSerialCallback(void* obj,char const * a, char * b)
alexpirciu 1:ceee5a608e7c 110 {
alexpirciu 1:ceee5a608e7c 111 CSafetyStopFunction* self = static_cast<CSafetyStopFunction*>(obj);
alexpirciu 1:ceee5a608e7c 112 self->serialCallback(a,b);
alexpirciu 1:ceee5a608e7c 113 }
alexpirciu 1:ceee5a608e7c 114
alexpirciu 1:ceee5a608e7c 115 /** @brief Serial callback actions
alexpirciu 1:ceee5a608e7c 116 *
alexpirciu 1:ceee5a608e7c 117 * Serial callback method setting controller to values received
alexpirciu 1:ceee5a608e7c 118 *
alexpirciu 1:ceee5a608e7c 119 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 120 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 121 *
alexpirciu 1:ceee5a608e7c 122 */
alexpirciu 1:ceee5a608e7c 123 void CSafetyStopFunction::serialCallback(char const * a, char * b){
alexpirciu 1:ceee5a608e7c 124 int l_isActivate=0;
alexpirciu 1:ceee5a608e7c 125 uint32_t l_res= sscanf(a,"%d",&l_isActivate);
alexpirciu 1:ceee5a608e7c 126 if(l_res==1){
alexpirciu 1:ceee5a608e7c 127 m_active=(l_isActivate>=1);
alexpirciu 1:ceee5a608e7c 128 sprintf(b,"ack;;");
alexpirciu 1:ceee5a608e7c 129 }else{
alexpirciu 1:ceee5a608e7c 130 sprintf(b,"sintax error;;");
alexpirciu 1:ceee5a608e7c 131 }
alexpirciu 1:ceee5a608e7c 132 }
alexpirciu 1:ceee5a608e7c 133
alexpirciu 1:ceee5a608e7c 134
alexpirciu 1:ceee5a608e7c 135