![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
a
Revision 1:ceee5a608e7c, committed 2019-03-28
- Comitter:
- alexpirciu
- Date:
- Thu Mar 28 07:44:42 2019 +0000
- Parent:
- 0:c3e774091195
- Commit message:
- assa
Changed in this revision
diff -r c3e774091195 -r ceee5a608e7c include/BezierCurve/beziercurve.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/BezierCurve/beziercurve.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,68 @@ +/** + ****************************************************************************** + * @file BezierCurve.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the Bezier Curve + * methods. + ****************************************************************************** + */ + +/* include guard */ +#ifndef BEZIERCURVE_HPP +#define BEZIERCURVE_HPP + +/* complex numbers in cartesian form and several functions and overloads to operate with them */ +#include <complex> +#include <BezierCurve/PolynomialFunction.hpp> + +#define BEZIER_ORDER 3 + +namespace math{ + /** + * @brief It implements the functionality of a Bezier curve. The points on the curve are represented by complex numbers and the input value of the curve have to belong to the interval [0,1]. + * + * @tparam T The type of the Bezier curve. + */ + template<class T> + class BezierCurve + { + public: + /* Constructors */ + BezierCurve(); + BezierCurve(std::complex<T> points[BEZIER_ORDER+1]); + BezierCurve(std::complex<T> a,std::complex<T> b,std::complex<T> c,std::complex<T> d); + /* Destructor */ + virtual ~BezierCurve(); + /* Set curve */ + void setBezierCurve(std::complex<T> a,std::complex<T> b,std::complex<T> c,std::complex<T> d); + /* Get value */ + std::complex<T> getValue(float input_value); + /* First order derivate value */ + std::complex<T> get_FO_DerivateValue(float input_value); + /* Second order derivate value */ + std::complex<T> get_SO_DerivateValue(float input_value); + + math::PolynomialFunction<std::complex<T>,BEZIER_ORDER > getBezierCurve(); + math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-1> getFODerivate(); + math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-2> getSODerivate(); + + protected: + + private: + // PRIVATE FUNCTIONS + // Convert the complex points to polynomial function + math::PolynomialFunction<std::complex<float>,BEZIER_ORDER> CP2PF(); + + // PRIVATE PARAMETERS + std::complex<T> points[BEZIER_ORDER+1]; + math::PolynomialFunction<std::complex<T>,BEZIER_ORDER> bezierCurve; + math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-1> FOder_bezierCurve; + math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-2> SOder_bezierCurve; + }; +};//namespace math +#include "beziercurve.inl" + +#endif // BEZIERCURVE_HPP +
diff -r c3e774091195 -r ceee5a608e7c include/BezierCurve/beziercurve.inl --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/BezierCurve/beziercurve.inl Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,167 @@ + +namespace math{ + + /** + * @brief Construct a new Bezier Curve< T>:: Bezier Curve object + * + */ + template<class T> + BezierCurve<T>::BezierCurve(){ + } + + /** + * @brief Construct a new Bezier Curve< T>:: Bezier Curve object + * + * @param a point A + * @param b point B + * @param c point C + * @param d point D + */ + template<class T> + BezierCurve<T>::BezierCurve(std::complex<T> a,std::complex<T> b,std::complex<T> c,std::complex<T> d){ + this->points[0]=a; + this->points[1]=b; + this->points[2]=c; + this->points[3]=d; + + this->bezierCurve=this->CP2PF(); + this->FOder_bezierCurve=this->bezierCurve.derivateFO(); + this->SOder_bezierCurve=this->FOder_bezierCurve.derivateFO(); + } + + /** + * @brief Construct a new Bezier Curve< T>:: Bezier Curve object + * + * @param points Array of points + */ + template<class T> + BezierCurve<T>::BezierCurve(std::complex<T> points[BEZIER_ORDER+1]) + { + for(int32_t i=0;i<=BEZIER_ORDER;++i){ + this->points[i]=points[i]; + } + this->bezierCurve=this->CP2PF(); + this->FOder_bezierCurve=this->bezierCurve.derivateFO(); + this->SOder_bezierCurve=this->FOder_bezierCurve.derivateFO(); + } + + /** + * @brief Destroy the Bezier Curve< T>:: Bezier Curve object + * + */ + template<class T> + BezierCurve<T>::~BezierCurve() + { + //dtor + } + + /** + * @brief This metohd create a new polynomial function based on the complex points. + * + * @return The result polynomial function represent the Bezier curve. + */ + template<class T> + math::PolynomialFunction<std::complex<float>,BEZIER_ORDER> BezierCurve<T>::CP2PF(){ + math::PolynomialFunction<std::complex<float>,BEZIER_ORDER> pf; + + const std::complex<T> temp_cst_3(3,0); + const std::complex<T> temp_cst_2(2,0); + + std::complex<T> coef1=temp_cst_3*(this->points[1]-this->points[0]); + std::complex<T> coef2=temp_cst_3*(this->points[0]-temp_cst_2*this->points[1]+this->points[2]); + std::complex<T> coef3=temp_cst_3*(this->points[1]-this->points[2])+this->points[3]-this->points[0]; + pf.setCoefficientValue(0,this->points[0]); + pf.setCoefficientValue(1,coef1); + pf.setCoefficientValue(2,coef2); + pf.setCoefficientValue(3,coef3); + return pf; + } + + /** + * @brief Get the point on the bezier curve. + * + * @param input_value The input value, it must belong to interval [0,1]. + * @return The point as complex number + */ + template<class T> + std::complex<T> BezierCurve<T>::getValue(float input_value){ + T input_value_T=static_cast<T>(input_value); + return this->bezierCurve.calculateValue(input_value); + } + + template<class T> + /** + * @brief Get the value of the first order derivative of Bezier curve + * + * @param input_value The input value [0,1] + * @return The point as a complex number. + * + */ + std::complex<T> BezierCurve<T>::get_FO_DerivateValue(float input_value){ + T input_value_T=static_cast<T>(input_value); + return this->FOder_bezierCurve.calculateValue(input_value_T); + } + + /** + * @brief Get the value of the second order derivative of Bezier Curve. + * + * @param input_value The input value of the function have to belong to interval [0,1] + * @return The resulted value as a complex number. + */ + template<class T> + std::complex<T> BezierCurve<T>::get_SO_DerivateValue(float input_value){ + T input_value_T=static_cast<T>(input_value); + + return this->SOder_bezierCurve.calculateValue(input_value_T); + } + + /** + * @brief Get the polynomial function, which respresents the Bezier curve. + * + */ + template<class T> + math::PolynomialFunction<std::complex<T>,BEZIER_ORDER> BezierCurve<T>::getBezierCurve(){ + return this->bezierCurve; + } + + /** + * @brief Get resulted polynomial function of the first order derivative + * + * @return math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-1> + */ + template<class T> + math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-1> BezierCurve<T>::getFODerivate(){ + return this->FOder_bezierCurve; + } + + template<class T> + /** + * @brief Get resulted polynomial function of the second order derivative + * + * @return math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-2> + */ + math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-2> BezierCurve<T>::getSODerivate(){ + return this->SOder_bezierCurve; + } + + template<class T> + /** + * @brief Set the points for creating a new Bezier curve + * + * @param a point A + * @param b point B + * @param c point C + * @param d point D + */ + void BezierCurve<T>::setBezierCurve(std::complex<T> a,std::complex<T> b,std::complex<T> c,std::complex<T> d){ + this->points[0]=a; + this->points[1]=b; + this->points[2]=c; + this->points[3]=d; + + this->bezierCurve=this->CP2PF(); + this->FOder_bezierCurve=this->bezierCurve.derivateFO(); + this->SOder_bezierCurve=this->FOder_bezierCurve.derivateFO(); + } + +}; // namespace math
diff -r c3e774091195 -r ceee5a608e7c include/BezierCurve/polynomialfunction.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/BezierCurve/polynomialfunction.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,67 @@ +/** + ****************************************************************************** + * @file PolynomialFunction.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the Plynomial Function + * methods. + ****************************************************************************** + */ + +/* include guard */ +#ifndef POLYNOMIALFUNCTION_HPP +#define POLYNOMIALFUNCTION_HPP + +#include <stdint.h> +#include <math.h> +#include <iostream> + + +namespace math{ + /** + * @brief It represents a one dimensional polynomial function + * + * @tparam T The type of the Polynomial function + * @tparam N The degree of the polynom. + */ + template <class T,int32_t N> + class PolynomialFunction + { + public: + /* Constructors */ + PolynomialFunction(); + PolynomialFunction(T coefficients[N+1]); + /* Destructors */ + virtual ~PolynomialFunction(); + + // template<int32_t N2> void add(PolynomialFunction<T,N2> poli); + /* Add */ + template<int32_t N2> math::PolynomialFunction<T,(N2<N?N:N2)> add(PolynomialFunction<T,N2> b); + /* Multiply */ + template<int32_t N2> math::PolynomialFunction<T,(N2+N)> multip(PolynomialFunction<T,N2> b); + /* Calculate value */ + T calculateValue(T input_value); + /* First order derivate */ + math::PolynomialFunction<T,N-1> derivateFO(); + /* Get degree */ + int32_t getDegree(); + /* Get coefficient value */ + T getCoefficientValue(int32_t index); + /* Set coefficient value */ + void setCoefficientValue(int32_t index,T value ); + private: + /* Coefficients array */ + /** + * @brief The list of the coeffients. + * + */ + T coefficients[N+1]; + }; + +}; // namepace math + +#include "polynomialfunction.inl" + +#endif // POLYNOMIALFUNCTION_HPP +
diff -r c3e774091195 -r ceee5a608e7c include/BezierCurve/polynomialfunction.inl --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/BezierCurve/polynomialfunction.inl Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,144 @@ + +namespace math{ + +/** + * @brief Construct a new Polynomial Function< T, N>:: Polynomial Function object + * + */ +template<class T,int32_t N> +PolynomialFunction<T,N>::PolynomialFunction() +{ + for(int i=0;i<=N;++i){ + this->coefficients[i]=static_cast<T>(0); + } +} + +/** + * @brief Construct a new Polynomial Function< T, N>:: Polynomial Function object + * + * @param coefficients The list of the coefficients for the polynomial function. + */ +template<class T,int32_t N> +PolynomialFunction<T,N>::PolynomialFunction(T coefficients[N+1]) +{ + for(int i=0;i<=N;++i){ + this->coefficients[i]=coefficients[i]; + } +} + +/** + * @brief Destroy the Polynomial Function< T, N>:: Polynomial Function object + * + */ +template<class T,int32_t N> +PolynomialFunction<T,N>::~PolynomialFunction() +{ +} + +/** + * @brief Get the degree of the polynomial function + * + * @return The degree of the polynom + */ +template<class T,int32_t N> +int32_t PolynomialFunction<T,N>::getDegree() +{ + return N; +} + +/** + * @brief Get coefficient value + * + * @param index The index of the coefficient + * @return The value of the coefficient + */ +template<class T,int32_t N> +T PolynomialFunction<T,N>::getCoefficientValue(int32_t index) +{ + if(index>N || index<0) return static_cast<T>(0); + return this->coefficients[index]; +} + +/** + * @brief Set the value of the coefficient + * + * @param index The index of the coefficient + * @param value The new value of the coefficient + */ +template<class T,int32_t N> +void PolynomialFunction<T,N>::setCoefficientValue(int32_t index, T value ){ + if(index<=N){ + this->coefficients[index]=value; + } +} + +/** + * @brief It sums the polynom with the input polynom and return a result in the new polynom. + * + * @param b The input polynom. + * @return The result polynom. + */ +template<class T,int32_t N> +template<int32_t N2> +math::PolynomialFunction<T,(N2<N?N:N2)> PolynomialFunction<T,N>::add(PolynomialFunction<T,N2> b){ + math::PolynomialFunction<T,N2<N?N:N2> p; + for(int i=0;i<=N2 || i<=N;++i){ + p.setCoefficientValue(i,this->getCoefficientValue(i)+b.getCoefficientValue(i)); + } + return p; +} + + +template<class T,int32_t N> +template<int32_t N2> +/** + * @brief It multiply the polynom with the given polynom and return the result polynom. + * + * @param b The input polynom + * @return The result polynom + */ +math::PolynomialFunction<T,(N2+N)> PolynomialFunction<T,N>::multip(PolynomialFunction<T,N2> b){ + math::PolynomialFunction<T,(N2+N)> result; + for(int32_t i=0;i<=N2+N;++i){ + T sum=static_cast<T>(0); + for(int32_t j=0;j<=i;++j){ + sum+=this->getCoefficientValue(j)*b.getCoefficientValue(i-j); + } + result.setCoefficientValue(i,sum); + } + + return result; +} + +template<class T,int32_t N> +/** + * @brief Calculate the polynom value + * + * @param input_value The given value + * @return The result value of the polynom. + */ +T PolynomialFunction<T,N>::calculateValue(T input_value){ + T output_value=static_cast<T>(0); + for(int32_t i=0;i<=N;++i){ + output_value+=this->coefficients[i]*static_cast<T>(pow(input_value,i)); + } +// std::cout<<"OOO"<<output_value<<std::endl; + return output_value; +} + +template<class T, int32_t N> +/** + * @brief It calcutes the first derivate of the polinom and return it by creating a new polynomial function object. + * + * @return The result polynom. + */ +math::PolynomialFunction<T,N-1> PolynomialFunction<T,N>::derivateFO(){ + math::PolynomialFunction<T,N-1> derivate; + for(int32_t i=0;i<N;++i){ + T coeff=static_cast<T>(i+1); + derivate.setCoefficientValue(i,coeff*this->getCoefficientValue(i+1)); + } + return derivate; +}; + +};
diff -r c3e774091195 -r ceee5a608e7c include/BezierMotionPlanner/beziermotionplanner.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/BezierMotionPlanner/beziermotionplanner.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,86 @@ +/** + ****************************************************************************** + * @file BezierMotionPlanner.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the Bezier Motion + * Planner methods. + ****************************************************************************** + */ + +/* include guard */ +#ifndef MOTIONPLANNER_HPP +#define MOTIONPLANNER_HPP + +#include <complex> +#include <utility> +#include <math.h> +#include <BezierCurve/BezierCurve.hpp> + +/* Module defines */ +#define WHEELBASE 0.265 + +namespace planner{ + + /** CBezierMotionPlanner class. + * @brief This class implements the functionality of planner to keep the robot on the given Bezier curve. + * + */ + class CBezierMotionPlanner + { + public: + /* Constructor */ + CBezierMotionPlanner(); + /* Constructor */ + CBezierMotionPlanner(bool isForward, + std::complex<float> a, + std::complex<float> b, + std::complex<float> c, + std::complex<float> d, + float motion_duration_i, + float timestep_i); + /* Set parameters */ + void setMotionPlannerParameters(bool isForward, + std::complex<float> a, + std::complex<float> b, + std::complex<float> c, + std::complex<float> d, + float motion_duration_i, + float timestep_i); + /* Destructor */ + virtual ~CBezierMotionPlanner(); + /* Get Bezier Curve */ + math::BezierCurve<float> getBezierCurve(); + // Velocity longitudinal and angular + /* Get next velocity value */ + std::pair<float,float> getNextVelocity(); + // Velocity longitudinal and angular, Input value [0,1] + /* Get velocity value */ + std::pair<float,float> getVelocity(float input_value); + /* Next value valid */ + bool hasValidValue(); + /* Is movement forward */ + bool getForward(); + + private: + /* forward movement indicator */ + bool isForward; + /* Bezier Curve object */ + math::BezierCurve<float> bezierCurve; + /* motion duration */ + float motion_duration; + /* time step */ + float time_step; + /* Bezier value input step */ + float bezierValueInput_step; + /* next Bezier value */ + float next_bezierValueInput; + /* intialization indicator */ + bool isInitialized; + }; + +}; // namespace planner + +#endif // CMotionPlanner_HPP +
diff -r c3e774091195 -r ceee5a608e7c include/CommandInterpreter/commandinterpreter.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/CommandInterpreter/commandinterpreter.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * @file CommandInterpreter.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class declaration for the command interpreter + * functionality. + ****************************************************************************** + */ + +/* Inclusion guard */ +#ifndef COMMAND_INTERPRETER_HPP +#define COMMAND_INTERPRETER_HPP + +/* The mbed library */ +#include <mbed.h> +#include <Move/move.hpp> +#include <Queue/queue.hpp> + +//! CTimer class. +/*! + * It is used for implementing the interpreter of commands received by Nucleo. + * */ +class CCommandInterpreter +{ +public: + /* Constuctor */ + CCommandInterpreter(Move& f_car); + /* Interpret character */ + inline void interpretChar(unsigned char f_c); + /* Execute command */ + inline void executeCommand(); +private: + /* Reset */ + inline void reset(); + /* Interpret command */ + inline void intepretCommand(); + /* buffer */ + CQueue<unsigned char, 7> m_buffer; + /* reference to MOVE object */ + Move& m_car; + unsigned char m_commandID; + /* command value */ + float m_commandValue; + /* speed value */ + float m_speedValue; + /* angle value */ + float m_angleValue; +}; + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Controllers/controller.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Controllers/controller.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,83 @@ +/** + ****************************************************************************** + * @file Controller.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the controller + * functionality. + ****************************************************************************** + */ + +/* Include guard */ + +#ifndef CONTROLLER_HPP +#define CONTROLLER_HPP +#include<cmath> +#include<Controllers/pidcontroller.hpp> + +#include <Encoders/encoderinterface.hpp> +#include <Controllers/converters.hpp> + +#include <mbed.h> + +namespace controllers +{ + + /** CControllerSiso class + * @brief It implements a controller with a single input and a single output. It needs an encoder getter interface to get the measured values, a controller to calculate the control signal. It can be completed with a converter to convert the measaurment unit of the control signal. + * + */ + class CControllerSiso + { + /* PID controller declaration*/ + template <class T> + using ControllerType=siso::IController<T>; + + public: + /* Construnctor */ + CControllerSiso(encoders::IEncoderGetter& f_encoder + ,ControllerType<double>& f_pid + ,controllers::IConverter* f_converter=NULL); + /* Set controller reference value */ + void setRef(double f_RefRps); + /* Get controller reference value */ + double getRef(); + /* Get control value */ + double get(); + /* Get error */ + double getError(); + /* Clear PID parameters */ + void clear(); + /* Control action */ + bool control(); + + private: + /* PWM onverter */ + double converter(double f_u); + + /* Enconder object reference */ + encoders::IEncoderGetter& m_encoder; + /* PID object reference */ + ControllerType<double>& m_pid; + /* Controller reference */ + double m_RefRps; + /* Control value */ + double m_u; + /* Error */ + double m_error; + /* Converter */ + controllers::IConverter* m_converter; + /* Scaled PWM control signal limits */ + const float m_control_sup; + const float m_control_inf; + /* Inferior limit of absolut reference */ + const float m_ref_abs_inf; + /* Inferior and superior limits of absolute speed measurment for standing state */ + const float m_mes_abs_inf; + const float m_mes_abs_sup; + + }; +}; // namespace controllers + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Controllers/converters.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Controllers/converters.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,74 @@ +/** +****************************************************************************** +* @file converters.hpp +* @author RBRO/PJ-IU +* @version V1.0.0 +* @date 2018-10-29 +* @brief +****************************************************************************** +*/ +#ifndef CONVERTERS_H +#define CONVERTERS_H + +#include<cmath> +#include<stdint.h> +#include<array> + +namespace controllers{ + /** + * @brief Converter interface with single input and single output + * + */ + class IConverter{ + public: + virtual float operator()(float) = 0; + }; + + /** + * @brief Polynomial converter. + * + * @tparam NOrd The degree of the polynomial converter + */ + template<uint8_t NOrd> + class CConverterPolynom:public IConverter + { + public: + CConverterPolynom(std::array<float,NOrd+1>); + float operator()(float); + + private: + std::array<float,NOrd+1> m_coeff; + }; + + /** + * @brief A converter based on the set of break point and the multiple polynomial function. + * + * @tparam NrBreak Number of the break. + * @tparam NOrd Degree of the polynomial converter. + */ + template<uint8_t NrBreak,uint8_t NOrd> + class CConverterSpline:public IConverter + { + public: + using CCoeffContainerType = std::array<float,NOrd+1>; + using CSplineContainerType = std::array<CCoeffContainerType,NrBreak+1>; + using CBreakContainerType = std::array<float,NrBreak>; + + + CConverterSpline(CBreakContainerType f_breaks,CSplineContainerType f_splines); + float operator()(float); + private: + float splineValue( CCoeffContainerType,float); + + CBreakContainerType m_breaks; + CSplineContainerType m_splines; + + }; + + #include "converters.inl" +}; //namespace controllers + + + +#endif // CONVERTERS_H +
diff -r c3e774091195 -r ceee5a608e7c include/Controllers/converters.inl --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Controllers/converters.inl Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,77 @@ + + +/** + * @brief Construct a new CConverterPolynom<NOrd>::CConverterPolynom object + * + * @param f_coeff The list of the coeffience. + */ +template<uint8_t NOrd> +CConverterPolynom<NOrd>::CConverterPolynom(std::array<float,NOrd+1> f_coeff) +:m_coeff(f_coeff) +{ +} + + +/** + * @brief Convert the input value. + * + */ +template<uint8_t NOrd> +float CConverterPolynom<NOrd>::operator()(float f_v){ + float l_res = m_coeff[NOrd]; + for (uint8_t i = 0;i<NOrd;++i){ + l_res += std::pow(f_v, NOrd-i)*m_coeff[i]; + } + return l_res; +} + + +/** + * @brief Construct a new CConverterSpline<NrBreak, NOrd>::CConverterSpline object + * + * @tparam NrBreak + * @tparam NOrd + * @param f_breaks The list of the break points. + * @param f_splines The list of the polynomial function. + */ +template <uint8_t NrBreak, uint8_t NOrd> +CConverterSpline<NrBreak, NOrd>::CConverterSpline(CConverterSpline<NrBreak, NOrd>::CBreakContainerType f_breaks,CConverterSpline<NrBreak, NOrd>::CSplineContainerType f_splines) +:m_breaks(f_breaks) +,m_splines(f_splines) +{ +} + +/** + * @brief Convert the input value. + * + */ +template <uint8_t NrBreak, uint8_t NOrd> +float CConverterSpline<NrBreak, NOrd>::operator()(float f_value){ + typename CConverterSpline<NrBreak, NOrd>::CBreakContainerType::iterator it_breaks0=NULL,it_breaks1=NULL; + typename CConverterSpline<NrBreak, NOrd>::CSplineContainerType::iterator it_spline = m_splines.begin(); + for (it_breaks1 = m_breaks.begin();it_breaks1 != m_breaks.end();){ + if((it_breaks0==NULL || (it_breaks0!=NULL && (*it_breaks0)<f_value)) && f_value<=(*it_breaks1)){ + break; + } + it_breaks0 = it_breaks1; + ++it_breaks1; + ++it_spline; + } + return this->splineValue(*it_spline, f_value); +} + +/** + * @brief Convert the given value, by using the given polynom. + * + * @param f_coeff The coeffiences of the polynom + * @param f_value The input value. + */ +template <uint8_t NrBreak, uint8_t NOrd> +float CConverterSpline<NrBreak, NOrd>::splineValue( CCoeffContainerType f_coeff,float f_value){ + float l_res = f_coeff[NOrd]; + for (uint8_t i = 0;i<NOrd;++i) + { + l_res+=std::pow(f_value,NOrd-i)*f_coeff[i]; + } + return l_res; +}
diff -r c3e774091195 -r ceee5a608e7c include/Controllers/pidcontroller.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Controllers/pidcontroller.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,82 @@ +#ifndef PID_CONTROLLER_H +#define PID_CONTROLLER_H + +#include <cstdio> +#include <Linalg/linalg.h> +#include <SystemModels/systemmodels.hpp> +#include <mbed.h> + +namespace controllers +{ + namespace siso + { + + /** + * @brief General interface class for the controller + * + * @tparam T type of the variables (float, double) + */ + template<class T> + class IController{ + public: + virtual T calculateControl(const T&)=0; + virtual void clear()=0; + }; + + /** + * @brief It generates a discrete transferfunction for realizing a proportional–integral–derivative controller, which is discretized by the Euler’s method. + * + * @tparam T type of the variables (float, double) + */ + template<class T> + class CPidController:public IController<T> + { + public: + /* Discrete transfer function type */ + using CPidSystemmodelType = systemmodels::lti::siso::CDiscreteTransferFucntion<T,3,3>; + + /* Constructor */ + CPidController(T f_kp + ,T f_ki + ,T f_kd + ,T f_tf + ,T f_dt); + CPidController(CPidSystemmodelType f_pid,T f_dt); + + /* Overloaded operator */ + T calculateControl(const T& f_input); + /* Serial callback */ + static void staticSerialCallback(void* obj,char const * a, char * b); + /* Initialization */ + void clear(); + private: + #ifdef CONTROL_TEST_HPP + FRIEND_TEST(PidControlTest,Initialization); + FRIEND_TEST(PidControlTest,Setter); + #endif + /* Controller setting */ + void setController(T f_kp + ,T f_ki + ,T f_kd + ,T f_tf); + /* Serial callback implementation */ + void serialCallback(char const * a, char * b); + + + + /* Discrete transfer function */ + CPidSystemmodelType m_pidTf; + + + /* delta-time term */ + T m_dt; + + }; + /* Include function definitions */ + #include "PidController.inl" + }; // namespace siso +}; // namespace controllers + + + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Controllers/pidcontroller.inl --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Controllers/pidcontroller.inl Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,114 @@ +/** @brief Class constructor + * + * Constructor method + * + * @param f_kp proportional factor + * @param f_ki integral factor + * @param f_kd derivative factor + * @param f_tf derivative time filter constant + * @param f_dt sample time + */ +template<class T> +CPidController<T>::CPidController(T f_kp + ,T f_ki + ,T f_kd + ,T f_tf + ,T f_dt) + :m_pidTf() + ,m_dt(f_dt) +{ + linalg::CMatrix<T,1,3> l_numPid({ (f_kd+f_tf*f_kp)/f_tf , (f_tf*f_dt*f_ki+f_dt*f_kp-2*f_tf*f_kp-2*f_kd)/f_tf , (f_kd+f_tf*f_kp+f_dt*f_dt*f_ki-f_dt*f_tf*f_ki-f_dt*f_kp)/f_tf}); + linalg::CMatrix<T,1,3> l_denPid({ 1.0,-(2*f_tf-f_dt)/f_tf,(f_tf-f_dt)/f_tf }); + + m_pidTf.setNum(l_numPid.transpose()); + m_pidTf.setDen(l_denPid.transpose()); +} + +// template<class T> +// CPidController<T>::CPidController( CPidSystemmodelType f_pid +// ,T f_dt) +// :m_pidTf(f_pid) +// ,m_dt(f_dt){ + +// } + + +/** @brief Operator + * + * @param f_input input function + * \return control value + */ +template<class T> +T CPidController<T>::calculateControl(const T& f_input) +{ + return m_pidTf(f_input); +} + +/** @brief Serial callback method + * + * Serial callback attaching serial callback to controller object + * + * @param obj PID controller object + * @param a string to read data from + * @param b string to write data to + * \return None + */ +template<class T> +void CPidController<T>::staticSerialCallback(void* obj,char const * a, char * b) +{ + CPidController* self = static_cast<CPidController*>(obj); + self->serialCallback(a,b); +} + +/** @brief Reset to zero all memory of the controller. + * + */ +template<class T> +void CPidController<T>::clear() +{ + m_pidTf.clearMemmory(); +} + +/** @brief Set the parameter of the controller + * + * + * @param f_kp proportional factor + * @param f_ki integral factor + * @param f_kd derivative factor + * @param f_tf derivative time filter constant + */ +template<class T> +void CPidController<T>::setController( + T f_kp, + T f_ki, + T f_kd, + T f_tf) +{ + linalg::CMatrix<T,1,3> l_numPid({ (f_kd+f_tf*f_kp)/f_tf , (f_tf*m_dt*f_ki+m_dt*f_kp-2*f_tf*f_kp-2*f_kd)/f_tf , (f_kd+f_tf*f_kp+m_dt*m_dt*f_ki-m_dt*f_tf*f_ki-m_dt*f_kp)/f_tf }); + linalg::CMatrix<T,1,3> l_denPid({ 1,-(2*f_tf-m_dt)/f_tf,(f_tf-m_dt)/f_tf }); + + m_pidTf.setNum(l_numPid.transpose()); + m_pidTf.setDen(l_denPid.transpose()); +} + +/** @brief Serial callback method setting controller to values received + * + * + * @param a string to read data from + * @param b string to write data to + */ +template<class T> +void CPidController<T>::serialCallback(char const * a, char * b) +{ + float l_kp,l_ki,l_kd,l_tf; + uint32_t l_res = sscanf(a,"%f;%f;%f;%f;",&l_kp,&l_ki,&l_kd,&l_tf); + if (4 == l_res) + { + setController(l_kp,l_ki,l_kd,l_tf); + sprintf(b,"ack;;%2.5f;%2.5f;%2.5f;%2.5f;",l_kp,l_ki,l_kd,l_tf); + } + else + { + sprintf(b,"sintax error;;"); + } +}
diff -r c3e774091195 -r ceee5a608e7c include/Drivers/servo.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Drivers/servo.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,44 @@ +/** + ****************************************************************************** + * @file SERVO.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class declaration for the steering SERVO + * functionality. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef SERVO_HPP +#define SERVO_HPP + +#include <mbed.h> + + +namespace drivers{ + /** + * @brief It is used for implementing SERVO functionality. + * + */ + class SERVO + { + public: + /* Constructor */ + SERVO(PinName _pwm); + /* Destructor */ + ~SERVO(); + /* Set angle */ + void SetAngle(float angle); //-25 to 25 degr + private: + /* convert angle to duty cycle */ + float conversion(float angle); //angle to duty cycle + /* PWM value */ + PwmOut pwm; + /* Current angle */ + float current_angle; + }; +}; // namespace drivers + + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Drivers/vnh.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Drivers/vnh.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,82 @@ +/** + ****************************************************************************** + * @file VNH.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class declaration for the VNH Bridge Driver + * functionality. + ****************************************************************************** + */ + +/* Inclusion guard */ +#ifndef VNH_HPP +#define VNH_HPP + +/* The mbed library */ +#include <mbed.h> +/* Functions to compute common mathematical operations and transformations */ +#include <cmath> + +namespace drivers{ + // ! Current getter interface + class ICurrentGetter{ + public: + virtual float GetCurrent(void)=0; + }; + + /** + * @brief Command setter interface. + * + */ + class ICommandSetter{ + public: + virtual void Run(float pwm); + }; + + //! VNH class. + /*! + * It is used for implementing motor bridge control. + * */ + class VNH:public ICurrentGetter, public ICommandSetter + { + public: + /* Constructor */ + VNH(PinName, PinName, PinName, PinName); + /* Destructor */ + ~VNH(); + /* Start */ + void Start(void); + /* Stop */ + void Stop(void); + /* Run */ + void Run(float speed); + /* Brake */ + void Brake(); + /* Inverse */ + void Inverse(float f_speed); + /* Get current */ + float GetCurrent(void); + + private: + /* Go current */ + void Go(float speed); + /* increment member */ + float increment; + /* current speed member */ + float current_speed; + /* PWM */ + PwmOut pwm; + /* pin A */ + DigitalOut ina; + /* pin B */ + DigitalOut inb; + /* current value */ + AnalogIn current; + }; + + +}; // namespace drivers + + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Encoders/encoder.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Encoders/encoder.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,107 @@ +/** + ****************************************************************************** + * @file Encoder.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the encoder + * functionality. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef MAG_ENCODER_HPP +#define MAG_ENCODER_HPP + +#include <mbed.h> +#include<TaskManager/taskmanager.hpp> +#include<Encoders/encoderinterface.hpp> + +//! CCounter class. +/*! + * It is used for counting encoder steps. + */ +class CCounter +{ + public: + /* Constructor */ + CCounter(PinName f_pin); + /* Reset method */ + void reset(); + /* Increment method */ + void increment(); + /* Get counts method */ + int32_t getCount(); + /* Increment */ + static void staticIncrement(void* obj); + private: + /* Interrupt object */ + InterruptIn m_interrupt; + /* Counts member */ + volatile int32_t m_count; +}; + +//! CEncoder class. +/*! + * It inherits class task::CTask. + * It is used for computing revolustions per second. + */ +class CEncoder: public task::CTask, public:: encoders::IEncoderGetter +{ + public: + /* Constructor */ + CEncoder(uint32_t f_period + ,float f_period_sec + ,uint32_t f_cpr + ,PinName f_pinName); + /* Get rotations per second */ + virtual float getSpeedRps(); + virtual int16_t getCount(); + virtual bool isAbs(){return true;} + + protected: + /* Run method */ + virtual void _run(); + + /* Counter values */ + CCounter m_counter; + /* Period in seconds */ + const float m_period_sec; + /* Value of counts per revolution */ + const uint32_t m_cpr;//count per revolution (rise and fall edges) + /* Value of rotation per second */ + float m_rps; +}; + +//! CMagEncoderTime class. +/*! + * It is used for computing high/low periods. + */ +class CMagEncoderTime +{ + public: + /* Constructor */ + CMagEncoderTime(PinName f_pin); + /* Callback for rising edge interrupt */ + void riseCallback(); + /* Callback for falling edge interrupt */ + void fallCallback(); + /* Callback for rising edge interrupt attached to pbject */ + static void staticRise(void* obj); + /* Callback for falling edge interrupt attached to pbject */ + static void staticFall(void* obj); + /* Get high time */ + float getHighTime(); + /* Get low time */ + float getLowTime(); + private: + /* General purpose timer */ + Timer m_Timer,m_fallTimer; + /* Digital interrupt input */ + InterruptIn m_interrupt; + /* High time */ + volatile float m_highTime; + /* Low time */ + volatile float m_lowTime; +}; +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Encoders/encoderfilter.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Encoders/encoderfilter.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * @file EncoderFilter.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the encoder filter + * functionality. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef ENCODER_FILTER_HPP +#define ENCODER_FILTER_HPP +#include <Encoders/encoder.hpp> +#include <Filter/filter.hpp> + +//! CEncoderFilter class. +/*! + * It inherits class CEncoder. + * It is used for obtaining filtered encoder output. + */ +class CEncoderFilter:public CEncoder +{ + /* Filter object */ + template <class T> + using CFilterFunction = filter::CFilterFunction<T>; + public: + /* Constructor */ + CEncoderFilter (uint32_t f_period + ,float f_period_sec + ,uint32_t f_cpr + ,PinName f_pinName + ,CFilterFunction<float>& f_filter); + + virtual float getSpeedRps(); + virtual int16_t getCount() = 0; + protected: + /* Run method */ + virtual void _run(); + + private: + /* Filter member */ + CFilterFunction<float>& m_filter; + /* Value of filtered rotations per second */ + float m_filteredRps; +}; + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Encoders/encoderinterface.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Encoders/encoderinterface.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,43 @@ +/** + * @file encoderinterface.hpp + * @author RBRO/PJ-IU + * @brief + * @version 0.1 + * @date 2018-10-24 + * + * @copyright Copyright (c) 2018 + * + */ +#ifndef ENDOCER_INTERFACE_HPP +#define ENDOCER_INTERFACE_HPP + +#include <stdint.h> +namespace encoders{ + /** + * @brief Rotary encoder interface class + * + */ + class IEncoderGetter{ + public: + /** + * @brief Get the Count value + * + * @return int16_t + */ + virtual int16_t getCount() = 0; + /** + * @brief Get the Speed Rps + * + * @return float + */ + virtual float getSpeedRps() = 0; + /** + * @brief Get the encoder capability. If it's true, than the encoder can give the oriantation, else it returns the absolute value of the rotation without the oriantation. + * + * @return bool + */ + virtual bool isAbs() = 0; + }; +}; // namepsace encoders; + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Encoders/quadratureencoder.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Encoders/quadratureencoder.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,63 @@ +/** + * @file quadratureencoder.hpp + * @author RBRO/PJ-IU + * @brief + * @version 0.1 + * @date 2018-10-23 + * + * @copyright Copyright (c) 2018 + * + */ + +#ifndef Quadrature_ENCODER_HPP +#define Quadrature_ENCODER_HPP + +#include <mbed.h> + +namespace encoders{ +/** + * @brief It an interface for accessing the encoder position. It can be used to get and to reset the position of the encoder. + * + */ +class CQuadratureEncoder_TIMX{ + public: + virtual int16_t getCount() = 0; + virtual void reset() = 0; +}; + +/** + * @brief It's a singleton class for receiving and decoding the Quadrature signal by using the timer TIM4. It can get the direction of the rotation. + * + */ +class CQuadratureEncoder_TIM4:public CQuadratureEncoder_TIMX{ + /** + * @brief It uses to destroy the singleton class. + * + */ + class CQuadratureEncoder_TIM4_Destroyer{ + public: + CQuadratureEncoder_TIM4_Destroyer(){} + ~CQuadratureEncoder_TIM4_Destroyer(); + void SetSingleton(CQuadratureEncoder_TIM4* s); + private: + CQuadratureEncoder_TIM4* m_singleton; + }; + + public: + static CQuadratureEncoder_TIM4 *Instance(); + int16_t getCount(); + void reset(); + virtual ~CQuadratureEncoder_TIM4() {} + protected: + CQuadratureEncoder_TIM4() {} + + private: + void initialize(); + static CQuadratureEncoder_TIM4* m_instance; + static CQuadratureEncoder_TIM4_Destroyer m_destroyer; +}; + +};// namepsace drivers + + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Encoders/quadratureencodertask.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Encoders/quadratureencodertask.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,75 @@ +/** + * @file quadratureencodertask.hpp + * @author RBRO/PJ-IU + * @brief + * @version 0.1 + * @date 2018-10-23 + * + * @copyright Copyright (c) 2018 + * + */ +#ifndef QUADRATURE_ENCODER_TASK_HPP +#define QUADRATURE_ENCODER_TASK_HPP + +#include <Encoders/encoderinterface.hpp> +#include <Encoders/QuadratureEncoder.hpp> +#include <Filter/Filter.hpp> + +#include <rtos.h> + +namespace encoders{ + +/** + * @brief Rotary encoder with filter interface class + * + */ +class CQuadratureEncoderNonFilteredGetterInterface{ + public: + virtual int16_t getNonFilteredCount()=0; + virtual float getNonFilteredSpeedRps() = 0; +}; + +/** + * @brief It implements a periodic task, which get the value from the counter and reset it to zero. + * + */ +class CQuadratureEncoderTask:public IEncoderGetter{ + public: + CQuadratureEncoderTask(float,encoders::CQuadratureEncoder_TIMX*,uint16_t); + static void static_callback(void* obj); + + void startTimer(); + protected: + virtual void _run(); + virtual int16_t getCount(); + virtual float getSpeedRps(); + virtual bool isAbs(){return false;} + protected: + encoders::CQuadratureEncoder_TIMX *m_Quadratureencoder; + int16_t m_encoderCnt; + const float m_taskperiod_s; + const uint16_t m_resolution; + RtosTimer m_timer; +}; + +/** + * @brief It implements the same functionality than CQuadratureEncoderTask class, but in additional it can filter the values. + * + */ +class CQuadratureEncoderWithFilterTask: public CQuadratureEncoderTask, public CQuadratureEncoderNonFilteredGetterInterface{ + public: + CQuadratureEncoderWithFilterTask(float,encoders::CQuadratureEncoder_TIMX *, uint16_t,filter::CFilterFunction<float>&); + virtual void _run(); + virtual int16_t getCount(); + virtual float getSpeedRps(); + virtual int16_t getNonFilteredCount(); + virtual float getNonFilteredSpeedRps(); + protected: + double m_encoderCntFiltered; + filter::CFilterFunction<float>& m_filter; + +}; + +}; // namespace encoders + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Examples/Filter/ekf_am.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Examples/Filter/ekf_am.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,48 @@ +/** + ****************************************************************************** + * @file Ekf_am.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the Ackermann model example + * functionality. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef EKF_AM_HPP +#define EKF_AM_HPP + +#include <Filter/filter.hpp> +#include <Examples/SystemModels/ackermannmodel.hpp> + +namespace examples +{ + namespace filter + { + /* Extended Kalman Filter */ + template <class T, uint32_t NA, uint32_t NB, uint32_t NC> + using CEKF = ::filter::ltv::mimo::CEKF<T,NA,NB,NC>; + //! CEKF_AM class. + /*! + * It inherits class CEKF. + * Extended Kalman Filter aplied on the Ackermann Model. + */ + class CEKF_AM:public CEKF<double,2,10,5> + { + private: + /* Ackermann model*/ + using CAckermannModel = ::examples::systemmodels::ackermannmodel::CAckermannModel; + /* Ackermann model Jacobian Matrix */ + using CJMAckermannModel = ::examples::systemmodels::ackermannmodel::CJMAckermannModel; + public: + /* Constructor */ + CEKF_AM(CAckermannModel& f_ackermannModel + ,CJMAckermannModel f_jacobianMatrixCalc + ,const CJMTransitionType& f_Q + ,const CObservationNoiseType& f_R); + }; + }; +}; + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Examples/Sensors/sensorpublisher.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Examples/Sensors/sensorpublisher.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,61 @@ +/** + ****************************************************************************** + * @file SensorPublisher.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the sensor publisher + * functionality. Because template classes are used, the code was + * placed in a corresponding .inl file and not in a .cpp one. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef SENSOR_PUBLISHER_HPP +#define SENSOR_PUBLISHER_HPP + +/* The mbed library */ +#include <mbed.h> +/* Container that encapsulates fixed size arrays */ +#include <array> +/* String types, character traits and a set of converting functions */ +#include <string> +#include<TaskManager/taskmanager.hpp> + + +namespace examples{ + namespace sensors + { + //! ProximityPublisher class. + /*! + * It inherits class task::CTask. + * It is used for publishing proximity sensor values. + */ + template <class C_Sensor,uint Nr_Senrsor> + class ProximityPublisher:public task::CTask + { + public: + /* Sensor array */ + using SensorArrayT = std::array<C_Sensor*,Nr_Senrsor>; + /* Constructor */ + ProximityPublisher(uint32_t,SensorArrayT,Serial&); + /* Serial callback attached to object */ + static void staticSerialCallback(void* obj,char const * a, char * b); + protected: + /* Run method */ + void _run(); + /* Serial callback */ + void serialCallback(char const * a, char * b); + private: + /* Sensor array */ + SensorArrayT m_sensors; + /* Reference to serial object */ + Serial& m_serial; + /* Active flag */ + bool m_isActivate; + }; + }; // namespace sensors +}; // namespace examples + +#include "sensorpublisher.inl" +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Examples/Sensors/sensorpublisher.inl --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Examples/Sensors/sensorpublisher.inl Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,78 @@ +namespace examples{ + namespace sensors{ + /** \brief Constructor for the CEncoderFilter class + * + * Constructor method + * + * @param f_period period value + * @param f_sensors sensor array + * @param f_serial reference to the serial object + */ + template <class C_Sensor,uint Nr_Senrsor> + ProximityPublisher<C_Sensor,Nr_Senrsor>::ProximityPublisher(uint32_t f_period + ,SensorArrayT f_sensors + ,Serial& f_serial) + :CTask(f_period) + ,m_sensors(f_sensors) + ,m_serial(f_serial) + ,m_isActivate(false) + { + } + + /** \brief Serial callback method + * + * Serial callback attaching serial callback to controller object + * + * @param obj PID controller object + * @param a string to read data from + * @param b string to write data to + * + */ + template <class C_Sensor,uint Nr_Senrsor> + void ProximityPublisher<C_Sensor,Nr_Senrsor>::staticSerialCallback(void* obj,char const * a, char * b) + { + ProximityPublisher* self = static_cast<ProximityPublisher*>(obj); + self->serialCallback(a,b); + } + + /** \brief Serial callback actions + * + * Serial callback + * + * @param a string to read data from + * @param b string to write data to + * + */ + template <class C_Sensor,uint Nr_Senrsor> + void ProximityPublisher<C_Sensor,Nr_Senrsor>::serialCallback(char const * a, char * b){ + int l_isActivate=0; + uint32_t l_res = sscanf(a,"%d",&l_isActivate); + if(l_res==1){ + m_isActivate=(l_isActivate>=1); + sprintf(b,"ack;;"); + }else{ + sprintf(b,"sintax error;;"); + } + } + + /** \brief Method called each f_period + * + * + * + */ + template <class C_Sensor,uint Nr_Senrsor> + void ProximityPublisher<C_Sensor,Nr_Senrsor>::_run(){ + if(!m_isActivate) return; + char l_buf[100]; + sprintf(l_buf,"@DSPB:"); + for(uint8_t i=0;i<Nr_Senrsor;++i){ + + sprintf(l_buf,"%s%2.2f;",l_buf,m_sensors[i]->getValue()); + } + sprintf(l_buf,"%s;\n\r",l_buf); + m_serial.printf("%s",l_buf); + + } + }; // namespace sensors +}; // namespace examples +
diff -r c3e774091195 -r ceee5a608e7c include/Examples/Sensors/sensortask.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Examples/Sensors/sensortask.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,85 @@ +/** + ****************************************************************************** + * @file SensorTask.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the sensor task + * functionality. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef SENSOR_TASK_HPP +#define SENSOR_TASK_HPP + +/* Standard C library for basic mathematical operations */ +#include <math.h> +#include<TaskManager/taskmanager.hpp> +#include <Drivers/vnh.hpp> +#include <SerialMonitor/serialmonitor.hpp> +#include <Controllers/controller.hpp> +#include <SystemModels/systemmodels.hpp> + +#include <Encoders/encoderinterface.hpp> + +namespace examples +{ + + namespace sensors{ + + + //! CDISTFTest class. + /*! + * It inherits class task::CTask. + * It is used for testing distance sensors operation. + */ + class CDISTFTest:public task::CTask + { + public: + /* Constructor */ + CDISTFTest(uint32_t f_period + ,Serial& f_serial); + + private: + /* Run method */ + void _run(); + + /* Reference to serial object */ + Serial& m_serial; + /* Transfer function */ + systemmodels::lti::siso::CDiscreteTransferFucntion<float,3,3> m_tf; + }; + + //! CEncoderSender class. + /*! + * It inherits class task::CTask. + * It is used for sending acquired data. + */ + class CEncoderSender:public task::CTask + { + public: + /* Constructor */ + CEncoderSender(uint32_t f_period + ,encoders::IEncoderGetter& f_encoder + ,Serial& f_serial); + /* Serial callback */ + static void staticSerialCallback(void* obj,char const * a, char * b); + private: + /* Serial callback implementation */ + void serialCallback(char const * a, char * b); + /* Run method */ + void _run(); + + /* Active flag */ + bool m_isActived; + /* Encoder value getter */ + encoders::IEncoderGetter& m_encoder; + /* Serial communication member*/ + Serial& m_serial; + }; + }; // namespace sensors +}; // namespace examples + + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Examples/SystemModels/ackermannmodel.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Examples/SystemModels/ackermannmodel.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,182 @@ + +/** + ****************************************************************************** + * @file AckermannModel.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the Ackermann model. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef ACKERMANN_MODEL_HPP +#define ACKERMANN_MODEL_HPP + +#include <math.h> +#include <SystemModels/systemmodels.hpp> +#include <Examples/SystemModels/ackermanntypes.hpp> + +/* Definition of PI value */ +#ifndef M_PI + #define M_PI 3.14159265358979323846 +#endif + +/* Definition of degrees to radians transformation */ +#ifndef DEG2RAD + #define DEG2RAD M_PI/180.0 +#endif + +namespace examples +{ + namespace systemmodels + { + namespace ackermannmodel + { + /* System model type */ + template<class T,uint32_t NA,uint32_t NB,uint32_t NC> + using CSystemModelType = ::systemmodels::nlti::mimo::CDiscreteTimeSystemModel<T,NA,NB,NC>; + /* Jacobian matrix*/ + template<class T,uint32_t NA,uint32_t NB,uint32_t NC> + using CJacobianMatricesType = ::systemmodels::nlti::mimo::CJacobianMatrices<T,NA,NB,NC>; + //! CAckermannModel class. + /*! + * It inherits class CSystemModelType. + */ + class CAckermannModel:public CSystemModelType<double,2,10,5> + { + public: + /* Constructor */ + CAckermannModel(const double f_dt + ,const double f_gamma + ,const double f_wb + ,const double f_b + ,const double f_J + ,const double f_K + ,const double f_R + ,const double f_L); + /* Constructor */ + CAckermannModel(const CStatesType& f_states + ,const double f_dt + ,const double f_gamma + ,const double f_wb + ,const double f_b + ,const double f_J + ,const double f_K + ,const double f_R + ,const double f_L); + /* Update method */ + CStatesType update(const CInputType& f_input); + + /* Calculate output method */ + COutputType calculateOutput(const CInputType& f_input); + private: + /* gamma=Meter/Rotation */ + const double m_gamma; + /* Wheel base distance in meter */ + const double m_wb; + /* Motor Constants */ + const double m_bJ,m_KJ,m_KL,m_RL,m_L; + }; + + //! CJMAckermannModel class. + /*! + * It inherits class CJacobianMatricesType. + */ + class CJMAckermannModel:public CJacobianMatricesType<double,2,10,5> + { + public: + CJMAckermannModel (double f_dt + ,double f_gamma + ,double f_wb + ,double f_b + ,double f_J + ,double f_K + ,double f_R + ,double f_L) + :m_dt(f_dt) + ,m_gamma(f_gamma) + ,m_wb(f_wb) + ,m_bJ(f_b/f_J) + ,m_KJ(f_K/f_J) + ,m_KL(f_K/f_L) + ,m_RL(f_R/f_L) + { + m_ObservationMatrix=initObservationMatrix(); + } + + + + CJMObservationType getJMObservation( const CStatesType& f_states + ,const CInputType& f_input){ + return m_ObservationMatrix; + } + + private: + linalg::CMatrix<double,5,10> initObservationMatrix(){ + linalg::CMatrix<double,5,10> l_data; + l_data[0][2]=l_data[1][3]=1.f/m_dt; + l_data[0][4]=l_data[1][5]=-1.f/m_dt; + l_data[2][7]=1.f; + l_data[3][8]=m_gamma; + l_data[4][9]=1.f; + return l_data; + } + + inline void setJacobianStateMatrixWithOne(CJMTransitionType& f_matrix){ + f_matrix[0][0]=f_matrix[1][1]=1; + f_matrix[4][2]=f_matrix[5][3]=1; + f_matrix[6][6]=1; + return; + } + + public: + CJMTransitionType getJMTransition( const CStatesType& f_states + ,const CInputType& f_input){ + + CJMTransitionType l_data(CJMTransitionType::zeros()); + setJacobianStateMatrixWithOne(l_data); + CState l_states(f_states); + CInput l_input(f_input); + + //Setting values in the jacobian matrix + l_data[0][2]=m_dt; + l_data[1][3]=m_dt; + + l_data[2][6]=-m_gamma*l_states.omega()*sin(l_states.teta_rad()); + l_data[2][8]=m_gamma*cos(l_states.teta_rad()); + + l_data[3][6]=m_gamma*l_states.omega()*cos(l_states.teta_rad()); + l_data[3][8]=m_gamma*sin(l_states.teta_rad()); + + double l_alpha_rad=l_input.alpha()*DEG2RAD; + l_data[7][8]=l_data[6][8]=m_dt*m_gamma*tan(l_alpha_rad)/m_wb; + //l_data[7][11]=l_data[6][11]=m_dt*m_gamma*f_state.omega/(m_wb*pow(cos(l_alpha_rad),2)); + + l_data[8][8]=(1-m_dt*m_bJ); + l_data[8][9]=m_dt*m_KJ; + l_data[9][8]=m_dt*m_KL; + l_data[9][9]=(1-m_dt*m_RL); + //l_data[9][10]=m_dt; + return l_data; + } + + private: + /* Constant values */ + /* Time step */ + const double m_dt; + /* gamma=Meter/Rotation */ + const double m_gamma; + /* Wheel base distance in meter */ + const double m_wb; + /* Motor Constants */ + const double m_bJ,m_KJ,m_KL,m_RL; + /* Please leave this matrix to be the last member in this class, + as it will be initialized */ + linalg::CMatrix<double,5,10> m_ObservationMatrix; + }; + }; + }; +}; + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Examples/SystemModels/ackermanntypes.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Examples/SystemModels/ackermanntypes.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,109 @@ +/** + ****************************************************************************** + * @file AckermannTypes.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration and implementation for the + Ackermann types. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef ACKERMANN_TYPES_HPP +#define ACKERMANN_TYPES_HPP + +#include <Linalg/linalg.h> + +namespace examples +{ + namespace systemmodels + { + namespace ackermannmodel + { + class CState: public linalg::CMatrix<double,10,1> + { + public: + CState():linalg::CMatrix<double,10,1>(){} + CState(const linalg::CMatrix<double,10,1>& f_matrix):linalg::CMatrix<double,10,1>(f_matrix){} + double& x(){return m_data[0][0];} + const double& x() const {return m_data[0][0];} + + double& y(){return m_data[1][0];} + const double& y() const {return m_data[1][0];} + + double& x_dot(){return m_data[2][0];} + const double& x_dot() const {return m_data[2][0];} + + double& y_dot(){return m_data[3][0];} + const double& y_dot() const{return m_data[3][0];} + + double& x_dot_prev(){return m_data[4][0];} + const double& x_dot_prev() const{return m_data[4][0];} + + double& y_dot_prev(){return m_data[5][0];} + const double& y_dot_prev() const{return m_data[5][0];} + + double& teta_rad(){return m_data[6][0];} + const double& teta_rad() const{return m_data[6][0];} + + double& teta_rad_dot(){return m_data[7][0];} + const double& teta_rad_dot() const{return m_data[7][0];} + + double& omega(){ return m_data[8][0];} + const double& omega() const{return m_data[8][0];} + + double& i(){return m_data[9][0];} + const double& i() const{return m_data[9][0];} + + // CState& operator=(const linalg::CMatrix<double,10,1>& f_matrix){ + // for (uint32_t l_row = 0; l_row < 10; ++l_row) + // { + // for (uint32_t l_col = 0; l_col < 1; ++l_col) + // { + // this->m_data[l_row][l_col] = f_matrix[l_row][l_col]; + // } + // } + // return *this; + // } + }; + + class CInput:public linalg::CMatrix<double,2,1> + { + public: + CInput():linalg::CMatrix<double,2,1>(){} + CInput(const linalg::CMatrix<double,2,1>& f_matrix):linalg::CMatrix<double,2,1>(f_matrix){} + double& v(){return m_data[0][0];} + const double& v()const{return m_data[0][0];} + double& alpha(){return m_data[1][0];} + const double& alpha()const{return m_data[1][0];} + }; + + class COutput:public linalg::CMatrix<double,5,1> + { + public: + COutput():linalg::CMatrix<double,5,1>(){} + COutput(const linalg::CMatrix<double,5,1>& f_matrix):linalg::CMatrix<double,5,1>(f_matrix){} + double& x_ddot(){return m_data[0][0];} + const double& x_ddot() const{return m_data[0][0];} + double& y_ddot(){return m_data[1][0];} + const double& y_ddot()const {return m_data[1][0];} + double& teta_rad_dot(){return m_data[2][0];} + const double& teta_rad_dot()const{return m_data[2][0];} + double& speed(){return m_data[3][0];} + const double& speed()const {return m_data[3][0];} + + // double& alpha(){ + // return m_data[4][0]; + // } + // const double& alpha()const{ + // return m_data[4][0]; + // } + double& i(){return m_data[4][0];} + const double& i()const{return m_data[4][0];} + }; + }; + }; +}; + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Examples/blinker.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Examples/blinker.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,40 @@ +/** + ****************************************************************************** + * @file Blinker.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class definition for the blinker + * functionality. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef BLINKER_HPP +#define BLINKER_HPP + +/* The mbed library */ +#include <mbed.h> +#include <TaskManager/taskmanager.hpp> + + +namespace examples{ + + /** + * @brief It is used for toggling an LED. + * + */ + class CBlinker : public task::CTask + { + public: + /* Construnctor */ + CBlinker(uint32_t f_period, DigitalOut f_led); + private: + /* Run method */ + virtual void _run(); + /* Digital output line connected to a LED */ + DigitalOut m_led; + }; +}; // namespace examples + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Examples/echoer.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Examples/echoer.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,41 @@ +/** + ****************************************************************************** + * @file Echoer.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class definition for the serial echoer + * functionality. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef ECHOER_HPP +#define ECHOER_HPP + +/* The mbed library */ +#include <mbed.h> +#include <TaskManager/taskmanager.hpp> + + +namespace examples{ + /** + * @brief It is used for echoing messages over UART. + * + */ + class CEchoer : public task::CTask + { + public: + /* Construnctor */ + CEchoer(uint32_t f_period, Serial& f_serialPort); + private: + /* Run method */ + virtual void _run(); + + /* Serial communication member*/ + Serial& m_serialPort; + }; + +}; // namespace examples + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Filter/filter.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Filter/filter.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,318 @@ +/** + ****************************************************************************** + * @file Filter.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the filter + * functionality. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef FILTER_HPP +#define FILTER_HPP + +#include <Linalg/linalg.h> +#include <SystemModels/systemmodels.hpp> + +namespace filter +{ + /** + * @brief The filter interface + * + * @tparam T The type of the input and output signal + */ + template<class T> + class CFilterFunction + { + public: + virtual T operator()(T&)=0; + }; + + namespace lti + { + namespace siso + { + /** + * @brief Infinite impulse response (IIR) discrete-time filter. + * + */ + template <class T, uint32_t NA, uint32_t NB> + class CIIRFilter:public CFilterFunction<T> + { + public: + /* Constructor */ + CIIRFilter(const linalg::CRowVector<T,NA>& f_A,const linalg::CRowVector<T,NB>& f_B); + /* Operator */ + T operator()(T& f_u); + private: + CIIRFilter(){} + linalg::CRowVector<T,NA> m_A; + linalg::CRowVector<T,NB> m_B; + linalg::CColVector<T,NA> m_Y; + linalg::CColVector<T,NB> m_U; + }; + + /** + * @brief Finite impulse response (FIR) discrete-time filter. + * + */ + template <class T, uint32_t NB> + class CFIRFilter:public CFilterFunction<T> + { + public: + /* Constructor */ + CFIRFilter(const linalg::CRowVector<T,NB>& f_B); + /* Operator */ + T operator()(T& f_u); + private: + CFIRFilter() {} + linalg::CRowVector<T,NB> m_B; + linalg::CColVector<T,NB> m_U; + }; + + /** + * @brief Mean filter + * + * @tparam T + * @tparam NB + */ + template <class T, uint32_t NB> + class CMeanFilter:public CFilterFunction<T> + { + public: + /* Constructor */ + CMeanFilter(); + /* Operator */ + virtual T operator()(T& f_u); + private: + T m_B; + linalg::CColVector<T,NB> m_U; + }; + }; + + namespace mimo + { + //! CSSModel class. + /*! + * + * */ + template <class T, uint32_t NA, uint32_t NB, uint32_t NC> + class CSSModel + { + public: + using CStateType = linalg::CColVector<T,NA>; + using CStateTransitionType = linalg::CMatrix<T,NA,NA>; + using CInputType = linalg::CColVector<T,NB>; + using CMeasurementType = linalg::CColVector<T,NC>; + using CInputMatrixType = linalg::CMatrix<T,NA,NB>; + using CMeasurementMatrixType = linalg::CMatrix<T,NC,NA>; + using CDirectTransferMatrixType = linalg::CMatrix<T,NC,NB>; + /* Constructor */ + CSSModel( + const CStateTransitionType& f_stateTransitionMatrix, + const CInputMatrixType& f_inputMatrix, + const CMeasurementMatrixType& f_measurementMatrix); + /* Constructor */ + CSSModel( + const CStateTransitionType& f_stateTransitionMatrix, + const CInputMatrixType& f_inputMatrix, + const CMeasurementMatrixType& f_measurementMatrix, + const CDirectTransferMatrixType& f_directTransferMatrix); + /* Constructor */ + CSSModel( + const CStateTransitionType& f_stateTransitionMatrix, + const CInputMatrixType& f_inputMatrix, + const CMeasurementMatrixType& f_measurementMatrix, + const CDirectTransferMatrixType& f_directTransferMatrix, + const CStateType& f_state); + + /* Get state method */ + const CStateType& state() const {return m_stateVector;} + CStateType& state() {return m_stateVector;} + + /* Operator */ + CMeasurementType operator()(const CInputType& f_inputVector); + /* Update state */ + void updateState(const CInputType& f_inputVector); + /* Get output */ + CMeasurementType getOutput(const CInputType& f_inputVector); + + private: + CSSModel() {} + /* state vector */ + CStateType m_stateVector; + /* state transition matrix */ + CStateTransitionType m_stateTransitionMatrix; + /* input matrix */ + CInputMatrixType m_inputMatrix; + /* measurement matrix */ + CMeasurementMatrixType m_measurementMatrix; + /* direct transfer matrix */ + CDirectTransferMatrixType m_directTransferMatrix; + }; + + //! CKalmanFilter class. + /*! + * + * */ + template <class T, uint32_t NA, uint32_t NB, uint32_t NC> + class CKalmanFilter + { + public: + using CStateType = linalg::CVector<T, NA>; + using CInputVectorType = linalg::CVector<T, NB>; + using COutputVectorType = linalg::CVector<T, NC>; + using CInputType = linalg::CMatrix<T, NA, NB>; + using CControInputType = linalg::CMatrix<T, NA, NC>; + using CModelCovarianceType = linalg::CMatrix<T, NA, NA>; + using CMeasurementCovarianceType = linalg::CMatrix<T, NC, NC>; + using CStateTransType = linalg::CMatrix<T, NA, NA>; + using CMeasurementType = linalg::CMatrix<T, NC, NA>; + using CKalmanGainType = linalg::CMatrix<T, NA, NC>; + + /* Constructor */ + CKalmanFilter( + const CStateTransType& f_stateTransitionModel, + const CControInputType& f_controlInput, + const CMeasurementType& f_observationModel, + const CModelCovarianceType& f_covarianceProcessNoise, + const CMeasurementCovarianceType& f_observationNoiseCovariance); + + /* Get state method */ + const CStateType& state() const + { + return m_posterioriState; + } + /* Get state method */ + CStateType& state() + { + return m_posterioriState; + } + /* Operator */ + CMeasurementType operator()(const CInputVectorType& f_input); + /* Operator */ + CMeasurementType operator()(); + /* Predict */ + void predict(); + /* Predict */ + void predict(const CInputVectorType& f_input); + /* Update */ + const CMeasurementType& update(void); + + private: + CKalmanFilter() {} + + CStateType m_previousState; // previous state + CStateType m_prioriState; // priori state + CStateType m_posterioriState; // posteriori state + CControInputType m_controlInput; // control input modelþ + CModelCovarianceType m_previousCovariance; // previous covariance estimate // <- + CModelCovarianceType m_prioriCovariance; // priori covariance estimate // <- + CModelCovarianceType m_posterioriCovariance; // posteriori covariance estimate // <- + CStateTransType m_stateTransitionModel; // state transition model + CModelCovarianceType m_covarianceProcessNoise; // covariance of process noise // done + CMeasurementType m_measurementResidual; // innovation or measurement residual + CMeasurementType m_measurement; // observation (or measurement) + CMeasurementType m_observationModel; // observation model + CMeasurementCovarianceType m_residualCovariance; // innovation or residual covariance // <- + CMeasurementCovarianceType m_observationNoiseCovariance; // covariance of observation noise // <- + CKalmanGainType m_kalmanGain; // optimal kalman gain + }; + + }; + }; + namespace ltv + { + namespace mimo + { + // Extended Kalman Filter + // Template parameters: + // - T variable type + // - NA no. of inputs + // - NB no. of states + // - NC no. of outputs + // Input parameters + template <class T, uint32_t NA, uint32_t NB, uint32_t NC> + class CEKF + { + public: + using CSystemModelType = systemmodels::nlti::mimo::CDiscreteTimeSystemModel<T,NA,NB,NC>; + using CJacobianMatricesType = systemmodels::nlti::mimo::CJacobianMatrices<T,NA,NB,NC>; + using CStatesType = linalg::CMatrix<T,NB,1>; + using CInputType = linalg::CMatrix<T,NA,1>; + using COutputType = linalg::CMatrix<T,NC,1>; + using CJMTransitionType = linalg::CMatrix<T,NB,NB>; + using CJMObservationType = linalg::CMatrix<T,NC,NB>; + using CObservationNoiseType = linalg::CMatrix<T,NC,NC>; + using CKalmanGainType = linalg::CMatrix<T,NC,NB>; + + /* Constructor */ + CEKF(CSystemModelType& f_systemModel + ,CJacobianMatricesType& f_jbMatrices + ,const CJMTransitionType& f_Q + ,const CObservationNoiseType& f_R); + /* Predict */ + void predict (const CInputType& f_input); + /* Update */ + void update (const CInputType& f_input + ,const COutputType& f_measurement); + protected: + private: + CSystemModelType& m_systemModel; + CJacobianMatricesType& m_jbMatrices; + //Predicted covariance matrix + CJMTransitionType m_covarianceMatrix; + //The covariance of the process noise + CJMTransitionType m_Q; + //The covariance of the observation noise + CObservationNoiseType m_R; + + }; + }; + }; + + namespace nonlinear + { + namespace siso + { + + /** + * @brief Median filter + * + */ + template <class T, uint32_t N> + class CMedianFilter:public CFilterFunction<T> + { + public: + /* Constructor */ + CMedianFilter(); + inline T addNewValue(T& f_val); + inline T getMedian(); + inline T operator()(T& f_v); + + private: + struct my_structure + { + T info; + my_structure *next; + my_structure *prev; + + }; + + my_structure *m_median; //pointer to the loist element that represents the median + my_structure *m_smallest; //pionter to the smallest element of the list (the first) + uint32_t m_new; //index to tyhe newest element in the list + uint32_t m_size; //the size of the list/queue + + my_structure m_queue[N]; + }; + } + } +}; + +#include "filter.inl" + +#endif // FILTER_H +
diff -r c3e774091195 -r ceee5a608e7c include/Filter/filter.inl --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Filter/filter.inl Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,558 @@ +/** + ****************************************************************************** + * @file Filter.inl + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class implementations for the filter + * functionality. + ****************************************************************************** + */ + +template <class T, uint32_t NC> +using CMeasurementType = linalg::CColVector<T,NC>; + +/******************************************************************************/ +/** @brief CIIRFilter Class constructor + * + * Constructor method + * + * @param f_A the feedback filter coefficients + * @param f_B the feedforward filter coefficients + */ +template <class T, uint32_t NA, uint32_t NB> +filter::lti::siso::CIIRFilter<T,NA,NB>::CIIRFilter(const linalg::CRowVector<T,NA>& f_A,const linalg::CRowVector<T,NB>& f_B) + : m_A(f_A) + , m_B(f_B) + , m_Y() + , m_U() +{ +} + +/** @brief Operator + * + * @param f_u reference to input data + * @return the filtered output data + */ +template <class T, uint32_t NA, uint32_t NB> +T filter::lti::siso::CIIRFilter<T,NA,NB>::operator()(T& f_u) +{ + for (uint32_t l_idx = NB-1; l_idx > 0; --l_idx) + { + m_U[l_idx] = m_U[l_idx-1]; + } + m_U[0][0] = f_u; + + linalg::CMatrix<T,1,1> l_y = m_B*m_U - m_A*m_Y; + // T l_y = m_B*m_U - m_A*m_Y; + + for (uint32_t l_idx = NA-1; l_idx > 0 ; --l_idx) + { + m_Y[l_idx] = m_Y[l_idx-1]; + } + m_Y[0][0] = l_y[0][0]; + + return m_Y[0][0]; +} + +/******************************************************************************/ +/** @brief CFIRFilter Class constructor + * + * Constructor method + * + * @param f_B the feedforward filter coefficients + */ +template <class T, uint32_t NB> +filter::lti::siso::CFIRFilter<T,NB>::CFIRFilter(const linalg::CRowVector<T,NB>& f_B) + : m_B(f_B), m_U() +{ +} + +/** @brief Operator + * + * @param f_u reference to the input data + * @return the filtered output data + */ +template <class T, uint32_t NB> +T filter::lti::siso::CFIRFilter<T,NB>::operator()(T& f_u) +{ + for (uint32_t l_idx = NB-1; l_idx > 0; --l_idx) + { + m_U[l_idx] = m_U[l_idx-1]; + } + m_U[0] = f_u; + + linalg::CMatrix<T,1,1> l_y = m_B*m_U; + + return l_y[0][0]; +} + +/******************************************************************************/ +/** @brief MeanFilter Class constructor + * + * Constructor method + * + * + */ +template <class T, uint32_t NB> +filter::lti::siso::CMeanFilter<T,NB>::CMeanFilter() + : m_B(1./NB) + , m_U() +{ +} + +/** @brief Operator + * + * @param f_u reference to the input data + * @return the filtered output data + */ +template <class T, uint32_t NB> +T filter::lti::siso::CMeanFilter<T,NB>::operator()(T& f_u) +{ + T l_y =0; + + for (uint32_t l_idx = 1; l_idx < NB; ++l_idx) + { + m_U[l_idx][0] = m_U[l_idx-1][0]; + l_y += m_U[l_idx][0]; + } + m_U[0][0] = f_u; + l_y += m_U[0][0]; + + return m_B*l_y; +} + +/******************************************************************************/ +/** @brief CSSModel Class constructor + * + * Constructor method + * + * @param f_stateTransitionMatrix + * @param f_inputMatrix + * @param f_measurementMatrix + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel( + const CStateTransitionType& f_stateTransitionMatrix, + const CInputMatrixType& f_inputMatrix, + const CMeasurementMatrixType& f_measurementMatrix) + : m_stateVector() + , m_stateTransitionMatrix(f_stateTransitionMatrix) + , m_inputMatrix(f_inputMatrix) + , m_measurementMatrix(f_measurementMatrix) + , m_directTransferMatrix() +{ + // do nothing +} + +/** @brief CSSModel Class constructor + * + * Constructor method + * + * @param f_stateTransitionMatrix + * @param f_inputMatrix + * @param f_measurementMatrix + * @param f_directTransferMatrix + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel( + const CStateTransitionType& f_stateTransitionMatrix, + const CInputMatrixType& f_inputMatrix, + const CMeasurementMatrixType& f_measurementMatrix, + const CDirectTransferMatrixType& f_directTransferMatrix) + : m_stateVector() + , m_stateTransitionMatrix(f_stateTransitionMatrix) + , m_inputMatrix(f_inputMatrix) + , m_measurementMatrix(f_measurementMatrix) + , m_directTransferMatrix(f_directTransferMatrix) +{ + // do nothing +} + +/** @brief CSSModel Class constructor + * + * Constructor method + * + * @param f_stateTransitionMatrix + * @param f_inputMatrix + * @param f_measurementMatrix + * @param f_directTransferMatrix + * @param f_state + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel( + const CStateTransitionType& f_stateTransitionMatrix, + const CInputMatrixType& f_inputMatrix, + const CMeasurementMatrixType& f_measurementMatrix, + const CDirectTransferMatrixType& f_directTransferMatrix, + const CStateType& f_state) + : m_stateVector(f_state) + , m_stateTransitionMatrix(f_stateTransitionMatrix) + , m_inputMatrix(f_inputMatrix) + , m_measurementMatrix(f_measurementMatrix) + , m_directTransferMatrix(f_directTransferMatrix) +{ + // do nothing +} + +/** @brief Operator + * + * @param f_inputVector reference to input vector + * @return the output data from the system model (output vector) + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +CMeasurementType<T,NC> filter::lti::mimo::CSSModel<T,NA,NB,NC>::operator()(const CInputType& f_inputVector) +{ + updateState(f_inputVector); + + return getOutput(f_inputVector); +} + +/** @brief Update state method + * + * @param f_inputVector reference to input vector + * @return None + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +void filter::lti::mimo::CSSModel<T,NA,NB,NC>::updateState(const CInputType& f_inputVector) +{ + m_stateVector = m_stateTransitionMatrix * m_stateVector + m_inputMatrix * f_inputVector; +} + +/** @brief Get output + * + * @param f_inputVector reference to input vector + * @return the output data from the system model (output vector) + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +CMeasurementType<T,NC> filter::lti::mimo::CSSModel<T,NA,NB,NC>::getOutput(const CInputType& f_inputVector) +{ + return m_measurementMatrix * m_stateVector + m_directTransferMatrix * f_inputVector; +} + +/******************************************************************************/ +/** @brief CKalmanFilter Class constructor + * + * Constructor method + * + * @param f_stateTransitionModel + * @param f_controlInput + * @param f_observationModel + * @param f_covarianceProcessNoise + * @param f_observationNoiseCovariance + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::CKalmanFilter( + const CStateTransType& f_stateTransitionModel, + const CControInputType& f_controlInput, + const CMeasurementType& f_observationModel, + const CModelCovarianceType& f_covarianceProcessNoise, + const CMeasurementCovarianceType& f_observationNoiseCovariance) + : m_stateTransitionModel(f_stateTransitionModel) + , m_controlInput(f_controlInput) + , m_observationModel(f_observationModel) + , m_covarianceProcessNoise(f_covarianceProcessNoise) + , m_observationNoiseCovariance(f_observationNoiseCovariance) +{ +} + +/** @brief Operator + * + * @param f_input reference to input vector + * @return Updated (a posteriori) state estimate + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +linalg::CMatrix<T, NC, NA> filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::operator()(const CInputVectorType& f_input) +{ + predict(f_input); + return update(); +} + +/** @brief Operator + * + * + * @return Updated (a posteriori) state estimate + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +linalg::CMatrix<T, NC, NA> filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::operator()() +{ + predict(); + return update(); +} + +/** @brief Predict + * + * + * + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +void filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::predict() +{ + m_previousState = m_prioriState; + m_previousCovariance = m_prioriCovariance; + m_prioriState = m_stateTransitionModel * m_previousState; + m_prioriCovariance = m_stateTransitionModel * m_previousCovariance * transpose(m_stateTransitionModel) + m_covarianceProcessNoise; +} + +/** @brief Predict + * + * @param f_input vector input + * + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +void filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::predict(const CInputVectorType& f_input) +{ + m_previousState = m_prioriState; + m_previousCovariance = m_prioriCovariance; + m_prioriState = m_stateTransitionModel * m_previousState + m_controlInput * f_input; + m_prioriCovariance = m_stateTransitionModel * m_previousCovariance * transpose(m_stateTransitionModel) + m_covarianceProcessNoise; +} + +/** @brief Update + * + * + * @return Measurement residual + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +const linalg::CMatrix<T, NC, NA>& filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::update(void) +{ + m_measurementResidual = m_measurement - m_observationModel * m_prioriState; + m_measurement = m_observationModel * m_posterioriState; + m_residualCovariance = m_observationModel * m_prioriCovariance * transpose(m_observationModel) + m_observationNoiseCovariance; + m_kalmanGain = m_prioriCovariance * transpose(m_observationModel) * m_residualCovariance.inv(); + m_posterioriState = m_prioriState + m_kalmanGain * m_measurementResidual; + m_posterioriCovariance = ( CStateTransType::eye() - m_kalmanGain * m_observationModel ) * m_prioriCovariance; + return m_posterioriState; +} + +/******************************************************************************/ +/** @brief CEKF Class constructor + * + * Constructor method + * + * @param f_systemModel + * @param f_jbMatrices + * @param f_Q + * @param f_R + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +filter::ltv::mimo::CEKF<T,NA,NB,NC>::CEKF( + CSystemModelType& f_systemModel + ,CJacobianMatricesType& f_jbMatrices + ,const CJMTransitionType& f_Q + ,const CObservationNoiseType& f_R) + :m_systemModel(f_systemModel) + ,m_jbMatrices(f_jbMatrices) + ,m_covarianceMatrix(linalg::CMatrix<T,NB,NB>::ones()) + ,m_Q(f_Q) + ,m_R(f_R) +{ +} + +/** @brief Predict + * + * @param f_input vector input + * + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +void filter::ltv::mimo::CEKF<T,NA,NB,NC>::predict(const CInputType& f_input) +{ + //Previous updated state + CStatesType l_prev_states=m_systemModel.getStates(); + CJMTransitionType l_JF=m_jbMatrices.getJMTransition(l_prev_states,f_input); + //Predicted state estimate X_{k|k-1} + CStatesType l_pred_states=m_systemModel.update(f_input); + //Predicted covariance estimate + m_covarianceMatrix=l_JF*m_covarianceMatrix*l_JF.transpose()+m_Q; +} + +/** @brief Update + * + * @param f_input vector input + * @param f_measurement vector input + * + */ +template <class T, uint32_t NA, uint32_t NB, uint32_t NC> +void filter::ltv::mimo::CEKF<T,NA,NB,NC>::update(const CInputType& f_input + ,const COutputType& f_measurement) +{ + // Estimated system output + COutputType l_y_est=m_systemModel.calculateOutput(f_input); + // Innovation or measurement residual + COutputType l_mes_res=f_measurement-l_y_est; + // Innovation (or residual) covariance + CStatesType l_states=m_systemModel.getStates(); + CJMObservationType l_JH=m_jbMatrices.getJMObservation(f_input,l_states); + CObservationNoiseType l_s=l_JH*m_covarianceMatrix*l_JH.transpose()+m_R; + //Near-optimal Kalman gain + CKalmanGainType l_K=m_covarianceMatrix*l_JH.transpose()*l_s.inv(); + //Updated state estimate + CStatesType l_updated_states=l_states+l_K*l_mes_res; + m_systemModel.setStates(l_updated_states); + //Updated covariance estimate + m_covarianceMatrix=(CJMTransitionType::eye()-l_K*l_JH)*m_covarianceMatrix; +} + +/******************************************************************************/ +/** @brief CMedianFilter Class constructor + * + * Constructor method + * + * + */ +template <class T, uint32_t N> +filter::nonlinear::siso::CMedianFilter<T,N>::CMedianFilter() + :m_median() + ,m_smallest() + ,m_new(0) + ,m_size(N) + ,m_queue() +{ + // for (unsigned int l_idx = 0; l_idx < N; l_idx++) + // { + // m_queue[l_idx] = new structura; + + // } + + for (unsigned int l_idx = 0; l_idx < N ; l_idx++) + { + m_queue[l_idx].info = 0; + m_queue[l_idx].next = &(m_queue[(l_idx + 1) % N]); + m_queue[l_idx].prev = &(m_queue[(N + l_idx - 1) % N]); + } + + m_new = N - 1; + m_size = N; + m_median = &(m_queue[m_size / 2]); + m_smallest =&(m_queue[0]); +} + +/** @brief Operator + * + * @param f_val the input data + * @return filted the data + */ +template <class T, uint32_t N> +T filter::nonlinear::siso::CMedianFilter<T,N>::operator()(T& f_val) +{ + m_new = (m_new + 1) % m_size; //shift new index //inca e valoarea veche + /* // varianta pentru a decide valoarea mediana eficient-----eficient daca filtrul are dimensiuni mari + // ->V2 start remy_structurere EXISTA CAUZE CARE NU SUNT TRATATE CORECT SAU NETRATATE COMPLET!!!!!!!!!!! + if ((m_queue[m_new]->info > m_median->info) && (f_val <= m_median->info)) + { + if (f_val > m_median->prev->info) + { + m_median = m_queue[m_new]; //med=new + } + else + { + m_median = m_median->prev; // <- + } + } + else if ((m_queue[m_new]->info < m_median->info) && (f_val > m_median->info)) + { + if (f_val > m_median->next->info) + { + m_median = m_median->next; // -> + } + else + { + m_median = m_queue[m_new]; //med=new + } + } + else if ((m_queue[m_new]->info == m_median->info)) + { + if ((f_val < m_median->info)&&(f_val <= m_median->prev->info)) + { + m_median = m_median->prev; // <- + } + else if (f_val > m_median->info) + { + if (f_val <= m_median->next->info) + { + m_median = m_queue[m_new]; //med=new + } + else + { + m_median = m_median->next; // -> + } + } + else + { + m_median = m_queue[m_new]; //med=new + } + } + */ + m_queue[m_new ].info = f_val; //suprascrie cea mai veche valoare + + //ordonare dupa valoare + + //elementul new se "scoate" din lista + m_queue[m_new].prev->next = m_queue[m_new].next; //5. + m_queue[m_new].next->prev = m_queue[m_new].prev; //6. + + //update smallest value + my_structure* l_i = m_smallest; + if (&(m_queue[m_new]) == m_smallest) + { + if (m_queue[m_new].info > m_smallest->next->info) + { + m_smallest = m_smallest->next; + l_i = m_smallest; + } + } + else if (m_queue[m_new].info <= m_smallest->info) + { + l_i = m_smallest; + m_smallest = &m_queue[m_new]; + } + + //cautarea locului unde trebuie sa se amplaseze noul element in lista + unsigned int l_cnt = 1; + if (&(m_queue[m_new]) == l_i) + { + l_i = l_i->next; + } + + while ((l_i->info < m_queue[m_new].info) && (l_cnt <= m_size - 1)) + { + l_i = l_i->next; + l_cnt++; + } + + //inserarea elemntului new la locul potrivit + l_i->prev->next = &m_queue[m_new]; //1. + m_queue[m_new].next = l_i; //2. + m_queue[m_new].prev = l_i->prev; //3. + l_i->prev = &m_queue[m_new]; //4. + + //varianta ineficienta pentru aflarea medianului cand filtrul are dimensiuni mari + m_median=m_smallest; + for(uint8_t iddx=0; iddx < m_size/2; ++iddx) + { + m_median=m_median->next; + } + + return getMedian(); +} + +/* */ +/** @brief OperGet medianator + * + * + * @return median value + */ +template <class T, uint32_t N> +T filter::nonlinear::siso::CMedianFilter<T, N>::getMedian() +{ + T ret_val; + if (1 == m_size % 2) // daca filtrul are lungime impara + { + ret_val = m_median->info; + } + else // daca filtrul are lungime para + { + ret_val = 0.5*(m_median->info + m_median->prev->info); + } + return ret_val; +}
diff -r c3e774091195 -r ceee5a608e7c include/Linalg/linalg.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Linalg/linalg.h Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,473 @@ +#ifndef LINALG_H +#define LINALG_H + +// #include <mbed.h> +#include <stdint.h> +#include <array> + +namespace linalg +{ + template <class T, uint32_t M, uint32_t N> + class CMatrix + { + public: + using CThisType = CMatrix<T,M,N>; + using CTransposeType = CMatrix<T,N,M>; + using CContainerType = std::array<std::array<T,N>,M>; + using CDataType =T; + + template <uint32_t P> + using CLeftMultipliableType = CMatrix<T,P,M>; + + template <uint32_t P> + using CRightMultipliableType = CMatrix<T,N,P>; + + template <uint32_t P> + using CLeftMultiplicationResultType = CMatrix<T,P,N>; + + template <uint32_t P> + using CRightMultiplicationResultType = CMatrix<T,M,P>; + + friend class CMatrix<T,N,M>; + + CMatrix() : m_data() {} + CMatrix(const CThisType& f_matrix) : m_data(f_matrix.m_data) {} + CMatrix(const CThisType&& f_matrix) : m_data(f_matrix.m_data) {} + CMatrix(const CContainerType& f_data) : m_data(f_data) {} + CMatrix(const CContainerType&& f_data) : m_data(f_data) {} + + CThisType& operator=(const CThisType& f_matrix) + { + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + this->m_data[l_row][l_col] = f_matrix.m_data[l_row][l_col]; + } + } + return *this; + } + CThisType& operator=(const CThisType&& f_matrix) + { + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + this->m_data[l_row][l_col] = f_matrix.m_data[l_row][l_col]; + } + } + return *this; + } + + std::array<T,N>& operator[](uint32_t f_row) + { + return m_data[f_row]; + } + + CDataType& operator()(uint32_t f_row, uint32_t f_col) + { + return m_data[f_row][f_col]; + } + + const std::array<T,N>& operator[](uint32_t f_row) const + { + return m_data[f_row]; + } + + const CDataType& operator()(uint32_t f_row, uint32_t f_col) const + { + return m_data[f_row][f_col]; + } + + // template<> + // CMatrix<T,1,1>::operator CDataType() + // { + // return m_data[0][0]; + // } + + CThisType& operator+() {return *this;} + CThisType operator-() + { + CThisType l_matrix; + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + l_matrix.m_data[l_row][l_col] = -this->m_data[l_row][l_col]; + } + } + return l_matrix; + } + CThisType operator+(const CThisType& f_matrix) + { + CThisType l_matrix; + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] + f_matrix.m_data[l_row][l_col]; + } + } + return l_matrix; + } + CThisType operator-(const CThisType& f_matrix) + { + CThisType l_matrix; + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] - f_matrix.m_data[l_row][l_col]; + } + } + return l_matrix; + } + CThisType& operator+=(const CThisType& f_matrix) + { + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + this->m_data[l_row][l_col] += f_matrix.m_data[l_row][l_col]; + } + } + return *this; + } + CThisType& operator-=(const CThisType& f_matrix) + { + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + this->m_data[l_row][l_col] -= f_matrix.m_data[l_row][l_col]; + } + } + return *this; + } + + CThisType operator+(const CDataType& f_val) + { + CThisType l_matrix; + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] + f_val; + } + } + return l_matrix; + } + CThisType operator-(const CDataType& f_val) + { + CThisType l_matrix; + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] - f_val; + } + } + return l_matrix; + } + CThisType& operator+=(const CDataType& f_val) + { + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + this->m_data[l_row][l_col] += f_val; + } + } + return *this; + } + CThisType& operator-=(const CDataType& f_val) + { + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + this->m_data[l_row][l_col] -= f_val; + } + } + return *this; + } + CThisType& operator*=(const CDataType& f_val) + { + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + this->m_data[l_row][l_col] *= f_val; + } + } + return *this; + } + CThisType& operator*=(const CThisType& f_val) + { + CThisType& l_thisRef(*this); + l_thisRef = l_thisRef * f_val; + return l_thisRef; + } + CThisType& operator/=(const CDataType& f_val) + { + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + this->m_data[l_row][l_col] /= f_val; + } + } + return *this; + } + CThisType operator*(const CDataType& f_val) + { + CThisType l_matrix; + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] * f_val; + } + } + return l_matrix; + } + template <uint32_t P> + CRightMultiplicationResultType<P> operator*(const CRightMultipliableType<P>& f_matrix) + { + CRightMultiplicationResultType<P> l_matrix; + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < P; ++l_col) + { + l_matrix[l_row][l_col] = 0; + for (uint32_t l_idx = 0; l_idx < N; ++l_idx) + { + l_matrix[l_row][l_col] += this->m_data[l_row][l_idx] * f_matrix[l_idx][l_col]; + } + } + } + return l_matrix; + } + CThisType inv(); + + CTransposeType transpose() + { + CTransposeType l_trsp; + + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + l_trsp.m_data[l_col][l_row] = this->m_data[l_row][l_col]; + } + } + + return l_trsp; + } + + template <uint32_t P> + CRightMultiplicationResultType<P> solve(const CRightMultipliableType<P>& f_B) + { + return CLUDecomposition(*this).solve(); + } + + static CThisType zeros() + { + CThisType l_res; + + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + l_res[l_row][l_col] = 0; + } + } + + return l_res; + } + + static CThisType ones() + { + CThisType l_res; + + for (uint32_t l_row = 0; l_row < M; ++l_row) + { + for (uint32_t l_col = 0; l_col < N; ++l_col) + { + l_res[l_row][l_col] = 1; + } + } + + return l_res; + } + + static CThisType eye() + { + CThisType l_res(CThisType::zeros()); + uint32_t l_minDim = std::min(M,N); + + for (uint32_t l_row = 0; l_row < l_minDim; ++l_row) + { + l_res[l_row][l_row] = 1; + } + + return l_res; + } + + protected: + std::array<std::array<T,N>,M> m_data; + }; + + template<class T, uint32_t N> + class CLUDecomposition + { + public: + using CThisType = CLUDecomposition<T,N>; + using COriginalType = CMatrix<T,N,N>; + using CDataType =T; + + template <uint32_t P> + using CLeftMultipliableType = CMatrix<T,P,N>; + + template <uint32_t P> + using CRightMultipliableType = CMatrix<T,N,P>; + + template <uint32_t P> + using CLeftMultiplicationResultType = CMatrix<T,P,N>; + + template <uint32_t P> + using CRightMultiplicationResultType = CMatrix<T,N,P>; + + CLUDecomposition(const CThisType& f_decomposition) : m_L(f_decomposition.m_L), m_U(f_decomposition.m_U), m_P(f_decomposition.m_P) {} + CLUDecomposition(const CThisType&& f_decomposition) : m_L(f_decomposition.m_L), m_U(f_decomposition.m_U), m_P(f_decomposition.m_P) {} + CLUDecomposition(const COriginalType& f_matrix) : m_L(), m_U(), m_P() {decompose(f_matrix);} + CLUDecomposition(const COriginalType&& f_matrix) : m_L(), m_U(), m_P() {decompose(f_matrix);} + + operator COriginalType() + { + return m_L * m_U; + } + + COriginalType inv() + { + return triUInv() * triLInv(); + } + + COriginalType triLInv() + { + COriginalType l_invL(m_L); + + for (uint32_t l_jdx = 0; l_jdx < N; ++l_jdx) + { + l_invL[l_jdx][l_jdx] = 1/l_invL[l_jdx][l_jdx]; + + for (uint32_t l_idx = l_jdx+1; l_idx < N; ++l_idx) + { + l_invL[l_idx][l_jdx] = 0; + + for (uint32_t l_kdx = l_jdx; l_kdx < l_idx; ++l_kdx) + { + l_invL[l_idx][l_jdx] -= m_L[l_idx][l_kdx]*l_invL[l_kdx][l_jdx]; + } + + l_invL[l_idx][l_jdx] /= l_invL[l_idx][l_idx]; + } + } + + return l_invL; + } + + COriginalType triUInv() + { + COriginalType l_invU(m_U); + + for (int32_t l_jdx = (N-1); l_jdx >= 0; --l_jdx) + { + l_invU[l_jdx][l_jdx] = 1/l_invU[l_jdx][l_jdx]; + + for (int32_t l_idx = (l_jdx-1); l_idx >= 0; --l_idx) + { + l_invU[l_idx][l_jdx] = 0; + + for (int32_t l_kdx = (l_idx+1); l_kdx < (l_jdx+1); ++l_kdx) + { + l_invU[l_idx][l_jdx] -= m_U[l_idx][l_kdx]*l_invU[l_kdx][l_jdx]; + } + + l_invU[l_idx][l_jdx] /= l_invU[l_idx][l_idx]; + } + } + + return l_invU; + } + + template <uint32_t P> + CRightMultiplicationResultType<P> solve(const CRightMultipliableType<P>& f_B) + { + CRightMultipliableType<P> l_B; + + for (uint32_t l_idx = 0; l_idx < N; ++l_idx) + { + for (uint32_t l_jdx = 0; l_jdx < P; ++l_jdx) + { + l_B[l_idx][l_jdx] = f_B[m_P[l_idx]][l_jdx]; + } + } + + return sTriL(sTriU(l_B)); + } + + template <uint32_t P> + CRightMultiplicationResultType<P> sTriU(const CRightMultipliableType<P>& f_B) + { + return triUInv()*f_B; + } + + template <uint32_t P> + CRightMultiplicationResultType<P> sTriL(const CRightMultipliableType<P>& f_B) + { + // return triLInv()*f_B; + } + + + // private: + void decompose(const CMatrix<T,N,N>& f_matrix) + { + m_U = f_matrix; + for (uint32_t l_kdx = 0; l_kdx < (N-1); ++l_kdx) + { + m_L[l_kdx][l_kdx] = 1; + m_P[l_kdx] = l_kdx; + + for(uint32_t l_idx = l_kdx+1; l_idx < N; ++l_idx) + { + m_L[l_idx][l_kdx] = m_U[l_idx][l_kdx]/m_U[l_kdx][l_kdx]; + m_U[l_idx][l_kdx] = 0; + for(uint32_t l_jdx = l_kdx+1; l_jdx < N; ++l_jdx) + { + m_U[l_idx][l_jdx] = m_U[l_idx][l_jdx] - m_L[l_idx][l_kdx]*m_U[l_kdx][l_jdx]; + } + } + } + m_L[N-1][N-1] = 1; + m_P[N-1] = N-1; + } + CMatrix<T,N,N> m_L; + CMatrix<T,N,N> m_U; + std::array<uint32_t,N> m_P; + }; + + template <class T, uint32_t N> + using CColVector = CMatrix<T,N,1>; + + template <class T, uint32_t N> + using CVector = CColVector<T,N>; + + template <class T, uint32_t N> + using CRowVector = CMatrix<T,1,N>; +}; + +#include "linalg.inl" + +#endif // LINALG_H +
diff -r c3e774091195 -r ceee5a608e7c include/Linalg/linalg.inl --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Linalg/linalg.inl Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,6 @@ +template<class T, uint32_t N,uint32_t M> +typename linalg::CMatrix<T,N,M>::CThisType linalg::CMatrix<T,N,M>::inv() +{ + CThisType l_inv(linalg::CLUDecomposition<T,N>(*this).inv()); + return l_inv; +}
diff -r c3e774091195 -r ceee5a608e7c include/MotionController/motioncontroller.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/MotionController/motioncontroller.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,136 @@ +/** + ****************************************************************************** + * @file MotionController.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the motion controller + * functionality. + ****************************************************************************** + */ + +#ifndef MOTION_CONTROLLER_HPP +#define MOTION_CONTROLLER_HPP + +/* The mbed library */ +#include <mbed.h> +#include <TaskManager/taskmanager.hpp> +#include <Move/move.hpp> +#include <BezierMotionPlanner/bezierMotionplanner.hpp> +#include <Controllers/controller.hpp> +#include <SafetyStop/safetystopfunction.hpp> + +#include <rtos.h> + +/** + * @brief Limit of the pwm command [percentage] + * + */ +#define MCTL_PWM_COMMAND_LIMIT 75 +/** + * @brief Limit of the velocity command [Centimeter per second] + * + */ +#define MCTL_SPEED_COMMAND_LIMIT 100 + +//! CMotionController class. +/*! + * It inherits class task::CTask. + * It is used for executing move commands. + * */ +class CMotionController +{ +public: + CMotionController( + float f_period_sec, + Serial& f_serialPort, + Move& f_car, + CSafetyStopFunction* f_safetyStop, + controllers::CControllerSiso* f_control = NULL); + CMotionController( + float f_period_sec, + Serial& f_serialPort, + Move& f_car, + controllers::CControllerSiso* f_control = NULL); + + + /* Serial callback method for Move command */ + static void staticSerialCallbackMove(void* obj,char const * a, char * b); + /* Serial callback method for BRAKE command */ + static void staticSerialCallbackBrake(void* obj,char const * a, char * b); + /* Serial callback method for hard BRAKE command */ + static void staticSerialCallbackHardBrake(void* obj,char const * a, char * b) ; + /* Serial callback method for PID activation command */ + static void staticSerialCallbackPID(void* obj,char const * a, char * b); + /* Static serial callback for spline command */ + static void staticSerialCallbackSpline(void* obj,char const * a,char * b ); + /* Static callback function for run method */ + static void staticCallbackRun(void* obj); + /* Start the Rtos timer for applying run */ + void startRtosTimer(); + + /* Reset method */ + void reset(); + /* Get speed method */ + float getSpeed(); + /* Get angle method */ + float getAngle(); + /* BRAKE callback method */ + void BrakeCallback(); + /* Set state method */ + void setState(int f_state); +private: + /* Run method */ + virtual void _run(); + /* Serial callback implementation */ + void serialCallbackMove(char const * a, char * b); + /* BRAKE serial callback */ + void serialCallbackBrake(char const * a, char * b); + /* Hard BRAKE serial callback */ + void serialCallbackHardBrake(char const * a, char * b); + /* PID serial callback */ + void serialCallbackPID(char const * a, char * b); + /* Spline serial callback */ + void serialCallbackSpline(char const * a, char * b); + + + /* Static function to convert from linear velocity ( centimeter per second ) of robot to angular velocity ( rotation per second ) of motor */ + static float Mps2Rps(float f_vel_cmps); + // /* Velocity to PWM function */ + // static float VEL2PWM(float vel); + +private: + /* reference to Serial object */ + Serial& m_serialPort; + /* reference to MOVE object */ + Move& m_car; + /* Speed */ + float m_speed; + /* Angle */ + float m_angle; + /* PEriod i nseconds */ + float m_period_sec; + /* Spline activation state */ + bool m_isSplineActivated; + /* State machine state */ + uint8_t m_state; + /* PID activation state */ + bool m_ispidActivated; + // 0-none + // 1-normal + // 2-brake regeneration + + /* motion planner object */ + planner::CBezierMotionPlanner m_motionPlanner; + /* Timeout */ + Timeout m_hbTimeOut; + /* Reference to control object */ + controllers::CControllerSiso* m_control; + /* Reference to safety stop function */ + CSafetyStopFunction* m_safetyStop; + /* Rtos timer for periodically applying */ + RtosTimer m_timer; +}; + +#endif // MOTION_CONTROLLER_H +
diff -r c3e774091195 -r ceee5a608e7c include/Move/move.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Move/move.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @file MOVE.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the moving + * functionality. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef MOVE_HPP +#define MOVE_HPP + +#include <Drivers/servo.hpp> +#include <Drivers/vnh.hpp> + +/** + * @brief Move class + * It is used for executing move commands. + * + */ +class Move +{ +public: + /* Constructor */ + Move(PinName, PinName, PinName, PinName, PinName);//A0 + /* Destructor */ + ~Move(); + /* Steer */ + void Steer(float angle);// -25 to + 25 degrees, - (left), + (right) + /* Speed */ + void Speed(float speed);//-100 to + 100 -(back), + (front) + /* Brake */ + void Brake(); + /* Inverse */ + void Inverse(float f_speed); + /* Test */ + void TestCar(); + /* Reset */ + void ResetCar(); + /* Return bridge object */ + drivers::VNH& getVNH(); + +private: + /* Servo pbject */ + drivers::SERVO servo; + /* Bridge object */ + drivers::VNH vnh; +}; + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Queue/queue.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Queue/queue.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,61 @@ +/** + ****************************************************************************** + * @file Queue.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class declaration for the queue + * functionality. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef QUEUE_HPP +#define QUEUE_HPP + + +/** + * @brief It is used for executing queue container. + * + * @tparam T The type of the varieble + * @tparam N The size of the queue + */ +template <class T, unsigned int N> +class CQueue +{ +public: + /* Constructor */ + CQueue(); + /* Destructor */ + virtual ~CQueue(); + /* Is full method */ + inline bool isFull(); + /* Is full method */ + inline bool isEmpty(); + /* Peek */ + inline T peek(); + /* Pop */ + inline T pop(); + /* Get queue size */ + inline unsigned int getSize(); + /* Push single element */ + inline void push(T& f_char); + /* Push multiple elements */ + inline void push(T *f_char, unsigned int f_len); + /* Empty queue */ + inline void empty(); +private: + /* buffer */ + volatile T m_buffer[N]; + /* start */ + volatile unsigned int m_start; + /* end */ + volatile unsigned int m_end; + /* size */ + volatile unsigned int m_size; +}; + +#include "queue.inl" + +#endif +
diff -r c3e774091195 -r ceee5a608e7c include/Queue/queue.inl --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Queue/queue.inl Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,153 @@ +/** + ****************************************************************************** + * @file Queue.inl + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class definition for the queue + * functionality. Inlin eimplementation. + ****************************************************************************** + */ + +/** @brief Queue Class constructor + * + * Constructor method + * + * + */ +template <class T, unsigned int N> +CQueue<T,N>::CQueue() + : m_buffer() + , m_start(0) + , m_end(N-1) + , m_size(0) +{ +} + +/** @brief Queue Class destructor + * + * Destructor method + * + * + */ +template <class T, unsigned int N> +CQueue<T,N>::~CQueue() +{ +} + +/** @brief Is full method + * + * + * @return True if Queue FULL + */ +template <class T, unsigned int N> +bool CQueue<T,N>::isFull() +{ + return (m_start + 1)%N == m_end; +} + +/** @brief Is empty method + * + * + * @return True if Queue EMPTY + */ +template <class T, unsigned int N> +bool CQueue<T,N>::isEmpty() +{ + return (m_end + 1)%N == m_start; +} + +/** @brief Peek method + * + * + * @return Value on top of queue + */ +template <class T, unsigned int N> +T CQueue<T,N>::peek() +{ + return m_buffer[(m_end+1) % N]; +} + +/** @brief Pop method + * + * Method for removing the item on top of the queue and returning it + * + * + * @return Element at top of the queue + */ +template <class T, unsigned int N> +T CQueue<T,N>::pop() +{ + if(!isEmpty()) + { + m_end = (m_end+1) % N; + T l_char = m_buffer[m_end]; + m_size--; + return l_char; + } + else + { + return 0; + } +} + +/** @brief Push method + * + * Method for inserting an item on top of the queue + * + * @param f_char element to be added to the queue + * @return None + */ +template <class T, unsigned int N> +void CQueue<T,N>::push(T& f_char) +{ + if (!isFull()) + { + m_buffer[m_start] = f_char; + m_start = (m_start+1) % N; + m_size++; + } +} + +/** @brief Push method + * + * Method for inserting a sequence of items on top of the queue + * + * @param f_char pointer to element to be added to the queue + * @param f_len number of elements to be added + * @return None + */ +template <class T, unsigned int N> +inline void CQueue<T,N>::push(T *f_char, unsigned int f_len) +{ + for ( unsigned int l_idx = 0; l_idx < f_len; ++l_idx) + { + push(f_char[l_idx]); + } +} + +/** @brief Get size method + * + * Returns the size of the queue + * + * + * @return Queue size + */ +template <class T, unsigned int N> +unsigned int CQueue<T,N>::getSize() +{ + return m_size; +} + +/** @brief Empty queue method + * + * + * @return None + */ +template <class T, unsigned int N> +inline void CQueue<T,N>::empty() +{ + m_start = 0; + m_end = N-1; + m_size = 0; +}
diff -r c3e774091195 -r ceee5a608e7c include/SHARP_IR_distance/DistanceSensors/distancesensors.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/SHARP_IR_distance/DistanceSensors/distancesensors.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,59 @@ +/** + ****************************************************************************** + * @file DistanceSensors.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date 08-January-2018 + * @brief This file contains the class definition for the distance sensors read + * methods. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef DISTANCESENSORS_HPP +#define DISTANCESENSORS_HPP + +#include <mbed.h> +#include <array> +#include <string> +#include<TaskManager/taskmanager.hpp> +#include <SHARP_IR_distance\DistanceSensors\distancesensors.hpp> +using namespace std; + +/** + * @brief It is used for reading the attached distance sensors. + * + */ +template<class T> +class CDistanceSensors: public task::CTask +{ +private: + /* The sensors list */ + T m_sensor_list; + /* Index of current sensor */ + uint8_t m_index; + /* The maximum number of sensors that are used */ + const uint8_t m_N; + /* Flags for enabloing/disabling timeout */ + Timeout m_disableTimeOut; + Timeout m_enableTimeOut; + /* Message that contains the read values */ + string m_message; + +public: + /* Construnctor */ + CDistanceSensors(uint32_t f_period, + float f_baseTick, + T& f_sensor_list); + /* Destructor */ + ~CDistanceSensors(); + /* Callback for reading sensor value */ + void ReadCallback(void); + /* Run method */ + void _run(void); +}; + +#include "DistanceSensors.inl" + +/* Include guard */ +#endif
diff -r c3e774091195 -r ceee5a608e7c include/SHARP_IR_distance/DistanceSensors/distancesensors.inl --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/SHARP_IR_distance/DistanceSensors/distancesensors.inl Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,79 @@ +/** @brief Class constructor + + + @param f_period sensor reading period + @param f_baseTick base tick period, in seconds + @param f_sensor_list the list of sensor objects + */ +template<class T > +CDistanceSensors<T>::CDistanceSensors(const uint32_t f_period, + const float f_baseTick, + T& f_sensor_list) + : task::CTask(f_period) + , m_sensor_list(f_sensor_list) + , m_index(0) + , m_N(f_sensor_list.size()) + , m_disableTimeOut() + , m_enableTimeOut() + , m_message("@IRDS:") + // , m_buf("") +{ + //static_assert(f_sensor_list.size() * 0.03 < f_period*f_baseTick, + //"IR sensor reding period is too small! It must be grater than: number of sensors in \"conflict\" *0.3seconds!"); +} + +//static_assert(m_N * 0.03 < f_period*f_baseTick, +// "IR sensor reding period is too small! It must be grater than: number of sensors in \"conflict\" *0.3seconds!"); + +/** @brief Class destructor + + Destructor method + */ +template<class T > +CDistanceSensors<T>::~CDistanceSensors(){} + +/** @brief Read callback method + + Callback for reading the value of one distance sensor + + */ +template<class T > +void CDistanceSensors<T>::ReadCallback(){ + // // buffer for holding message data + // char buf [10]; + // read distance form sensor at position m_index in the sensor list + m_sensor_list[m_index]->ReadDistance(); + // disable sensor at position m_index in the sensor list + m_sensor_list[m_index]->disable(); + //sprintf(buf,"-%d-%3.2f;",m_index, m_sensor_list[m_index]->getValue());/// cu m_index + // format value so it can be appended to message + // sprintf(buf,"%3.2f;", m_sensor_list[m_index]->getValue()); // standard formatting + // // append value from sensor to message + // m_message.append(buf); + // if the last sensor is read, send message on UART and exit method + if(m_index == m_N-1){ + // m_message=m_message+";\r\n"; + // m_serial.printf(m_message.c_str()); + // m_message="@IRDS:"; + return; + } + // go to next sensor + m_index = m_index+1; + // enable next sensor + m_sensor_list[m_index]->enable(); + // disable sensor timeout, read next sensor after 0.03 seconds + m_disableTimeOut.attach(callback(this, &CDistanceSensors<T>::ReadCallback), 0.03); +} + +/** @brief Method called each f_period + * + */ +template<class T > +void CDistanceSensors<T>::_run(){ + // start from first sensor + m_index=0; + // enable first sensor + m_sensor_list[m_index]->enable(); + // read sensor after 0.03 seconds + m_disableTimeOut.attach(callback(this, &CDistanceSensors<T>::ReadCallback), 0.03); +}
diff -r c3e774091195 -r ceee5a608e7c include/SHARP_IR_distance/IR_DistanceSensorDriver/ir_distancesensordriver.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/SHARP_IR_distance/IR_DistanceSensorDriver/ir_distancesensordriver.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,85 @@ +/** + ****************************************************************************** + * @file IR_DistanceSensorDriver.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date 08-January-2018 + * @brief This file contains the class definition for the SHARP IR sensor read + * methods (driver). + ****************************************************************************** + */ + +/* Include guard */ +#ifndef IR_DISTANCESENSORDRIVER_HPP +#define IR_DISTANCESENSORDRIVER_HPP + +#include <mbed.h> +#include <math.h> + +/* Module defines */ +#define C_IR_DistanceSensorDriver_A -19057.99103709 +#define C_IR_DistanceSensorDriver_B 42562.72078701 +#define C_IR_DistanceSensorDriver_C -37447.6482552 +#define C_IR_DistanceSensorDriver_D 16482.62217927 +#define C_IR_DistanceSensorDriver_E -3758.54454044 +#define C_IR_DistanceSensorDriver_F 395.72452643 + +/* //scaling values, variant 1 +a=733.06912872; +b=-2221.23062859; +c=2502.20535578; +d=-1273.63067295; +e=269.64209866; +*/ + +/* +//scaling values, variant 2, used for obtaining more accurate values +a=-6144.18274059; +b=19775.78355498; +c=-25056.00451678; +d=15739.05258272; +e=-4986.65539646; +f=673.27540402; +*/ +/*scaling values, variant 3 (media benzii) +a=-19057.99103709; +b=42562.72078701; +c=-37447.6482552; +d=16482.62217927; +e=-3758.54454044; +f=395.72452643; +*/ + +/** + * @brief It is used for reading SHARP IR distance sensors. + * + */ +class C_IR_DistanceSensorDriver +{ +public: + /* Construnctor */ + C_IR_DistanceSensorDriver(AnalogIn ain_pin, DigitalOut en_pin); + /* Destrunctor */ + ~C_IR_DistanceSensorDriver(); + /* Method for reading distance */ + float ReadDistance(void); + /* Method for enabling sensor */ + void enable(void); + /* Method for disabling sensor */ + void disable(void); + //void setValue(float); + /* Method for obtaining read value */ + float getValue(void); + +private: + + /* the analog pin, giving the proportional voltage value */ + AnalogIn ain_pin; + /* enable pin */ + DigitalOut en_pin; + /* distance value */ + float value; +}; + +/* Include guard */ +#endif
diff -r c3e774091195 -r ceee5a608e7c include/SHARP_IR_distance/drivertask.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/SHARP_IR_distance/drivertask.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,53 @@ +#ifndef DRIVER_TASK_H +#define DRIVER_TASK_H + +#include <TaskManager.h> +#include <DistanceDriver.h> +#include <filter.h> + + + + +template<class T> +class CDriverTask: public task::CTask +{ + public: + using FilterFunc=filter::CFilterFunction<T>; + CDriverTask( uint32_t f_period + ,PinName f_pin + ,FilterFunc& f_filter + ,Serial& f_serial) + :CTask(f_period) + ,m_filter(f_filter) + ,m_distanceDriver(f_pin) + ,m_val_brut(0.0) + ,m_val_filtrat(0.0) + ,m_serial(f_serial) + ,idx(0) + { + } + + T getDistanceFiltered(){ + return m_val_filtrat; + } + + T getDistanceMeasured(){ + return m_val_brut; + } + + + + private: + virtual void _run(){ + m_val_brut=m_distanceDriver.ReadDistance(); + m_val_filtrat=m_filter(m_val_brut); + } + + FilterFunc& m_filter; + DistanceDriver m_distanceDriver; + T m_val_brut; + T m_val_filtrat; + Serial& m_serial; + int idx; +}; +#endif
diff -r c3e774091195 -r ceee5a608e7c include/SafetyStop/safetystopfunction.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/SafetyStop/safetystopfunction.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,75 @@ +/** + ****************************************************************************** + * @file SafetyStopFunction.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class declaration for the safety stop activation + * methods. + ****************************************************************************** + */ + +/* Include guard */ +#ifndef SAFETY_STOP_FUNCTION_HPP +#define SAFETY_STOP_FUNCTION_HPP + +/* Required for the use of C_IR_DistanceSensorDriver */ +#include <SHARP_IR_distance\IR_DistanceSensorDriver\ir_distancesensordriver.hpp> + + +/** + * @brief It is used for deciding whether to activate safetyu stop depending on + * distance sensors reading. + * + */ +class CSafetyStopFunction{ +public: + /* Construnctor */ + 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_senLeftBach + // , C_IR_DistanceSensorDriver& f_senRightBach + ); + + /* Destructor */ + ~CSafetyStopFunction(); + + /* Function for activating safety stop */ + bool isSafetyStopActive(float f_speed, float f_angle); + /* Serial callback method */ + static void staticSerialCallback(void* obj,char const * a, char * b); +private: + /* Serial callback actions */ + void serialCallback(char const * a, char * b); + /* The functionality active state*/ + bool m_active; + /* Current car speed */ + float m_speed; + /* Current steering angle */ + float m_angle; + /* Front sensors */ + C_IR_DistanceSensorDriver& m_senLeftFront; + C_IR_DistanceSensorDriver& m_senMidLeftFront; + C_IR_DistanceSensorDriver& m_senCenterFront; + C_IR_DistanceSensorDriver& m_senMidRightFront; + C_IR_DistanceSensorDriver& m_senRightFront; + /* Back sensors */ + // C_IR_DistanceSensorDriver& m_senLeftBach; + // C_IR_DistanceSensorDriver& m_senRightBach; + /* Serial communication member */ + /* USED FOR DEBUG */ +}; + +/* Include guard */ +#endif + + + + + + + +
diff -r c3e774091195 -r ceee5a608e7c include/SerialMonitor/serialmonitor.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/SerialMonitor/serialmonitor.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,70 @@ +/** + ****************************************************************************** + * @file SerialMonitor.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class declaration for the serial communication + * functionality. + ****************************************************************************** + */ + +/* Inclusion guard */ +#ifndef SERIAL_MONITOR_HPP +#define SERIAL_MONITOR_HPP + +/* The mbed library */ +#include <mbed.h> +#include <map> +#include <array> +/* Function objects */ +#include <functional> +#include<TaskManager/taskmanager.hpp> +#include <Queue/queue.hpp> + + +namespace serial{ + + /** + * @brief It is used for implementing serial communciation. + * + */ + class CSerialMonitor : public task::CTask + { + public: + typedef mbed::Callback<void(char const *, char *)> FCallback; + typedef std::map<string,FCallback> CSerialSubscriberMap; + + /* Constructor */ + CSerialMonitor(Serial& f_serialPort + ,CSerialSubscriberMap f_serialSubscriberMap); + private: + /* Rx callback */ + static void RxCallback(void *thisPointer); + /* Tx callback */ + static void TxCallback(void *thisPointer); + /* Rx callback actions */ + void serialRxCallback(); + /* Tx callback actions */ + void serialTxCallback(); + /* Run method */ + virtual void _run(); + + /* Reference to serial object */ + Serial& m_serialPort; + /* Rx buffer */ + CQueue<char,255> m_RxBuffer; + /* Tx buffer */ + CQueue<char,255> m_TxBuffer; + /* Data buffer */ + array<char,256> m_parseBuffer; + /* Parse iterator */ + array<char,256>::iterator m_parseIt; + /* Serial subscriber */ + CSerialSubscriberMap m_serialSubscriberMap; + }; + +}; // namespace serial + +#endif // SERIAL_MONITOR_HPP +
diff -r c3e774091195 -r ceee5a608e7c include/SystemModels/systemmodels.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/SystemModels/systemmodels.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,155 @@ +/** + ****************************************************************************** + * @file SystemModels/systemmodels.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class declarations for system model. + ****************************************************************************** + */ + +/* Inclusion guard */ +#ifndef SYSTEM_MODELS_HPP +#define SYSTEM_MODELS_HPP + +#include <Linalg/linalg.h> + +// Discrete System Models +namespace systemmodels{ + //Linear time variant models + namespace lti{ + namespace siso{ + /** + * @brief Discrete transfer function + * + * @tparam T The type of the variable + * @tparam NNum The order of the polynomial + * @tparam NDen The order of the polynomial + */ + template <class T,uint32_t NNum,uint32_t NDen> + class CDiscreteTransferFucntion{ + public: + using CDenType = linalg::CMatrix<T,NDen,1>; + using CDenModType = linalg::CMatrix<T,NDen-1,1>; + using CNumType = linalg::CMatrix<T,NNum,1>; + // using CNumModType = linalg::CMatrix<T,NNum-1,1>; + using CInputMem = linalg::CMatrix<T,1,NNum>; + using COutputMem = linalg::CMatrix<T,1,NDen-1>; + /* Constructor */ + CDiscreteTransferFucntion(); + + CDiscreteTransferFucntion(const CNumType& f_num,const CDenType& f_den); + + /* Clear memory */ + void clearMemmory(); + /* Shift memory */ + template<uint32_t N> + void shiftMemory(linalg::CMatrix<T,1,N>& f_mem); + /* Operator */ + T operator()(const T& f_input); + /* Set num */ + void setNum(const CNumType& f_num); + /* Set den */ + void setDen(const CDenType& f_den); + + const CNumType& getNum(); + const CDenModType& getDen(); + float getDenCurrent(); + /* Get output */ + T getOutput(); + + private: + + #ifdef SYSTEMMODEL_TEST_HPP + FRIEND_TEST(CDiscreteTransferFucntionTest,Reset); + #endif + + /* nominator type */ + CNumType m_num; + // CDenType m_den; + /* denominator type */ + CDenModType m_den; + T m_denCoef; + /* input memory */ + CInputMem m_memInput; + /* output memory */ + COutputMem m_memOutput; + }; + } + + }; + // Nonlinea time invariant models + namespace nlti{ + //Multi-input and multi-output + namespace mimo{ + // Discrete time system model of a system nonlinear time invariant with multi-input and multi-output + // T -variable type + // NA -number of inputs + // NB -number of states + // NC -number of outputs + template <class T,uint32_t NA, uint32_t NB,uint32_t NC> + class CDiscreteTimeSystemModel{ + public: + using CStatesType = linalg::CMatrix<T,NB,1>; + using CInputType = linalg::CMatrix<T,NA,1>; + using COutputType = linalg::CMatrix<T,NC,1>; + /* Constructor */ + CDiscreteTimeSystemModel(const double f_dt);//:m_states(),m_dt(f_dt){} + /* Constructor */ + CDiscreteTimeSystemModel(const CStatesType& f_states + ,const double f_dt); + //State transition model + //Calculate the system state depending on input, after calculation the relevant class members need to be syncronized. + virtual CStatesType update(const CInputType& f_input)=0; + //State observation model + //Calculate the system output depending on input, after calculation the relevant class members need to be syncronized. + virtual COutputType calculateOutput(const CInputType& f_input)=0; + // GETTERS + // The method returns the current system states + CStatesType getStates();//{return m_states;} + // The method returns the current system output + COutputType getOutput(){return m_outputs;} + float getTimeStep(){return m_dt;} + // SETTERS + void setStates(const CStatesType& f_states){ + m_states=f_states; + } + protected: + // States + CStatesType m_states; + // Output + COutputType m_outputs; + // Time step + const double m_dt; + private: + }; + + // the state transition and observation matrices defined by Jacobians + // T -variable type + // NA -number of inputs + // NB -number of states + // NC -number of outputs + template <class T,uint32_t NA, uint32_t NB,uint32_t NC> + class CJacobianMatrices{ + public: + using CStatesType = linalg::CMatrix<T,NB,1>; + using CInputType = linalg::CMatrix<T,NA,1>; + // using COutputType = linalg::CMatrix<T,NC,1>; + using CJMTransitionType = linalg::CMatrix<T,NB,NB>; + using CJMObservationType = linalg::CMatrix<T,NC,NB>; + + //The method calculates and returns the states transition matrix + virtual CJMTransitionType getJMTransition(const CStatesType& f_states + ,const CInputType& f_inputs)=0; + // The method calculates and returns the states observation matrix + virtual CJMObservationType getJMObservation(const CStatesType& f_states + ,const CInputType& f_input)=0; + private: + }; + }; + }; +}; + +#include "systemmodels.inl" + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/SystemModels/systemmodels.inl --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/SystemModels/systemmodels.inl Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,171 @@ +/******************************************************************************/ +/** \brief CDiscreteTransferFucntion class constructor + * + * Constructor method + */ +template <class T,uint32_t NNum,uint32_t NDen> +systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::CDiscreteTransferFucntion() + :m_num(CNumType::zeros()) + ,m_den(CDenModType::zeros()) + ,m_denCoef(1) + ,m_memInput() + ,m_memOutput() +{ +} + + +template <class T,uint32_t NNum,uint32_t NDen> +systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::CDiscreteTransferFucntion(const CNumType& f_num,const CDenType& f_den) + :m_num(CNumType::zeros()) + ,m_den(CDenModType::zeros()) + ,m_denCoef(1) + ,m_memInput() + ,m_memOutput() +{ + this->setNum(f_num); + this->setDen(f_den); +} + + +/** \brief Clear memory + * + * + * + */ +template <class T,uint32_t NNum,uint32_t NDen> +void systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::clearMemmory() +{ + m_memInput=CInputMem::zeros(); + m_memOutput=COutputMem::zeros(); +} + +/** \brief Clear memory + * + * @param f_input reference to input + * @return l_output + */ +template <class T,uint32_t NNum,uint32_t NDen> +T systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::operator()(const T& f_input) +{ + shiftMemory<NNum>(m_memInput); + m_memInput[0][0]=f_input; + T l_output=((m_memInput*m_num-m_memOutput*m_den)[0][0])/m_denCoef; + shiftMemory<NDen-1>(m_memOutput); + m_memOutput[0][0]=l_output; + return l_output; +} + +/** \brief Set num + * + * @param f_num reference to nominator + * + */ +template <class T,uint32_t NNum,uint32_t NDen> +void systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::setNum(const CNumType& f_num) +{ + for(uint32_t i=0;i<NNum;++i) + { + m_num[i][0]=f_num[i][0]; + } +} + +/** \brief Set den + * + * @param f_den reference to denominator + * + */ +template <class T,uint32_t NNum,uint32_t NDen> +void systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::setDen(const CDenType& f_den) +{ + for(uint32_t i=1;i<NDen;++i) + { + m_den[i-1][0]=f_den[i][0]; + } + m_denCoef = f_den[0][0]; +} + +/** \brief Get output + * + * + * @return Output memory + */ +template <class T,uint32_t NNum,uint32_t NDen> +T systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::getOutput() +{ + return m_memOutput[0][0]; +} + +/** \brief Shift memory + * + * @param f_mem reference to memory matrix + * @return none + */ +template <class T,uint32_t NNum,uint32_t NDen> +template<uint32_t N> +void systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::shiftMemory(linalg::CMatrix<T,1,N>& f_mem) +{ + for(uint32_t i=N-1;i>0;--i) + { + f_mem[0][i]=f_mem[0][i-1]; + } +} + +/******************************************************************************/ +/** \brief CDiscreteTimeSystemModel class constructor + * + * Constructor method + * + * @param f_dt Time step + */ +template <class T,uint32_t NA, uint32_t NB,uint32_t NC> +systemmodels::nlti::mimo::CDiscreteTimeSystemModel<T,NA,NB,NC>::CDiscreteTimeSystemModel(const double f_dt) + : m_states() + , m_dt(f_dt) +{ +} + +/** \brief CDiscreteTimeSystemModel class constructor + * + * Constructor method + * + * @param f_states States + * @param f_dt Time step + */ +template <class T,uint32_t NA, uint32_t NB,uint32_t NC> +systemmodels::nlti::mimo::CDiscreteTimeSystemModel<T,NA,NB,NC>::CDiscreteTimeSystemModel( + const CStatesType& f_states + ,const double f_dt) + : m_states(f_states) + , m_dt(f_dt) +{ +} + + + +/** \brief Getter numitor + * + * @return const reference to numitor + */ +template <class T,uint32_t NNum,uint32_t NDen> +const typename systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::CNumType& systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::getNum(){ + return m_num; +} + +/** \brief Getter denomitor + * Return the denomitor without the first coefficeint. + * + * @return const reference to denomitor + */ +template <class T,uint32_t NNum,uint32_t NDen> +const typename systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::CDenModType& systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::getDen(){ + return m_den; +} + +/** \brief Getter denomitor first coefficeint + * + * @return value of the denomitor first coefficeint + */ +template <class T,uint32_t NNum,uint32_t NDen> +float systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::getDenCurrent(){ + return m_denCoef; +}
diff -r c3e774091195 -r ceee5a608e7c include/TaskManager/taskmanager.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/TaskManager/taskmanager.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,97 @@ +/** + ****************************************************************************** + * @file TaskManager.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class declaration for task manager. + ****************************************************************************** + */ + +/* Inclusion guard */ +#ifndef TASK_MANAGER_HPP +#define TASK_MANAGER_HPP + +#include <mbed.h> + +namespace task{ + /** + * @brief It is used for implementing the task functionality. + * + */ + class CTask + { + public: + /* Constructor */ + CTask(uint32_t f_period); + /* Destructor */ + virtual ~CTask(); + /* Run method */ + virtual void run(); + /* Timer callback */ + void timerCallback() + { + m_ticks++; + if (m_ticks >= m_period) + { + m_ticks = 0; + Trigger(); + } + } + /* Trigger */ + void Trigger() + { + m_triggered = true; + } + protected: + /* _run methof */ + virtual void _run() = 0; + /* period */ + const uint32_t m_period; + /* ticks */ + uint32_t m_ticks; + /* trigger flag */ + bool m_triggered; + }; + + /** + * @brief It is used for implementing the task manager functionality. + * + */ + class CTaskManager + { + public: + /* Constructor */ + CTaskManager(CTask** f_taskList, uint32_t f_taskCount, float f_baseFreq); + /* Destructor */ + virtual ~CTaskManager(); + /* Main callback */ + inline void mainCallback() + { + for(uint32_t i = 0; i < m_taskCount; i++) + { + m_taskList[i]->run(); + } + } + /* Timer callback */ + inline void timerCallback() + { + for(uint32_t i = 0; i < m_taskCount; i++) + { + m_taskList[i]->timerCallback(); + } + } + private: + /* Callback */ + static void callback(void *thisPointer); + /* task list */ + CTask** m_taskList; + /* task count */ + uint32_t m_taskCount; + /* ticker */ /* A Ticker is used to call a function at a recurring interval */ + Ticker m_ticker; + }; + +}; // namespace task + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Timer/timer.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Timer/timer.hpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,58 @@ +/** + ****************************************************************************** + * @file Timer.hpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class declaration for the timer functionality. + ****************************************************************************** + */ + +/* Inclusion guard */ +#ifndef TIMER_HPP +#define TIMER_HPP + +#include <mbed.h> + +/** + * @brief It is used for implementing timers. + * + * @tparam N + * @tparam 1 + */ +template <unsigned int N, unsigned int D = 1> +class CTimer +{ +public: + /* Constructor */ + CTimer(); + /* Destructor */ + virtual ~CTimer(); + /* Start timer */ + inline void start(); + /* Stop timer */ + inline void stop() ; + /* Get timer */ + inline uint32_t get(); +private: + /* Timer callback */ + static void callback(void *thisPointer); + /* Increase milliseconds value */ + void millisTicker(); + /* Ticker member */ + Ticker m_ticker; + /* Milliseconds value */ + volatile uint32_t m_millisValue; + /* Flag indicating timer started */ + bool m_started; +}; + +typedef CTimer<1,1000> CTimer_ms; +typedef CTimer<1,1000000> CTimer_us; +typedef CTimer<1,10000> CTimer_100us; +typedef CTimer<60> CTimer_min; + +/* Inline implementation */ +#include "timer.inl" + +#endif
diff -r c3e774091195 -r ceee5a608e7c include/Timer/timer.inl --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/Timer/timer.inl Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,95 @@ +/** + ****************************************************************************** + * @file Timer.inl + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class definition for the timer functionality. + * Inline implementation. + ****************************************************************************** + */ + +/** @brief CTimer class constructor + * + * Constructor method + * + * + */ +template <unsigned int N, unsigned int D> +CTimer<N,D>::CTimer() + : m_ticker() + , m_millisValue(0) + , m_started(0) +{ +} + +/** @brief CTimer class destructor + * + * Destructor method + * + * + */ +template <unsigned int N, unsigned int D> +CTimer<N,D>::~CTimer() +{ + if (m_started) + stop (); +} + +/** @brief Start timer + * + * + * + */ +template <unsigned int N, unsigned int D> +void CTimer<N,D>::start () +{ + m_started = 1; + m_ticker.attach (mbed::callback(&CTimer::callback, this), 1.f*N/D); +} + +/** @brief Stop timer + * + * + * + */ +template <unsigned int N, unsigned int D> +void CTimer<N,D>::stop() +{ + m_started = 0; + m_ticker.detach (); +} + +/** @brief Get timer + * + * + * @return Milliseconds value + */ +template <unsigned int N, unsigned int D> +uint32_t CTimer<N,D>::get() +{ + return m_millisValue; +} + +/** @brief Timer callback + * + * @param thisPointer The object pointer. + * + */ +template <unsigned int N, unsigned int D> +void CTimer<N,D>::callback(void *thisPointer) +{ + CTimer<N,D>* self = static_cast<CTimer<N,D>*>(thisPointer); + self->millisTicker(); +} + +/** @brief Increase milliseconds value + * + * + * + */ +template <unsigned int N, unsigned int D> +void CTimer<N,D>::millisTicker() +{ + m_millisValue ++; +}
diff -r c3e774091195 -r ceee5a608e7c src/BezierCurve/beziercurve.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/BezierCurve/beziercurve.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,12 @@ +/** + ****************************************************************************** + * @file BezierCurve.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class implementation for the Bezier Curve + * methods. + ****************************************************************************** + */ + +#include <BezierCurve/BezierCurve.hpp>
diff -r c3e774091195 -r ceee5a608e7c src/BezierCurve/polynomialfunction.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/BezierCurve/polynomialfunction.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,1 @@ +#include <BezierCurve/PolynomialFunction.hpp>
diff -r c3e774091195 -r ceee5a608e7c src/BezierMotionPlanner/beziermotionplanner.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/BezierMotionPlanner/beziermotionplanner.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,168 @@ +/** + ****************************************************************************** + * @file BezierMotionPlanner.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class definition for the Bezier Motion + * Planner methods. + ****************************************************************************** + */ + +#include <iostream> +#include <BezierMotionPlanner/bezierMotionplanner.hpp> + +namespace planner{ + + /** + * @brief Construct a new CBezierMotionPlanner::CBezierMotionPlanner object + * + */ + CBezierMotionPlanner::CBezierMotionPlanner() + { + this->isInitialized=false; + } + + /** + * @brief Construct a new CBezierMotionPlanner::CBezierMotionPlanner object + * + * @param isForward Forward movement flag + * @param a Point A + * @param b Point B + * @param c Point C + * @param d Point D + * @param motion_duration_i The motion duration in second. + * @param timestep_i The base period of the planner. (Sample time) + */ + CBezierMotionPlanner::CBezierMotionPlanner(bool isForward, + std::complex<float> a, + std::complex<float> b, + std::complex<float> c, + std::complex<float> d, + float motion_duration_i, + float timestep_i) + : isForward(isForward) + , bezierCurve(a,b,c,d) + , motion_duration(motion_duration_i) + , time_step(timestep_i) + { + this->bezierValueInput_step=1.0/(int)(this->motion_duration/this->time_step); + this->next_bezierValueInput=0.0; + this->isInitialized=true; + } + + + /** + * @brief Set motion planner parameters + * + * @param isForward Forward movement flag + * @param a Point A + * @param b Point B + * @param c Point C + * @param d Point D + * @param motion_duration_i The motion duration in second. + * @param timestep_i The base period of the planner. (Sample time) + */ + void CBezierMotionPlanner::setMotionPlannerParameters(bool isForward, + std::complex<float> a, + std::complex<float> b, + std::complex<float> c, + std::complex<float> d, + float motion_duration_i, + float timestep_i) + { + this->isForward=isForward; + this->motion_duration=motion_duration_i; + this->time_step=timestep_i; + this->bezierCurve.setBezierCurve(a,b,c,d); + this->bezierValueInput_step=1.0/(int)(this->motion_duration/this->time_step); + this->next_bezierValueInput=0.0; + this->isInitialized=true; + } + + /** + * @brief Destroy the CBezierMotionPlanner::CBezierMotionPlanner object + * + */ + CBezierMotionPlanner::~CBezierMotionPlanner() + { + } + + + /** + * @brief Get the Bezier curve. + * + */ + math::BezierCurve<float> CBezierMotionPlanner::getBezierCurve() + { + return this->bezierCurve; + } + + /** + * @brief Get the next control parameters. It calculates the velocity and angle, and increase input value. + * + * @return It returns the next forward velocity and direction angle. + */ + std::pair<float,float> CBezierMotionPlanner::getNextVelocity(){ + std::pair<float,float> commands=this->getVelocity(next_bezierValueInput); + this->next_bezierValueInput+=this->bezierValueInput_step; + return commands; + } + + /** + * @brief Get the forward velocity and steering angle, base on the given input value. + * + * @param input_value The input value have to belong to interval [0,1]. + * @return It returns a pair of number, where the fist variable and the second contains the forward velocity and the steering angular, respectively. + */ + std::pair<float,float> CBezierMotionPlanner::getVelocity(float input_value) + { + if(!this->isInitialized) return std::pair<float,float>(0,0); + + std::complex<float> dS=this->bezierCurve.get_FO_DerivateValue(input_value); + + float dl_absolute=sqrt((dS*std::conj(dS)).real());//[0,1]//Length of the vector + float dl_real=dl_absolute/this->motion_duration; + + std::complex<float> ddS=this->bezierCurve.get_SO_DerivateValue(input_value); + + std::complex<float> correctorValue(0,-2); + std::complex<float> temp1=(dS*std::conj(ddS)-std::conj(dS)*ddS)/correctorValue; + float num=temp1.real(); + if(dl_absolute==0) + { + return std::pair<float,float>(dl_real,0); + } + + float k=num/pow(dl_absolute,3); + + float angle_rad=atan(k*WHEELBASE); + float angle_deg=(180.f/M_PI)*angle_rad; + + std::pair<float,float> commands(dl_real,angle_deg); + return commands; + } + + /** + * @brief Get the state of the planner. + * + * @return true The planner has valid value. + * @return false The planner finished the last given curve, it cannnot get correct parameters. + */ + bool CBezierMotionPlanner::hasValidValue() + { + return (next_bezierValueInput>=0 && next_bezierValueInput<=1); + } + + /** @brief Get the direction of the motion + * + * + * @return true The motion direction is forward. + * @return false The motion direction is backward. + */ + bool CBezierMotionPlanner::getForward() + { + return this->isForward; + } + +}; // namespace planner
diff -r c3e774091195 -r ceee5a608e7c src/CommandInterpreter/commandinterpreter.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/CommandInterpreter/commandinterpreter.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,180 @@ +/** + ****************************************************************************** + * @file CommandInterpreter.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class definition for the command interpreter + * functionality. + ****************************************************************************** + */ + +#include <CommandInterpreter/commandinterpreter.hpp> + +/** \brief CCommandInterpreter Class constructor + * + * Constructor method + * + * \param[in] f_car reference to MOVE object + */ +CCommandInterpreter::CCommandInterpreter(Move& f_car) + : m_buffer() + , m_car(f_car) + , m_commandValue() +{ +} + +/** \brief Interpret character + * + * \param[in] f_c character value + */ +void CCommandInterpreter::interpretChar(unsigned char f_c) +{ + if ((f_c == 'A') || (f_c == 'S')) + { +// led1 = 1; + reset(); + m_buffer.push(f_c); + m_commandID = f_c; +// char s[100]; +// unsigned int l = sprintf(s,"storing command starting with %c, %c stored",f_c, m_buffer.peek()); +// g_rpiWriteBuffer.push(s,l); + } + else + { + m_buffer.push(f_c); +// char s[100]; +// unsigned int l = sprintf(s,"peeked %c, command_ID is %c, size is %d",m_buffer.peek(),m_commandID,m_buffer.getSize()); +// g_rpiWriteBuffer.push(s,l); + if (m_buffer.getSize() == 5) + { +// char s[100]; +// unsigned int l = sprintf(s,"interpreting command starting with %c",m_commandID); +// g_rpiWriteBuffer.push(s,l); + intepretCommand(); + reset(); + } + } +} + +/** \brief Execute command + * + */ +void CCommandInterpreter::executeCommand() +{ + m_car.Steer(m_angleValue); + m_car.Speed(m_speedValue); +// reset(); +} + +/** \brief Reset + * + * + * + */ +void CCommandInterpreter::reset() +{ + m_commandID = 0; + m_buffer.empty(); +} + +/** \brief Interpret command + * + * + * + */ +void CCommandInterpreter::intepretCommand() +{ + unsigned char test_char = m_buffer.pop(); + if (test_char != m_commandID) + { + reset(); +// led1 = 0; +// char s[100]; +// unsigned int l = sprintf(s,"interpretation failed at ID. Expected: %c, received: %c",m_commandID,test_char); +// g_rpiWriteBuffer.push(s,l); + } + else + { + float l_sign = 0; + if (m_buffer.peek() == '0') + { + l_sign = 1; + } + else if (m_buffer.peek() == '1') + { + l_sign = -1; + } + else + { + reset(); +// led1 = 0; +// char s[100]; +// unsigned int l = sprintf(s,"interpretation failed a sign"); +// g_rpiWriteBuffer.push(s,l); + return; + } + m_buffer.pop(); + if ((m_buffer.peek() < '0') || (m_buffer.peek() > '9')) + { + reset(); +// led1 = 0; +// char s[100]; +// unsigned int l = sprintf(s,"interpretation failed at val 1"); +// g_rpiWriteBuffer.push(s,l); + return; + } + else + { + m_commandValue = m_buffer.pop() - '0'; + } + if ((m_buffer.peek() < '0') || (m_buffer.peek() > '9')) + { + reset(); +// led1 = 0; +// char s[100]; +// unsigned int l = sprintf(s,"interpretation failed at val 2, value is %c",m_buffer.peek()); +// g_rpiWriteBuffer.push(s,l); + return; + } + else + { + m_commandValue *= 10; + m_commandValue += m_buffer.pop() - '0'; + m_commandValue *= l_sign; + } + if (m_buffer.pop() != ';') + { + reset(); +// led1 = 0; +// char s[100]; +// unsigned int l = sprintf(s,"interpretation failed at terminator"); +// g_rpiWriteBuffer.push(s,l); + return; + } + else + { + if(m_commandID == 'A') + { + m_angleValue = m_commandValue; +// char s[100]; +// unsigned int l = sprintf(s,"set angle to %f\n", m_angleValue); +// g_rpiWriteBuffer.push(s,l); +// led1 = 1; + reset(); + return; + } + if(m_commandID == 'S') + { + m_speedValue = m_commandValue; +// char s[100]; +// unsigned int l = sprintf(s,"set speed to %f\n", m_speedValue); +// g_rpiWriteBuffer.push(s,l); +// led1 = 1; + reset(); + return; + } + reset(); + } + } +}
diff -r c3e774091195 -r ceee5a608e7c src/Controllers/controller.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Controllers/controller.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,153 @@ +/** + ****************************************************************************** + * @file Controller.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class implementation for the controller + * functionality. + ****************************************************************************** + */ + +#include <Controllers/controller.hpp> + +namespace controllers{ + /** + * @brief Construct a new CControllerSiso::CControllerSiso object + * + * @param f_encoder Reference to the encoder getter interface. + * @param f_pid Reference to the controller interface. + * @param f_converter [Optional] Pointer to the converter interface. + */ + CControllerSiso::CControllerSiso(encoders::IEncoderGetter& f_encoder + ,ControllerType<double>& f_pid + ,controllers::IConverter* f_converter) + :m_encoder(f_encoder) + ,m_pid(f_pid) + ,m_converter(f_converter) + ,m_control_sup(0.5) + ,m_control_inf(-0.5) + ,m_ref_abs_inf(10.0) + ,m_mes_abs_inf(30.0) + ,m_mes_abs_sup(300.0) + { + } + + /** + * @brief Set the reference signal value. + * + * @param f_RefRps The value of the reference signal + */ + void CControllerSiso::setRef(double f_RefRps) + { + m_RefRps=f_RefRps; + } + + /** \brief Get the value of reference signal. + * + */ + double CControllerSiso::getRef() + { + return m_RefRps; + } + + /** @brief Get the value of the control signal calculated last time. + * + */ + double CControllerSiso::get() + { + return m_u; + } + + /** @brief Get the value of the error between the measured and reference signal. + * + */ + double CControllerSiso::getError() + { + return m_error; + } + + /** @brief Clear the memory of the controller. + * + */ + void CControllerSiso::clear() + { + m_pid.clear(); + } + + + /** + * @brief It calculates the next value of the control signal, by utilizing the given interfaces. + * + * @return true control works fine + * @return false appeared an error + */ + bool CControllerSiso::control() + { + // Mesurment speed value + float l_MesRps = m_encoder.getSpeedRps(); + bool l_isAbs = m_encoder.isAbs(); + float l_ref; + + // Check the measured value and the superior limit for avoid over control state. + // In this case deactivete the controller. + if(std::abs(l_MesRps) > m_mes_abs_sup){ + m_RefRps = 0.0; + m_u = 0.0; + error("@PIDA:CControllerSiso: To high speed!;;\r\n"); + return false; + } + // Check the inferior limits of reference signal and measured signal for standing state. + // Inactivate the controller + if(std::abs(m_RefRps) < m_ref_abs_inf && std::abs(l_MesRps) < m_mes_abs_inf ){ + m_u = 0.0; + m_error = 0.0; + return true; + } + + // Check measured value is oriantated or absolute + if ( l_isAbs ){ + l_ref = std::abs(m_RefRps); + } else{ + l_ref = m_RefRps; + } + float l_error=l_ref-l_MesRps; + float l_v_control = m_pid.calculateControl(l_error); + float l_pwm_control = converter(l_v_control); + + m_u=l_pwm_control; + m_error=l_error; + + // Check measured value is oriantated or absolute. + if(m_RefRps<0 && l_isAbs) + { + m_u=m_u*-1.0; + } + return true; + } + + /** @brief + * + * Apply the converter interface to change the measurment unit. + * + * @param f_u Input control signal + * @return Converted control signal + */ + double CControllerSiso::converter(double f_u) + { + double l_pwm=f_u; + // Convert the control signal from V to PWM + if(m_converter!=NULL){ + l_pwm = (*m_converter)(f_u); + } + + // Check the pwm control signal and limits + if( l_pwm < m_control_inf ){ + l_pwm = m_control_inf; + } else if( l_pwm > m_control_sup ){ + l_pwm = m_control_sup; + } + return l_pwm; + } + +}; // namespace controllers
diff -r c3e774091195 -r ceee5a608e7c src/Controllers/converters.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Controllers/converters.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,2 @@ +#include <Controllers\converters.hpp> +
diff -r c3e774091195 -r ceee5a608e7c src/Controllers/pidcontroller.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Controllers/pidcontroller.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,13 @@ +/** + ****************************************************************************** + * @file PidController.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the classes implementation for the PID controller + * functionality. Because template classes are used, the code was + * placed in a corresponding .inl file + ****************************************************************************** + */ + +#include <Controllers/pidcontroller.hpp>
diff -r c3e774091195 -r ceee5a608e7c src/Drivers/servo.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Drivers/servo.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,63 @@ +/** + ****************************************************************************** + * @file SERVO.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class definition for the steering SERVO + * functionality. + ****************************************************************************** + */ + +#include <Drivers/servo.hpp> + + +namespace drivers{ + /** @brief SERVO class constructor + * + * Constructor method + * + * @param _pwm pin connected to servo input + */ + SERVO::SERVO(PinName _pwm) + :pwm(_pwm) + { + pwm.period_ms(20); + // pwm.write(0.075); + pwm.write(0.07525); + }; + + /** @brief SERVO class destructor + * + * Destructor method + * + * + */ + SERVO::~SERVO() + { + }; + + /** @brief Set angle method + * + * @param angle angle + */ + void SERVO::SetAngle(float angle) + { + pwm.write(conversion(angle)); + }; + + /** @brief Conversion method + * + * @param angle angle + * \return duty cycle + */ + float SERVO::conversion(float angle) + { + //rc-servo-4519 + //return (0.0010321101 * angle + 0.0725); + //fs-5106B + // return (0.035 +0.0272+ (angle + 25.f) / 1953.135); + //RS2 MG/BB + return (0.0009505 * angle + 0.07525); + }; +}; // namespace drivers
diff -r c3e774091195 -r ceee5a608e7c src/Drivers/vnh.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Drivers/vnh.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,135 @@ +/** + ****************************************************************************** + * @file VNH.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class defoinition for the VNH Bridge Driver + * functionality. + ****************************************************************************** + */ + +#include <Drivers/vnh.hpp> + + +namespace drivers{ + + + + /** @brief VNH class constructor + * + * Constructor method + * + * @param _pwm PWM pin + * @param _ina pin A + * @param _inb pin B + * @param _current current analog pin + */ + VNH::VNH(PinName _pwm, PinName _ina, PinName _inb, PinName _current) + :pwm(_pwm) + ,ina(_ina) + ,inb(_inb) + ,current(_current) + { + pwm.period_us(200); + increment = 0.01; + current_speed = 0; + } + + /** @brief VNH class destructor + * + * Destructor method + * + * + */ + VNH::~VNH() + { + } + + /** @brief Start method + * + * + * + */ + void VNH::Start(void) + { + ina = !inb; + pwm.write(0); + } + + /** @brief Stop method + * + * + * + */ + void VNH::Stop(void) + { + ina = inb; + } + + /** @brief Run method + * + * @param speed speed value + * + */ + void VNH::Run(float speed) + { + Go(speed); + } + + /** @brief Get current method + * + * + * \return Current value + */ + float VNH::GetCurrent(void) + { + return (current.read()) * 5 / 0.14; + } + + /** @brief Go method + * + * @param speed speed value + * + */ + void VNH::Go(float speed) + { + //pwm.write(abs(speed)); + if (speed < 0) + { + ina = 0; + inb = 1; + } + else + { + ina = 1; + inb = 0; + } + pwm =std::abs(speed); + } + + /** @brief Inverse method + * + * @param f_speed speed value + * + */ + void VNH::Inverse(float f_speed) + { + ina=!ina; + inb=!inb; + pwm =std::abs(f_speed); + } + + /** @brief Brake method + * + * + * + */ + void VNH::Brake() + { + ina.write(0); + inb.write(0); + pwm.write(100.0); + } + +}; // namespace drivers
diff -r c3e774091195 -r ceee5a608e7c src/Encoders/encoder.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Encoders/encoder.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,204 @@ +/** + ****************************************************************************** + * @file Encoder.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class implementations for the encoder + * functionality. + ****************************************************************************** + */ + +#include <Encoders/encoder.hpp> + +/** \brief Constructor for the CCounter class + * + * Constructor method + * + * @param f_pin digital pin connected to encoder output + */ +CCounter::CCounter(PinName f_pin) + :m_interrupt(f_pin) + ,m_count(0) +{ + m_interrupt.rise(mbed::callback(CCounter::staticIncrement,this)); + m_interrupt.fall(mbed::callback(CCounter::staticIncrement,this)); +} + +/** \brief Counter reset method + * + * + * + */ +void CCounter::reset(){ + m_count=0; +} + +/** \brief Counter increment method + * + * + * + */ +void CCounter::increment(){ + m_count++; +} + +/** \brief Get counts method + * + * + * @return counts + */ +int32_t CCounter::getCount(){ + return m_count; +} + +/** \brief Get counts method + * + * Method attaches the increment method to an object. + * + * @param obj object + * + */ +void CCounter::staticIncrement(void* obj){ + CCounter* self=static_cast<CCounter*>(obj); + self->increment(); +} + +/** \brief Constructor for the CEncoder class + * + * Constructor method + * + * @param f_period period value + * @param f_period_sec period value in seconds + * @param f_cpr counts per revolution (rise and fall edges) + * @param f_pinName digital pin connected to encoder output + */ +CEncoder::CEncoder(uint32_t f_period + ,float f_period_sec + ,uint32_t f_cpr + ,PinName f_pinName) + :task::CTask(f_period) + ,m_counter(f_pinName) + ,m_period_sec(f_period_sec) + ,m_cpr(f_cpr) + ,m_rps(0) +{ +} + +/** \brief Get rps method + * + * Returns the value of the rotations per second value + * + * + * @return rps + */ +float CEncoder::getSpeedRps() +{ + return m_rps; +} + +/** \brief Get count + * + * Returns the value of the count + * + * + * @return rps + */ +int16_t CEncoder::getCount() +{ + return m_rps*m_period_sec*m_cpr; +} + + + +/** \brief Run method + * + * Method executed at predefined time intervals. + * Method called each f_period + * + * + */ +void CEncoder::_run() +{ + float l_count=m_counter.getCount(); + m_counter.reset(); + m_rps=l_count/m_period_sec/m_cpr; +} + +/** \brief Constructor for the CMagEncoderTime class + * + * Constructor method + * + * @param f_pin digital pin connected to encoder output + */ +CMagEncoderTime::CMagEncoderTime(PinName f_pin) + :m_interrupt(f_pin) +{ + m_interrupt.rise(mbed::callback(CMagEncoderTime::staticRise,this)); + // m_interrupt.fall(mbed::callback(CMagEncoderTime::staticFall,this)); + m_Timer.start(); +} + +/** \brief Callback method for rising edge interrupt + * + * + */ +void CMagEncoderTime::riseCallback() +{ + m_Timer.stop(); + m_lowTime=m_Timer.read(); + m_Timer.reset(); + m_Timer.start(); +} + +/** \brief Callback method for rising edge interrupt + * + * + */ +void CMagEncoderTime::fallCallback() +{ + m_Timer.stop(); + m_highTime=m_Timer.read(); + m_Timer.reset(); + m_Timer.start(); +} + +/** \brief Callback method for rising edge interrupt attached to pbject + * + * @param obj object + */ +void CMagEncoderTime::staticRise(void* obj) +{ + CMagEncoderTime* self=static_cast<CMagEncoderTime*>(obj); + self->riseCallback(); +} + +/** \brief Callback method for rising edge interrupt attached to pbject + * + * @param obj object + * @return None + */ +void CMagEncoderTime::staticFall(void* obj) +{ + CMagEncoderTime* self=static_cast<CMagEncoderTime*>(obj); + self->fallCallback(); +} + +/** \brief Callback method for rising edge interrupt + * + * + * @return High period value + */ +float CMagEncoderTime::getHighTime() +{ + return m_highTime; +} + +/** \brief Callback method for rising edge interrupt + * + * @return Low period value + */ +float CMagEncoderTime::getLowTime() +{ + return m_lowTime; +}
diff -r c3e774091195 -r ceee5a608e7c src/Encoders/encoderFilter.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Encoders/encoderFilter.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,66 @@ +/** + ****************************************************************************** + * @file EncoderFilter.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class implementation for the encoder filter + * functionality. + ****************************************************************************** + */ + +#include <Encoders/encoderFilter.hpp> + +/** \brief Constructor for the CEncoderFilter class + * + * Constructor method + * + * \param[in] f_period period value + * \param[in] f_period_sec period value in seconds + * \param[in] f_cpr counts per revolution (rise and fall edges) + * \param[in] f_pinName digital pin connected to encoder output + * \param[in] f_filter reference to filter object + */ +CEncoderFilter::CEncoderFilter(uint32_t f_period + ,float f_period_sec + ,uint32_t f_cpr + ,PinName f_pinName + ,CFilterFunction<float>& f_filter) + :CEncoder(f_period,f_period_sec,f_cpr,f_pinName) + ,m_filter(f_filter) +{ +} + +/** \brief Get filtered rps value + * + * Method for getting filtererd rotations per second value + * + * \return filtered rps value + */ +float CEncoderFilter::getSpeedRps(){ + return m_filteredRps; +} + +/** + * @brief Getter filtered count value + * + * @return int16_t + */ +int16_t CEncoderFilter::getCount(){ + return m_filteredRps*m_period_sec*m_cpr; +} + +/** \brief Run method + * + * Method executed at predefined time intervals. + * Method called each f_period + * + */ +void CEncoderFilter::_run() +{ + float l_count=m_counter.getCount(); + m_counter.reset(); + m_rps=static_cast<float>(l_count)/m_period_sec/m_cpr; + m_filteredRps=m_filter(m_rps); +} +
diff -r c3e774091195 -r ceee5a608e7c src/Encoders/quadratureencoder.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Encoders/quadratureencoder.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,111 @@ +/** + * @file quadratureencoder.cpp + * @author RBRO/PJ-IU + * @brief + * @version 0.1 + * @date 2018-10-23 + * + * @copyright Copyright (c) 2018 + * + */ +#include <Encoders/Quadratureencoder.hpp> + + +namespace encoders{ + + + +CQuadratureEncoder_TIM4 *CQuadratureEncoder_TIM4::m_instance = 0; +CQuadratureEncoder_TIM4::CQuadratureEncoder_TIM4_Destroyer CQuadratureEncoder_TIM4::m_destroyer; + + +/** + * @brief Setter function. + * + * @param s singleton object address + */ +void CQuadratureEncoder_TIM4::CQuadratureEncoder_TIM4_Destroyer::SetSingleton(CQuadratureEncoder_TIM4* s){ + m_singleton = s; +} + +/** + * @brief Destroy the cQuadratureencoder tim4::cQuadratureencoder tim4 destroyer::cQuadratureencoder tim4 destroyer object + * + */ +CQuadratureEncoder_TIM4::CQuadratureEncoder_TIM4_Destroyer::~CQuadratureEncoder_TIM4_Destroyer(){ + delete m_singleton; +} + + +/** + * @brief Constructor function. It verifies the existance of the singleton object and creates it. + * + * @return The address of the singleton object + */ +CQuadratureEncoder_TIM4* CQuadratureEncoder_TIM4::Instance(){ + if(!CQuadratureEncoder_TIM4::m_instance){ + CQuadratureEncoder_TIM4::m_instance = new CQuadratureEncoder_TIM4; + m_instance->initialize(); + CQuadratureEncoder_TIM4::m_destroyer.SetSingleton(m_instance); + } + return CQuadratureEncoder_TIM4::m_instance; +} + + +/** + * @brief Initialize the parameter of the object. + * + */ +void CQuadratureEncoder_TIM4::initialize(){ + //PB6 PB7 aka D10 MORPHO_PB7 + // Enable clock for GPIOA + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; + + //stm32f4xx.h + GPIOB->MODER |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1; //PB6 & PB7 as Alternate Function /*!< GPIO port mode register, Address offset: 0x00 */ + GPIOB->OTYPER |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7; //PB6 & PB7 as Inputs /*!< GPIO port output type register, Address offset: 0x04 */ + GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7; //Low speed /*!< GPIO port output speed register, Address offset: 0x08 */ + GPIOB->PUPDR |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1; //Pull Down /*!< GPIO port pull-up/pull-down register, Address offset: 0x0C */ + GPIOB->AFR[0] |= 0x22000000; //AF02 for PB6 & PB7 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ + GPIOB->AFR[1] |= 0x00000000; //nibbles here refer to gpio8..15 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ + + // configure TIM4 as Encoder input + // Enable clock for TIM4 + // __TIM4_CLK_ENABLE(); + RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; + + TIM4->CR1 = 0x0001; // CEN(Counter ENable)='1' < TIM control register 1 + TIM4->SMCR = TIM_ENCODERMODE_TI12; // < TIM slave mode control register + //TIM_ENCODERMODE_TI1 input 1 edges trigger count + //TIM_ENCODERMODE_TI2 input 2 edges trigger count + //TIM_ENCODERMODE_TI12 all edges trigger count + TIM4->CCMR1 = 0xF1F1; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1 + //0xF nibble sets up filter + TIM4->CCMR2 = 0x0000; // < TIM capture/compare mode register 2 + TIM4->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E; // < TIM capture/compare enable register + TIM4->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler + TIM4->ARR = 0xffff; // reload at 0xfffffff < TIM auto-reload register + + TIM4->CNT = 0x0000; //reset the counter before we use it +} +/** + * @brief Get the position of encoder. + * + */ +int16_t CQuadratureEncoder_TIM4::getCount(){ + if(m_instance){ + return TIM4->CNT; + } + return 0xffff; +} +/** + * @brief Reset the value of the counter to zero value. + */ +void CQuadratureEncoder_TIM4::reset(){ + if(m_instance){ + TIM4->CNT = 0; + } +} + + +}; // namespace drivers
diff -r c3e774091195 -r ceee5a608e7c src/Encoders/quadratureencodertask.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Encoders/quadratureencodertask.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,145 @@ +/** + * @file quadratureencodertask.cpp + * @author RBRO/PJ-IU + * @brief + * @version 0.1 + * @date 2018-10-23 + * + * @copyright Copyright (c) 2018 + * + */ +#include <Encoders/Quadratureencodertask.hpp> + + +namespace encoders{ + + + +/** + * @brief Construct a new CQuadratureEncoderTask::CQuadratureEncoderTask object + * + * @param f_period_sec Period of the task + * @param f_Quadratureencoder The counter object + * @param f_resolution The resolution of the rotation encoder. (Cpr count per revolution) + */ +CQuadratureEncoderTask::CQuadratureEncoderTask( float f_period_sec + ,encoders::CQuadratureEncoder_TIMX* f_Quadratureencoder + ,uint16_t f_resolution) + :m_Quadratureencoder(f_Quadratureencoder) + ,m_taskperiod_s(f_period_sec) + ,m_resolution(f_resolution) + ,m_timer(mbed::callback(CQuadratureEncoderTask::static_callback,this)) +{ +} + + +/** + * @brief Start the RosTimer to periodically apply the _run function. + * + */ +void CQuadratureEncoderTask::startTimer(){ + m_timer.start(static_cast<int>(m_taskperiod_s*1000)); +} + + +/** + * @brief The private run function, which will be applied periodically. + * + */ +void CQuadratureEncoderTask::_run(){ + m_encoderCnt = m_Quadratureencoder->getCount(); + m_Quadratureencoder->reset(); +} +/** + * @brief Static callback function for applying the run function. + * + * @param obj CQuadratureEncoderTask object + */ +void CQuadratureEncoderTask::static_callback(void* obj){ + CQuadratureEncoderTask* self = static_cast<CQuadratureEncoderTask*>(obj); + self->_run(); +} + +/** + * @brief Getter function for counted value. + * + * @return int16_t counted impulse + */ +int16_t CQuadratureEncoderTask::getCount(){ + return m_encoderCnt; +} +/** + * @brief Getter function for the rotation per second + * + * @return rotation speed (rotation per second) + */ + +float CQuadratureEncoderTask::getSpeedRps(){ + return static_cast<float>(m_encoderCnt)/ m_resolution / m_taskperiod_s; +} + +/** + * @brief Construct a new CQuadratureEncoderWithFilterTask::CQuadratureEncoderWithFilterTask object + * + * @param f_period_sec Period of the task + * @param f_Quadratureencoder The counter object + * @param f_resolution The resolution of the rotation encoder. (Cpr count per revolution) + * @param f_filter The reference to the filter. + */ +CQuadratureEncoderWithFilterTask::CQuadratureEncoderWithFilterTask( float f_period_sec + ,encoders::CQuadratureEncoder_TIMX* f_Quadratureencoder + ,uint16_t f_resolution + ,filter::CFilterFunction<float>& f_filter) + :CQuadratureEncoderTask(f_period_sec,f_Quadratureencoder,f_resolution) + ,m_filter(f_filter) + { +} + +/** + * @brief Private run method for getting the value from the counter, reseting it. In the last step it filters the readed values. + * + */ +void CQuadratureEncoderWithFilterTask::_run(){ + m_encoderCnt = m_Quadratureencoder->getCount(); + m_Quadratureencoder->reset(); + float temp = m_encoderCnt; + m_encoderCntFiltered = static_cast<int16_t>(m_filter(temp)); + +} + +/** + * @brief Getter function for the last filtered value. + * + */ +int16_t CQuadratureEncoderWithFilterTask::getCount(){ + return m_encoderCntFiltered; +} + +/** + * @brief Getter function for the last filtered rotation speed. + * + */ +float CQuadratureEncoderWithFilterTask::getSpeedRps(){ + return static_cast<double>(m_encoderCntFiltered)/m_resolution / m_taskperiod_s; + +} + +/** + * @brief Getter function for the last non-filtered value. + * + */ +int16_t CQuadratureEncoderWithFilterTask::getNonFilteredCount(){ + return m_encoderCntFiltered; +} + +/** + * @brief Getter function for the last non-filtered rotation speed. + * + */ +float CQuadratureEncoderWithFilterTask::getNonFilteredSpeedRps(){ + return static_cast<double>(m_encoderCntFiltered)/m_resolution / m_taskperiod_s; + +} + + +}; // namespace encoders
diff -r c3e774091195 -r ceee5a608e7c src/Examples/Filter/ekf_am.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Examples/Filter/ekf_am.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,30 @@ +/** + ****************************************************************************** + * @file Ekf_am.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class implementation for the Ackermann model example + * functionality. + ****************************************************************************** + */ + +#include <Examples/Filter/ekf_am.hpp> + +/** \brief Constructor for the CEKF_AM class + * + * Constructor method + * + * \param[in] f_ackermannModel reference to Ackermann model object + * \param[in] f_jacobianMatrixCalc Ackermann model Jacobian matrix + * \param[in] f_Q reference to transition type + * \param[in] f_R reference to observation noise type + */ +examples::filter::CEKF_AM::CEKF_AM(CAckermannModel& f_ackermannModel + ,CJMAckermannModel f_jacobianMatrixCalc + ,const CJMTransitionType& f_Q + ,const CObservationNoiseType& f_R) + :CEKF<double,2,10,5>(f_ackermannModel,f_jacobianMatrixCalc,f_Q,f_R) +{ + +}
diff -r c3e774091195 -r ceee5a608e7c src/Examples/Sensors/sensorpublisher.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Examples/Sensors/sensorpublisher.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,15 @@ +/** + ****************************************************************************** + * @file SensorPublisher.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class implementation for the sensor publisher + * functionality. Because template classes are used, the code was + * placed in a corresponding .inl file. + ****************************************************************************** + */ + +#include <Examples/Sensors/sensorpublisher.hpp> + +
diff -r c3e774091195 -r ceee5a608e7c src/Examples/Sensors/sensortask.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Examples/Sensors/sensortask.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,115 @@ +/** + ****************************************************************************** + * @file SensorTask.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class implementation for the sensor task + * functionality. + ****************************************************************************** + */ +#include <Examples/Sensors/sensortask.hpp> + +namespace examples +{ + + namespace sensors{ + + + /** \brief Constructor for the CCounter class + * + * Constructor method + * + * @param f_period period value + * @param f_serial reference to the serial object + */ + CDISTFTest::CDISTFTest(uint32_t f_period + ,Serial& f_serial) + :task::CTask(f_period) + ,m_serial(f_serial) + ,m_tf() + { + std::array<std::array<float,1>,3> l_num({std::array<float,1>({3}),std::array<float,1>({-5.897}),std::array<float,1>({2.9})}); + std::array<std::array<float,1>,3> l_den({std::array<float,1>({1}),std::array<float,1>({-1.949}),std::array<float,1>({0.9512})}); + m_tf.setNum(l_num); + m_tf.setDen(l_den); + } + + /** \brief Method called each f_period + * + * + * + */ + void CDISTFTest::_run() + { + float l_input=1.0; + float l_output=m_tf(l_input); + m_serial.printf("%.4f\n",l_output); + } + + /** \brief Constructor for the CCounter class + * + * Constructor method + * + * @param f_period period value + * @param f_encoder reference to encoder object + * @param f_serial reference to the serial object + */ + CEncoderSender::CEncoderSender(uint32_t f_period + ,encoders::IEncoderGetter& f_encoder + ,Serial& f_serial) + :task::CTask(f_period) + ,m_isActived(false) + ,m_encoder(f_encoder) + ,m_serial(f_serial) + { + } + + /** \brief Serial callback method + * + * Serial callback attaching serial callback to controller object + * + * @param obj PID controller object + * @param a string to read data from + * @param b string to write data to + * + */ + void CEncoderSender::staticSerialCallback(void* obj,char const * a, char * b){ + examples::sensors::CEncoderSender* self = static_cast<CEncoderSender*>(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 CEncoderSender::serialCallback(char const * a, char * b){ + int l_isActivate=0; + uint32_t l_res = sscanf(a,"%d",&l_isActivate); + if(l_res==1){ + m_isActived=(l_isActivate>=1); + sprintf(b,"ack;;"); + }else{ + sprintf(b,"sintax error;;"); + } + } + + /** \brief Method called each f_period + * + * + * + */ + void CEncoderSender::_run() + { + if(!m_isActived) return; + float l_filtRps=m_encoder.getSpeedRps(); + m_serial.printf("@ENPB:%.2f;;\r\n",l_filtRps); + } + + }; // namespace sensors +}; // namespace examples +
diff -r c3e774091195 -r ceee5a608e7c src/Examples/SystemModels/ackermannmodel.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Examples/SystemModels/ackermannmodel.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,141 @@ +/** + ****************************************************************************** + * @file AckermannModel.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class implementation for the Ackermann model. + ****************************************************************************** + */ + +#include <Examples/SystemModels/ackermannmodel.hpp> + +/* Specify model type */ +using ackermannmodeltype = systemmodels::nlti::mimo::CDiscreteTimeSystemModel<double,2,10,5>; + +/** \brief Constructor for the CAckermannModel class + * + * Constructor method + * + * \param[in] f_states reference to initial system states + * \param[in] f_dt sample time + * \param[in] f_gamma reduction factor from motor to wheel + * \param[in] f_wb wheel base + * \param[in] f_b motor viscous friction constant + * \param[in] f_J moment of inertia of the rotor + * \param[in] f_K motor torque constant + * \param[in] f_R electric resistance + * \param[in] f_L electric inductance + * + */ +examples::systemmodels::ackermannmodel::CAckermannModel::CAckermannModel( + const CStatesType& f_states + ,const double f_dt + ,const double f_gamma + ,const double f_wb + ,const double f_b + ,const double f_J + ,const double f_K + ,const double f_R + ,const double f_L) + :CSystemModelType<double,2,10,5>(f_states,f_dt) + ,m_gamma(f_gamma) + ,m_wb(f_wb) + ,m_bJ(f_b/f_J) + ,m_KJ(f_K/f_J) + ,m_KL(f_K/f_L) + ,m_RL(f_R/f_L) + ,m_L(f_L) +{ +} + +/** \brief Constructor for the CAckermannModel class + * + * Constructor method + * + * \param[in] f_dt sample time + * \param[in] f_gamma reduction factor from motor to wheel + * \param[in] f_wb wheel base + * \param[in] f_b motor viscous friction constant + * \param[in] f_J moment of inertia of the rotor + * \param[in] f_K motor torque constant + * \param[in] f_R electric resistance + * \param[in] f_L electric inductance + * + */ +examples::systemmodels::ackermannmodel::CAckermannModel::CAckermannModel( + const double f_dt + ,const double f_gamma + ,const double f_wb + ,const double f_b + ,const double f_J + ,const double f_K + ,const double f_R + ,const double f_L) + :CSystemModelType<double,2,10,5>(f_dt) + ,m_gamma(f_gamma) + ,m_wb(f_wb) + ,m_bJ(f_b/f_J) + ,m_KJ(f_K/f_J) + ,m_KL(f_K/f_L) + ,m_RL(f_R/f_L) + ,m_L(f_L) +{ +} + +/** \brief Update method + * + * Method for updating system states. + * + * \param[in] f_input reference to input type object + * \return system states + */ +ackermannmodeltype::CStatesType + examples::systemmodels::ackermannmodel::CAckermannModel::update( + const CInputType& f_input) +{ + CState l_states=m_states; + CInput l_input=f_input; + l_states.x()+=m_dt*l_states.x_dot(); + l_states.y()+=m_dt*l_states.y_dot(); + + l_states.x_dot_prev()=l_states.x_dot(); + l_states.y_dot_prev()=l_states.y_dot(); + + l_states.x_dot()=m_gamma*l_states.omega()*cos(l_states.teta_rad()); + l_states.y_dot()=m_gamma*l_states.omega()*sin(l_states.teta_rad()); + + double l_alpha_rad=l_input.alpha()*DEG2RAD; + l_states.teta_rad_dot()=m_gamma*l_states.omega()*tan(l_alpha_rad)/m_wb; + l_states.teta_rad()+=m_dt*l_states.teta_rad_dot(); + + double omega_k_1=(1-m_dt*m_bJ)*l_states.omega()+m_dt*m_KJ*l_states.i();//next state of the motor's rotation speed + l_states.i()=-1*m_dt*m_KL*l_states.omega()+(1-m_dt*m_RL)*l_states.i()+m_dt*l_input.v()/m_L; + l_states.omega()=omega_k_1; + m_states=l_states; + return l_states; +} + +/** \brief Calculate output method + * + * Method for calculating output depending on input. + * + * \param[in] f_input reference to input type object + * \return systemoutputs + */ +ackermannmodeltype::COutputType + examples::systemmodels::ackermannmodel::CAckermannModel::calculateOutput( + const CInputType& f_input) +{ + CState l_states=m_states; + // CInput l_input=f_input; + COutput l_outputs(COutputType::zeros()); + l_outputs.x_ddot()=l_states.x_dot()-l_states.x_dot_prev(); + l_outputs.y_ddot()=l_states.y_dot()-l_states.y_dot_prev(); + l_outputs.teta_rad_dot()=l_states.teta_rad_dot(); + l_outputs.speed()=l_states.omega()*m_gamma; + // output.alpha()=f_input.alpha(); + l_outputs.i()=l_states.i(); + m_outputs=l_outputs; + return l_outputs; +}
diff -r c3e774091195 -r ceee5a608e7c src/Examples/blinker.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Examples/blinker.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,38 @@ +/** + ****************************************************************************** + * @file Blinker.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class implementation for the blinker + * functionality. + ****************************************************************************** + */ + +#include <Examples/blinker.hpp> + + +namespace examples{ + /** \brief Class constructor + * + * Constructor method + * + * \param f_period LED toggling reading period + * \param f_led Digital output line to which the LED is connected + */ + CBlinker::CBlinker(uint32_t f_period, DigitalOut f_led) + : task::CTask(f_period) + , m_led(f_led) + { + m_led = 1; + } + + /** \brief Method called each f_period + * + */ + void CBlinker::_run() + { + m_led = !m_led; + } + +}; // namespace examples
diff -r c3e774091195 -r ceee5a608e7c src/Examples/echoer.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Examples/echoer.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,36 @@ +/** + ****************************************************************************** + * @file Echoer.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class implementation for the serial echoer + * functionality. + ****************************************************************************** + */ + +#include <Examples/echoer.hpp> + +namespace examples{ + /** \brief Class constructor + * + * Constructor method + * + * \param f_period echoer execution period + * \param f_serialPort Serial communication object + */ + CEchoer::CEchoer(uint32_t f_period, Serial& f_serialPort) + : task::CTask(f_period) + , m_serialPort(f_serialPort) + { + } + + /** \brief Method called each f_period + * + */ + void CEchoer::_run() + { + m_serialPort.printf(".\n\r"); + } + +}; // namespace examples
diff -r c3e774091195 -r ceee5a608e7c src/Filter/filter.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Filter/filter.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,14 @@ +/** + ****************************************************************************** + * @file Filter.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class implementations for the filter + * functionality. Because tmeplates are used, an .inl file contains the + * actual implementation. + ****************************************************************************** + */ + +#include <Filter/filter.hpp> +
diff -r c3e774091195 -r ceee5a608e7c src/Linalg/linalg.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Linalg/linalg.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,2 @@ +#include <Linalg/linalg.h> +
diff -r c3e774091195 -r ceee5a608e7c src/MotionController/motioncontroller.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/MotionController/motioncontroller.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,455 @@ +/** + ****************************************************************************** + * @file MotionController.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class definition for the motion controller + * functionality. + ****************************************************************************** + */ + +#include <MotionController/motioncontroller.hpp> + + +/** \brief CMotionController Class constructor + * + * Constructor method + * + * @param f_period_sec period for controller execution in seconds + * @param f_serialPort reference to serial communication object + * @param f_car reference to MOVE object + * @param f_safetyStop reference to spline safery stop object + * @param f_control reference to controller object + */ +CMotionController::CMotionController( + float f_period_sec, + Serial& f_serialPort, + Move& f_car, + CSafetyStopFunction* f_safetyStop, + controllers::CControllerSiso* f_control) + : m_serialPort(f_serialPort) + , m_car(f_car) + , m_speed() + , m_angle() + , m_period_sec(f_period_sec) + , m_isSplineActivated(false) + , m_ispidActivated(false) + , m_motionPlanner() + , m_hbTimeOut() + , m_control(f_control) + , m_safetyStop(f_safetyStop) + , m_timer(mbed::callback(CMotionController::staticCallbackRun,this)) +{ +} + + +/** \brief CMotionController Class constructor + * + * Constructor method + * + * @param f_period_sec period for controller execution in seconds + * @param f_serialPort reference to serial communication object + * @param f_car reference to MOVE object + * @param f_control reference to controller object + */ +CMotionController::CMotionController( + float f_period_sec, + Serial& f_serialPort, + Move& f_car, + controllers::CControllerSiso* f_control) + : m_serialPort(f_serialPort) + , m_car(f_car) + , m_speed() + , m_angle() + , m_period_sec(f_period_sec) + , m_isSplineActivated(false) + , m_ispidActivated(false) + , m_motionPlanner() + , m_hbTimeOut() + , m_control(f_control) + , m_safetyStop(NULL) + , m_timer(mbed::callback(CMotionController::staticCallbackRun,this)) +{ +} + +/** \brief Serial callback method + * + * Serial callback attaching serial callback to controller object + * + * @param obj PID controller object + * @param a string to read data from + * @param b string to write data to + * + */ +void CMotionController::staticSerialCallbackMove(void* obj,char const * a, char * b) +{ + CMotionController* self = static_cast<CMotionController*>(obj); + self->serialCallbackMove(a,b); +} + +/** \brief Serial callback method for BRAKE command + * + * Serial callback attaching serial callback to controller object + * + * @param obj PID controller object + * @param a string to read data from + * @param b string to write data to + * + */ +void CMotionController::staticSerialCallbackBrake(void* obj,char const * a, char * b) +{ + CMotionController* self = static_cast<CMotionController*>(obj); + self->serialCallbackBrake(a,b); +} + +/** \brief Serial callback method for hard BRAKE command + * + * Serial callback attaching serial callback to controller object + * + * @param obj PID controller object + * @param a string to read data from + * @param b string to write data to + * + */ +void CMotionController::staticSerialCallbackHardBrake(void* obj,char const * a, char * b) +{ + CMotionController* self = static_cast<CMotionController*>(obj); + self->serialCallbackHardBrake(a,b); +} + +/** \brief Serial callback method for PID activation command + * + * Serial callback attaching serial callback to controller object + * + * @param obj PID controller object + * @param a string to read data from + * @param b string to write data to + * + */ +void CMotionController::staticSerialCallbackPID(void* obj,char const * a, char * b) +{ + CMotionController* self = static_cast<CMotionController*>(obj); + self->serialCallbackPID(a,b); +} + +/** + * @brief Static seriel callback for spline command + * + * @param obj object with the serial callback method + * @param a string to read data from + * @param b string to write data to + */ +void CMotionController::staticSerialCallbackSpline(void* obj, char const * a,char * b){ + CMotionController* self = static_cast<CMotionController*>(obj); + self->serialCallbackSpline(a,b); +} + +/** \brief Reset method + * + * + * + */ +void CMotionController::reset() +{ + m_speed = 0; + m_angle = 0; +} + +/** \brief Get speed method + * + * + * \return Speed + */ +float CMotionController::getSpeed() +{ + return m_speed; +} + +/** \brief Get angle method + * + * + * \return Angle + */ +float CMotionController::getAngle() +{ + return m_angle; +} + +/** \brief BrakeCallback method + * + * + * + */ +void CMotionController::BrakeCallback(){ + m_state=2; +} + +/** \brief Set state method + * + * @param f_state + * + */ +void CMotionController::setState(int f_state){ + m_state = f_state; +} + +/** \brief Method called each f_period + * + * + * + */ +void CMotionController::_run() +{ + if(m_isSplineActivated) + { + if(m_motionPlanner.hasValidValue()) + { + std::pair<float,float> motion=m_motionPlanner.getNextVelocity(); + float l_dir=m_motionPlanner.getForward()?1:-1; + if(m_ispidActivated) + { + m_speed=motion.first*l_dir; + m_angle=motion.second; + } + else + { + //Pid isn't activated. It have to controllered with the robot speed value in meter per second. + m_serialPort.printf("@SPLN:Err1;;\r\n"); + m_state=2; + m_isSplineActivated=false; + m_speed=0.0; + } + } + else + { + m_serialPort.printf("@SPLN:Stop;;\r\n"); + m_speed=0.0; + m_state=2; + m_isSplineActivated=false; + } + } + + //safety stop function + /** + * @brief It's a part of safety stop functionilty. Check if it's actived or created. + * + */ + if ((m_state!=2)&&(m_safetyStop!=NULL && m_safetyStop->isSafetyStopActive(m_speed,m_angle)==true)) + { + m_state = 2; + } + + switch(m_state) + { + // Move + case 1: + m_car.Steer(m_angle); + if(m_ispidActivated && m_control!=NULL) + { + m_control->setRef(CMotionController::Mps2Rps( m_speed )); + m_serialPort.printf("%f",CMotionController::Mps2Rps( m_speed )); + // Calculate control signal + bool l_isCorrect = m_control->control(); + // Check the correct working of the control method + if( !l_isCorrect ){ + m_car.Brake(); + m_control->clear(); + m_state = 2; + } + m_car.Speed(m_control->get()*100.0);//Y + } + else + { + m_car.Speed(m_speed); + } + break; + // Brake + case 2: + m_car.Steer(m_angle); + m_car.Brake(); + if( m_control!=NULL){ + m_control->clear(); + } + break; + } + +} + +/** \brief Serial callback actions for MOVE command + * + * Serial callback method setting controller to values received + * + * @param a string to read data from + * @param b string to write data to + * + */ +void CMotionController::serialCallbackMove(char const * a, char * b) +{ + float l_speed; + float l_angle; + uint32_t l_res = sscanf(a,"%f;%f",&l_speed,&l_angle); + if (2 == l_res) + { + if( !m_ispidActivated && std::abs(l_speed) > MCTL_PWM_COMMAND_LIMIT ){ + sprintf(b,"Command is too high;;"); + return; + }if( m_ispidActivated && std::abs(l_speed) > MCTL_SPEED_COMMAND_LIMIT ){ + sprintf(b,"Command is too high;;"); + return; + } + + m_speed = l_speed; + m_angle = l_angle; + m_isSplineActivated=false; + m_state=1; + sprintf(b,"ack;;"); + } + else + { + sprintf(b,"sintax error;;"); + } +} + +/** \brief Serial callback actions for BRAKE command + * + * Serial callback method setting controller to values received + * + * @param a string to read data from + * @param b string to write data to + * + */ +void CMotionController::serialCallbackBrake(char const * a, char * b) +{ + float l_angle; + uint32_t l_res = sscanf(a,"%f",&l_angle); + if(1 == l_res) + { + m_speed = 0; + m_angle = l_angle; + m_state = 2; + if( m_control!=NULL){ + m_control->setRef(0); + } + + sprintf(b,"ack;;"); + } + else + { + sprintf(b,"sintax error;;"); + } +} + +/** \brief Serial callback actions for hard BRAKE command + * + * Serial callback method setting controller to values received + * + * @param a string to read data from + * @param b string to write data to + * + */ +void CMotionController::serialCallbackHardBrake(char const * a, char * b) +{ + float l_brake,l_angle; + uint32_t l_res = sscanf(a,"%f;%f",&l_brake,&l_angle); + if(2 == l_res && m_state!=0) + { + m_speed=0; + m_angle = l_angle; + m_car.Inverse(l_brake); + m_hbTimeOut.attach(callback(this,&CMotionController::BrakeCallback),0.04); + m_state = 0; + sprintf(b,"ack;;"); + } + else + { + sprintf(b,"sintax error;;"); + } +} + +/** \brief Serial callback actions for hard PID activation command + * + * Serial callback method setting controller to values received + * + * @param a string to read data from + * @param b string to write data to + * + */ +void CMotionController::serialCallbackPID(char const * a, char * b) +{ + int l_isActivate=0; + uint32_t l_res = sscanf(a,"%d",&l_isActivate); + if(l_res==1) + { + if(m_control==NULL){ + sprintf(b,"Control object wans't instances. Cannot be activate pid controller;;"); + }else{ + m_ispidActivated=(l_isActivate>=1); + m_state = 2; + sprintf(b,"ack;;"); + } + + }else + { + sprintf(b,"sintax error;;"); + } +} + +/** + * @brief Serial callback actions for bezier spline command + * + * @param a string to read data from + * @param b string to write data to + */ +void CMotionController::serialCallbackSpline(char const * a, char * b){ + float a_x,a_y,b_x,b_y,c_x,c_y,d_x,d_y,duration_sec; + int isForward=1; + int32_t nrData=sscanf(a,"%d;%f;%f;%f;%f;%f;%f;%f;%f;%f", + &isForward, + &a_x, + &a_y, + &b_x, + &b_y, + &c_x, + &c_y, + &d_x, + &d_y, + &duration_sec); + if(10==nrData && duration_sec>0 && (isForward==1 || isForward==0)){ + m_motionPlanner.setMotionPlannerParameters(static_cast<bool>(isForward), std::complex<float>(a_x,a_y),std::complex<float>(b_x,b_y),std::complex<float>(c_x,c_y),std::complex<float>(d_x,d_y),duration_sec,m_period_sec); + m_isSplineActivated=true; + m_state=1; + sprintf(b,"ack;;"); + }else{ + sprintf(b,"sintax error;;"); + } +} + + +/** + * @brief Function to convert from linear velocity ( centimeter per second ) of robot to angular velocity ( rotation per second ) of motor. + * + * @param f_vel_mps linear velocity + * @return float angular velocity + */ +float CMotionController::Mps2Rps(float f_vel_mps){ + return f_vel_mps * 150.0; +} + +/** + * @brief Static callback function for run method + * + * @param obj pointer for the object + */ +void CMotionController::staticCallbackRun(void* obj){ + CMotionController* self = static_cast<CMotionController*>(obj); + self->_run(); +} + +/** + * @brief Start RtosTimer, which periodically apply the run method. The period of the task is defined in the contructor. + * + */ +void CMotionController::startRtosTimer(){ + this->m_timer.start(static_cast<int>(m_period_sec*1000)); +}
diff -r c3e774091195 -r ceee5a608e7c src/Move/move.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Move/move.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,134 @@ +/** + ****************************************************************************** + * @file Move.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class definition for the moving + * functionality. + ****************************************************************************** + */ + +#include "mbed.h" +#include <Move/move.hpp> +//#include <array.h> + +/** \brief Move Class constructor + * + * Constructor method + * + * @param _pwm_servo + * @param _pwm_driver + * @param _ina_driver + * @param _inb_driver + * @param _current_driver + */ +Move::Move( + PinName _pwm_servo, + PinName _pwm_driver, + PinName _ina_driver, + PinName _inb_driver, + PinName _current_driver) + :servo(_pwm_servo), + vnh(_pwm_driver,_ina_driver,_inb_driver,_current_driver) +{ +}; + +/** \brief Move Class destructor + * + * Destructor method + * + * + */ +Move::~Move() +{ +}; + +/** \brief Steer method + * + * @param angle + * + */ +void Move::Steer(float angle) +{ + if ((angle <= 23) && (angle >= -23)) + servo.SetAngle(angle); +}; + +/** \brief Move method + * + * @param speed The Pwm value, must belong to [0,100]. + * + */ +void Move::Speed(float speed) +{ + speed /=100; + vnh.Run(speed); +}; + +/** \brief Brake method + * + * + * + */ +void Move::Brake() +{ + vnh.Brake(); +} + +/** \brief Hard brake method + * + * @param f_speed The Pwm value, must belong to [0,100]. + * + */ +void Move::Inverse(float f_speed) +{ + f_speed /=100; + vnh.Inverse(f_speed); +} + +/** \brief Car testing method + * + * + * + */ +void Move::TestCar() +{ + Steer(20); + wait(1); + Steer(0); + wait(1); + Steer(-20); + wait(1); + ResetCar(); + wait(1); + Speed(25); + wait(1); + Speed(0); + wait(1); + Speed(-25); + wait(1); + ResetCar(); + wait(1); +}; + +/** \brief Reset speed and steer method + * + * + * + */ +void Move::ResetCar() +{ + Steer(0); + Speed(0); +}; + +/** \brief Get bridge (driver) object reference method + * + * + * @return reference to VNH object + */ +drivers::VNH& Move::getVNH() +{ + return vnh; +}
diff -r c3e774091195 -r ceee5a608e7c src/Queue/queue.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Queue/queue.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,12 @@ +/** + ****************************************************************************** + * @file Queue.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class definition for the queue + * functionality. + ****************************************************************************** + */ + +#include <Queue/queue.hpp>
diff -r c3e774091195 -r ceee5a608e7c src/SHARP_IR_distance/DistanceSensors/distancesensors.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/SHARP_IR_distance/DistanceSensors/distancesensors.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,13 @@ +/** + ****************************************************************************** + * @file DistanceSensors.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date 08-January-2018 + * @brief This file contains the class implementation for the distance sensors + * read methods. + ****************************************************************************** + */ + +#include <SHARP_IR_distance\DistanceSensors\distancesensors.hpp> +
diff -r c3e774091195 -r ceee5a608e7c src/SHARP_IR_distance/IR_DistanceSensorDriver/ir_distancesensordriver.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/SHARP_IR_distance/IR_DistanceSensorDriver/ir_distancesensordriver.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,91 @@ +/** + ****************************************************************************** + * @file IR_DistanceSensorDriver.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date 08-January-2018 + * @brief This file contains the class implementation for the SHARP IR sensor read + * methods. + ****************************************************************************** + */ + +#include <SHARP_IR_distance\IR_DistanceSensorDriver\ir_distancesensordriver.hpp> + +/** + * @brief Construct a new c ir distancesensordriver::c ir distancesensordriver object + * + * @param _ain_pin analog entrance pin + * @param _en_pin digital enable pin + */ +C_IR_DistanceSensorDriver::C_IR_DistanceSensorDriver(AnalogIn _ain_pin, DigitalOut _en_pin) + : ain_pin(_ain_pin) + , en_pin(_en_pin) + , value(0) +{ +}; + +/** @brief Class destructor + + Destructor method + */ +C_IR_DistanceSensorDriver::~C_IR_DistanceSensorDriver() +{ +}; + +/** @brief Method for enabling sensor + + + + */ +void C_IR_DistanceSensorDriver::enable(){ + en_pin=1; +} + +/** @brief Method for disabling sensor + + + + */ +void C_IR_DistanceSensorDriver::disable(){ + en_pin=0; +} +/* +void C_IR_DistanceSensorDriver::setValue(float val){ + this->value=val; +} +*/ + +/** @brief Method for getting value read from sensor + + + @return value read from sensor + */ +float C_IR_DistanceSensorDriver::getValue(){ + return this->value; +} + +/** @brief Method for getting scaled value read from sensor + + + @return scaled value read from sensor + */ +float C_IR_DistanceSensorDriver::ReadDistance() +{ + // read only if sensor enabled + if (this->en_pin == 1){ + //read analog pin + float current_value=ain_pin.read(); + //-scaling + float scaled_value = C_IR_DistanceSensorDriver_A * pow(current_value,5) + + C_IR_DistanceSensorDriver_B * pow(current_value,4) + + C_IR_DistanceSensorDriver_C * pow(current_value,3) + + C_IR_DistanceSensorDriver_D * pow(current_value,2) + + C_IR_DistanceSensorDriver_E * current_value + + C_IR_DistanceSensorDriver_F; + + //float scaled_value=current_value; // without scaling, gross signal + this->value=scaled_value; + return this->value; + } + else return 0; +}
diff -r c3e774091195 -r ceee5a608e7c src/SafetyStop/safetystopfunction.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/SafetyStop/safetystopfunction.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,135 @@ +/** + ****************************************************************************** + * @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;;"); + } + } + + +
diff -r c3e774091195 -r ceee5a608e7c src/SerialMonitor/serialmonitor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/SerialMonitor/serialmonitor.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,139 @@ +/** + ****************************************************************************** + * @file SerialMonitor.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class definition for the serial communication + * functionality. + ****************************************************************************** + */ + +#include <SerialMonitor/serialmonitor.hpp> + + +namespace serial{ + + /** @brief CSerialMonitor class constructor + * + * + * @param f_serialPort reference to serial object + * @param f_serialSubscriberMap sensor mounteed on left front part + */ + CSerialMonitor::CSerialMonitor(Serial& f_serialPort + ,CSerialSubscriberMap f_serialSubscriberMap) + :task::CTask(0) + , m_serialPort(f_serialPort) + , m_RxBuffer() + , m_TxBuffer() + , m_parseBuffer() + , m_parseIt(m_parseBuffer.begin()) + , m_serialSubscriberMap(f_serialSubscriberMap) + { + m_serialPort.attach(mbed::callback(&CSerialMonitor::RxCallback, this), Serial::RxIrq); + m_serialPort.attach(mbed::callback(&CSerialMonitor::TxCallback, this), Serial::TxIrq); + } + + /** @brief Rx callback + * + * @param thisPointer the object pointer + * + */ + void CSerialMonitor::RxCallback(void *thisPointer) + { + CSerialMonitor* self = static_cast<CSerialMonitor*>(thisPointer); + self->serialRxCallback(); + } + + /** @brief Tx callback + * + * @param thisPointer the object pointer + * + */ + void CSerialMonitor::TxCallback(void *thisPointer) + { + CSerialMonitor* self = static_cast<CSerialMonitor*>(thisPointer); + self->serialTxCallback(); + } + + /** @brief Rx callback actions + * + * @param None + * + */ + void CSerialMonitor::serialRxCallback() + { + __disable_irq(); + while ((m_serialPort.readable()) && (!m_RxBuffer.isFull())) { + char l_c = m_serialPort.getc(); + m_RxBuffer.push(l_c); + } + __enable_irq(); + return; + } + + /** @brief Tx callback actions + * + * @param None + * + */ + void CSerialMonitor::serialTxCallback() + { + __disable_irq(); + while ((m_serialPort.writeable()) && (!m_TxBuffer.isEmpty())) { + m_serialPort.putc(m_TxBuffer.pop()); + } + __enable_irq(); + return; + } + + /** @brief Run method + * + * @param None + * @param None + */ + void CSerialMonitor::_run() + { + if ((!m_RxBuffer.isEmpty())) + { + char l_c = m_RxBuffer.pop(); + // m_serialPort.printf("%c",l_c); + if ('#' == l_c) + { + m_parseIt = m_parseBuffer.begin(); + m_parseIt[0] = l_c; + m_parseIt++; + return; + } + if (m_parseIt != m_parseBuffer.end()) + { + if (l_c == '\n') + { + if ((';' == m_parseIt[-3]) && (';' == m_parseIt[-2]) && ('\r' == m_parseIt[-1])) + { + char l_msgID[5]; + char l_msg[256]; + + uint32_t res = sscanf(m_parseBuffer.data(),"#%4s:%s;;",l_msgID,l_msg); + if (res == 2) + { + auto l_pair = m_serialSubscriberMap.find(l_msgID); + if (l_pair != m_serialSubscriberMap.end()) + { + char l_resp[256] = "no response given"; + string s(l_resp); + l_pair->second(l_msg,l_resp); + m_serialPort.printf("@%s:%s\r\n",l_msgID,l_resp); + } + } + m_parseIt = m_parseBuffer.begin(); + } + } + m_parseIt[0] = l_c; + m_parseIt++; + return; + } + } + } + +}; // namespace serial
diff -r c3e774091195 -r ceee5a608e7c src/SystemModels/systemmodels.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/SystemModels/systemmodels.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,11 @@ +/** + ****************************************************************************** + * @file SystemModels.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class definitions for system model. + ****************************************************************************** + */ + +#include <SystemModels/systemmodels.hpp>
diff -r c3e774091195 -r ceee5a608e7c src/TaskManager/taskmanager.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/TaskManager/taskmanager.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,92 @@ +/** + ****************************************************************************** + * @file TaskManager.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class definition for task manager. + ****************************************************************************** + */ + +#include <TaskManager/taskmanager.hpp> + +namespace task{ + + /******************************************************************************/ + /** \brief CTask class constructor + * + * Constructor method + * + * @param f_period execution period + */ + CTask::CTask(uint32_t f_period) + : m_period(f_period) + , m_ticks(0) + , m_triggered(false) + { + } + + /** \brief CTask class destructor + * + * Destructor method + * + * + */ + CTask::~CTask() + { + } + + /** \brief Run method + * + * Destructor method + * + * + * + */ + void CTask::run() + { + if (m_triggered) + { + m_triggered = false; + _run(); + } + } + + /******************************************************************************/ + /** \brief CTaskManager class constructor + * + * Constructor method + * + * @param f_taskList task list + * @param f_taskCount task count + * @param f_baseFreq base frequency + */ + CTaskManager::CTaskManager(task::CTask** f_taskList, uint32_t f_taskCount, float f_baseFreq) + : m_taskList(f_taskList) + , m_taskCount(f_taskCount) + { + m_ticker.attach(mbed::callback(&task::CTaskManager::callback, this), f_baseFreq); + } + + /** \brief CTaskManager class destructor + * + * Destructor method + * + * + */ + CTaskManager::~CTaskManager() + { + m_ticker.detach(); + } + + /** \brief Callback method + * + * @param thisPointer The object pointer + */ + void CTaskManager::callback(void *thisPointer) + { + task::CTaskManager* self = static_cast<task::CTaskManager*>(thisPointer); + self->timerCallback(); + } + +}; // namespace task
diff -r c3e774091195 -r ceee5a608e7c src/Timer/timer.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Timer/timer.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,11 @@ +/** + ****************************************************************************** + * @file Timer.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class definition for the timer functionality. + ****************************************************************************** + */ + +#include <Timer/timer.hpp>