Alex Pirciu / Mbed 2 deprecated BFMC

a

Dependencies:   mbed mbed-rtos

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alexpirciu 1:ceee5a608e7c 1 /**
alexpirciu 1:ceee5a608e7c 2 ******************************************************************************
alexpirciu 1:ceee5a608e7c 3 * @file BezierMotionPlanner.cpp
alexpirciu 1:ceee5a608e7c 4 * @author RBRO/PJ-IU
alexpirciu 1:ceee5a608e7c 5 * @version V1.0.0
alexpirciu 1:ceee5a608e7c 6 * @date day-month-year
alexpirciu 1:ceee5a608e7c 7 * @brief This file contains the class definition for the Bezier Motion
alexpirciu 1:ceee5a608e7c 8 * Planner methods.
alexpirciu 1:ceee5a608e7c 9 ******************************************************************************
alexpirciu 1:ceee5a608e7c 10 */
alexpirciu 1:ceee5a608e7c 11
alexpirciu 1:ceee5a608e7c 12 #include <iostream>
alexpirciu 1:ceee5a608e7c 13 #include <BezierMotionPlanner/bezierMotionplanner.hpp>
alexpirciu 1:ceee5a608e7c 14
alexpirciu 1:ceee5a608e7c 15 namespace planner{
alexpirciu 1:ceee5a608e7c 16
alexpirciu 1:ceee5a608e7c 17 /**
alexpirciu 1:ceee5a608e7c 18 * @brief Construct a new CBezierMotionPlanner::CBezierMotionPlanner object
alexpirciu 1:ceee5a608e7c 19 *
alexpirciu 1:ceee5a608e7c 20 */
alexpirciu 1:ceee5a608e7c 21 CBezierMotionPlanner::CBezierMotionPlanner()
alexpirciu 1:ceee5a608e7c 22 {
alexpirciu 1:ceee5a608e7c 23 this->isInitialized=false;
alexpirciu 1:ceee5a608e7c 24 }
alexpirciu 1:ceee5a608e7c 25
alexpirciu 1:ceee5a608e7c 26 /**
alexpirciu 1:ceee5a608e7c 27 * @brief Construct a new CBezierMotionPlanner::CBezierMotionPlanner object
alexpirciu 1:ceee5a608e7c 28 *
alexpirciu 1:ceee5a608e7c 29 * @param isForward Forward movement flag
alexpirciu 1:ceee5a608e7c 30 * @param a Point A
alexpirciu 1:ceee5a608e7c 31 * @param b Point B
alexpirciu 1:ceee5a608e7c 32 * @param c Point C
alexpirciu 1:ceee5a608e7c 33 * @param d Point D
alexpirciu 1:ceee5a608e7c 34 * @param motion_duration_i The motion duration in second.
alexpirciu 1:ceee5a608e7c 35 * @param timestep_i The base period of the planner. (Sample time)
alexpirciu 1:ceee5a608e7c 36 */
alexpirciu 1:ceee5a608e7c 37 CBezierMotionPlanner::CBezierMotionPlanner(bool isForward,
alexpirciu 1:ceee5a608e7c 38 std::complex<float> a,
alexpirciu 1:ceee5a608e7c 39 std::complex<float> b,
alexpirciu 1:ceee5a608e7c 40 std::complex<float> c,
alexpirciu 1:ceee5a608e7c 41 std::complex<float> d,
alexpirciu 1:ceee5a608e7c 42 float motion_duration_i,
alexpirciu 1:ceee5a608e7c 43 float timestep_i)
alexpirciu 1:ceee5a608e7c 44 : isForward(isForward)
alexpirciu 1:ceee5a608e7c 45 , bezierCurve(a,b,c,d)
alexpirciu 1:ceee5a608e7c 46 , motion_duration(motion_duration_i)
alexpirciu 1:ceee5a608e7c 47 , time_step(timestep_i)
alexpirciu 1:ceee5a608e7c 48 {
alexpirciu 1:ceee5a608e7c 49 this->bezierValueInput_step=1.0/(int)(this->motion_duration/this->time_step);
alexpirciu 1:ceee5a608e7c 50 this->next_bezierValueInput=0.0;
alexpirciu 1:ceee5a608e7c 51 this->isInitialized=true;
alexpirciu 1:ceee5a608e7c 52 }
alexpirciu 1:ceee5a608e7c 53
alexpirciu 1:ceee5a608e7c 54
alexpirciu 1:ceee5a608e7c 55 /**
alexpirciu 1:ceee5a608e7c 56 * @brief Set motion planner parameters
alexpirciu 1:ceee5a608e7c 57 *
alexpirciu 1:ceee5a608e7c 58 * @param isForward Forward movement flag
alexpirciu 1:ceee5a608e7c 59 * @param a Point A
alexpirciu 1:ceee5a608e7c 60 * @param b Point B
alexpirciu 1:ceee5a608e7c 61 * @param c Point C
alexpirciu 1:ceee5a608e7c 62 * @param d Point D
alexpirciu 1:ceee5a608e7c 63 * @param motion_duration_i The motion duration in second.
alexpirciu 1:ceee5a608e7c 64 * @param timestep_i The base period of the planner. (Sample time)
alexpirciu 1:ceee5a608e7c 65 */
alexpirciu 1:ceee5a608e7c 66 void CBezierMotionPlanner::setMotionPlannerParameters(bool isForward,
alexpirciu 1:ceee5a608e7c 67 std::complex<float> a,
alexpirciu 1:ceee5a608e7c 68 std::complex<float> b,
alexpirciu 1:ceee5a608e7c 69 std::complex<float> c,
alexpirciu 1:ceee5a608e7c 70 std::complex<float> d,
alexpirciu 1:ceee5a608e7c 71 float motion_duration_i,
alexpirciu 1:ceee5a608e7c 72 float timestep_i)
alexpirciu 1:ceee5a608e7c 73 {
alexpirciu 1:ceee5a608e7c 74 this->isForward=isForward;
alexpirciu 1:ceee5a608e7c 75 this->motion_duration=motion_duration_i;
alexpirciu 1:ceee5a608e7c 76 this->time_step=timestep_i;
alexpirciu 1:ceee5a608e7c 77 this->bezierCurve.setBezierCurve(a,b,c,d);
alexpirciu 1:ceee5a608e7c 78 this->bezierValueInput_step=1.0/(int)(this->motion_duration/this->time_step);
alexpirciu 1:ceee5a608e7c 79 this->next_bezierValueInput=0.0;
alexpirciu 1:ceee5a608e7c 80 this->isInitialized=true;
alexpirciu 1:ceee5a608e7c 81 }
alexpirciu 1:ceee5a608e7c 82
alexpirciu 1:ceee5a608e7c 83 /**
alexpirciu 1:ceee5a608e7c 84 * @brief Destroy the CBezierMotionPlanner::CBezierMotionPlanner object
alexpirciu 1:ceee5a608e7c 85 *
alexpirciu 1:ceee5a608e7c 86 */
alexpirciu 1:ceee5a608e7c 87 CBezierMotionPlanner::~CBezierMotionPlanner()
alexpirciu 1:ceee5a608e7c 88 {
alexpirciu 1:ceee5a608e7c 89 }
alexpirciu 1:ceee5a608e7c 90
alexpirciu 1:ceee5a608e7c 91
alexpirciu 1:ceee5a608e7c 92 /**
alexpirciu 1:ceee5a608e7c 93 * @brief Get the Bezier curve.
alexpirciu 1:ceee5a608e7c 94 *
alexpirciu 1:ceee5a608e7c 95 */
alexpirciu 1:ceee5a608e7c 96 math::BezierCurve<float> CBezierMotionPlanner::getBezierCurve()
alexpirciu 1:ceee5a608e7c 97 {
alexpirciu 1:ceee5a608e7c 98 return this->bezierCurve;
alexpirciu 1:ceee5a608e7c 99 }
alexpirciu 1:ceee5a608e7c 100
alexpirciu 1:ceee5a608e7c 101 /**
alexpirciu 1:ceee5a608e7c 102 * @brief Get the next control parameters. It calculates the velocity and angle, and increase input value.
alexpirciu 1:ceee5a608e7c 103 *
alexpirciu 1:ceee5a608e7c 104 * @return It returns the next forward velocity and direction angle.
alexpirciu 1:ceee5a608e7c 105 */
alexpirciu 1:ceee5a608e7c 106 std::pair<float,float> CBezierMotionPlanner::getNextVelocity(){
alexpirciu 1:ceee5a608e7c 107 std::pair<float,float> commands=this->getVelocity(next_bezierValueInput);
alexpirciu 1:ceee5a608e7c 108 this->next_bezierValueInput+=this->bezierValueInput_step;
alexpirciu 1:ceee5a608e7c 109 return commands;
alexpirciu 1:ceee5a608e7c 110 }
alexpirciu 1:ceee5a608e7c 111
alexpirciu 1:ceee5a608e7c 112 /**
alexpirciu 1:ceee5a608e7c 113 * @brief Get the forward velocity and steering angle, base on the given input value.
alexpirciu 1:ceee5a608e7c 114 *
alexpirciu 1:ceee5a608e7c 115 * @param input_value The input value have to belong to interval [0,1].
alexpirciu 1:ceee5a608e7c 116 * @return It returns a pair of number, where the fist variable and the second contains the forward velocity and the steering angular, respectively.
alexpirciu 1:ceee5a608e7c 117 */
alexpirciu 1:ceee5a608e7c 118 std::pair<float,float> CBezierMotionPlanner::getVelocity(float input_value)
alexpirciu 1:ceee5a608e7c 119 {
alexpirciu 1:ceee5a608e7c 120 if(!this->isInitialized) return std::pair<float,float>(0,0);
alexpirciu 1:ceee5a608e7c 121
alexpirciu 1:ceee5a608e7c 122 std::complex<float> dS=this->bezierCurve.get_FO_DerivateValue(input_value);
alexpirciu 1:ceee5a608e7c 123
alexpirciu 1:ceee5a608e7c 124 float dl_absolute=sqrt((dS*std::conj(dS)).real());//[0,1]//Length of the vector
alexpirciu 1:ceee5a608e7c 125 float dl_real=dl_absolute/this->motion_duration;
alexpirciu 1:ceee5a608e7c 126
alexpirciu 1:ceee5a608e7c 127 std::complex<float> ddS=this->bezierCurve.get_SO_DerivateValue(input_value);
alexpirciu 1:ceee5a608e7c 128
alexpirciu 1:ceee5a608e7c 129 std::complex<float> correctorValue(0,-2);
alexpirciu 1:ceee5a608e7c 130 std::complex<float> temp1=(dS*std::conj(ddS)-std::conj(dS)*ddS)/correctorValue;
alexpirciu 1:ceee5a608e7c 131 float num=temp1.real();
alexpirciu 1:ceee5a608e7c 132 if(dl_absolute==0)
alexpirciu 1:ceee5a608e7c 133 {
alexpirciu 1:ceee5a608e7c 134 return std::pair<float,float>(dl_real,0);
alexpirciu 1:ceee5a608e7c 135 }
alexpirciu 1:ceee5a608e7c 136
alexpirciu 1:ceee5a608e7c 137 float k=num/pow(dl_absolute,3);
alexpirciu 1:ceee5a608e7c 138
alexpirciu 1:ceee5a608e7c 139 float angle_rad=atan(k*WHEELBASE);
alexpirciu 1:ceee5a608e7c 140 float angle_deg=(180.f/M_PI)*angle_rad;
alexpirciu 1:ceee5a608e7c 141
alexpirciu 1:ceee5a608e7c 142 std::pair<float,float> commands(dl_real,angle_deg);
alexpirciu 1:ceee5a608e7c 143 return commands;
alexpirciu 1:ceee5a608e7c 144 }
alexpirciu 1:ceee5a608e7c 145
alexpirciu 1:ceee5a608e7c 146 /**
alexpirciu 1:ceee5a608e7c 147 * @brief Get the state of the planner.
alexpirciu 1:ceee5a608e7c 148 *
alexpirciu 1:ceee5a608e7c 149 * @return true The planner has valid value.
alexpirciu 1:ceee5a608e7c 150 * @return false The planner finished the last given curve, it cannnot get correct parameters.
alexpirciu 1:ceee5a608e7c 151 */
alexpirciu 1:ceee5a608e7c 152 bool CBezierMotionPlanner::hasValidValue()
alexpirciu 1:ceee5a608e7c 153 {
alexpirciu 1:ceee5a608e7c 154 return (next_bezierValueInput>=0 && next_bezierValueInput<=1);
alexpirciu 1:ceee5a608e7c 155 }
alexpirciu 1:ceee5a608e7c 156
alexpirciu 1:ceee5a608e7c 157 /** @brief Get the direction of the motion
alexpirciu 1:ceee5a608e7c 158 *
alexpirciu 1:ceee5a608e7c 159 *
alexpirciu 1:ceee5a608e7c 160 * @return true The motion direction is forward.
alexpirciu 1:ceee5a608e7c 161 * @return false The motion direction is backward.
alexpirciu 1:ceee5a608e7c 162 */
alexpirciu 1:ceee5a608e7c 163 bool CBezierMotionPlanner::getForward()
alexpirciu 1:ceee5a608e7c 164 {
alexpirciu 1:ceee5a608e7c 165 return this->isForward;
alexpirciu 1:ceee5a608e7c 166 }
alexpirciu 1:ceee5a608e7c 167
alexpirciu 1:ceee5a608e7c 168 }; // namespace planner