Alex Pirciu
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
--- /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
+
--- /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
--- /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
+
--- /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;
+};
+
+};
--- /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
+
--- /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
--- /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
--- /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
+
--- /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;
+}
--- /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
--- /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;;");
+ }
+}
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
+
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
+
--- /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;
+}
--- /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
+
--- /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;
+}
--- /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
+
--- /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
--- /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
+
--- /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;
+}
--- /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
--- /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);
+}
--- /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
--- /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
--- /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
+
+
+
+
+
+
+
+
--- /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
+
--- /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
--- /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;
+}
--- /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
--- /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
--- /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 ++;
+}
--- /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>
--- /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>
--- /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
--- /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();
+ }
+ }
+}
--- /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
--- /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> +
--- /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>
--- /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
--- /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
--- /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;
+}
--- /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);
+}
+
--- /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
--- /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
--- /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)
+{
+
+}
--- /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> + +
--- /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
+
--- /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;
+}
--- /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
--- /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
--- /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> +
--- /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> +
--- /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));
+}
--- /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;
+}
--- /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>
--- /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> +
--- /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;
+}
--- /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;;");
+ }
+ }
+
+
+
--- /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
--- /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>
--- /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
--- /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>