altb_pmic / Mbed OS cuboid_balance

for Infotag

Dependencies:   Library_Cntrl Library_Misc

path_1d.cpp@5:768e10f6d372, 2021-02-25 (annotated)

Committer:
altb2
Date:
Thu Feb 25 20:28:45 2021 +0000
Revision:
5:768e10f6d372
first commit of Mirror actuator

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 5:768e10f6d372 1 #include "path_1d.h"
altb2 5:768e10f6d372 2 using namespace std;
altb2 5:768e10f6d372 3
altb2 5:768e10f6d372 4 path_1d::path_1d(void)
altb2 5:768e10f6d372 5 {
altb2 5:768e10f6d372 6 this->finished = false;
altb2 5:768e10f6d372 7 this->started = false;
altb2 5:768e10f6d372 8 this->initialized = false;
altb2 5:768e10f6d372 9
altb2 5:768e10f6d372 10 }
altb2 5:768e10f6d372 11
altb2 5:768e10f6d372 12
altb2 5:768e10f6d372 13 path_1d::~path_1d() {}
altb2 5:768e10f6d372 14
altb2 5:768e10f6d372 15
altb2 5:768e10f6d372 16 void path_1d::initialize(float a,float v, float s,float v_0,float v_end, float s_start){
altb2 5:768e10f6d372 17 this->a_max = a;
altb2 5:768e10f6d372 18 this->v_max = v;
altb2 5:768e10f6d372 19 this->s_start = s_start;
altb2 5:768e10f6d372 20 this->sig = (float)(((s-s_start)>=0) -((s-s_start)<0));
altb2 5:768e10f6d372 21 this->v_0 = abs(v_0);
altb2 5:768e10f6d372 22 this->dT=-this->v_0/this->a_max;
altb2 5:768e10f6d372 23 v_end = abs(v_end);
altb2 5:768e10f6d372 24 this->s_0 = abs(s-s_start);
altb2 5:768e10f6d372 25 this->s_end = this->s_0+.5f*v_end*v_end/this->a_max+.5f*this->v_0*this->v_0/this->a_max;
altb2 5:768e10f6d372 26 this->T_end=2*sqrt(this->s_end/this->a_max);
altb2 5:768e10f6d372 27 float VT= a * this->T_end/2.0f;
altb2 5:768e10f6d372 28 if(VT<=this->v_max)
altb2 5:768e10f6d372 29 {
altb2 5:768e10f6d372 30 this->T1=this->T_end/2.0f;
altb2 5:768e10f6d372 31 this->T2=this->T_end/2.0f;
altb2 5:768e10f6d372 32 }
altb2 5:768e10f6d372 33 else
altb2 5:768e10f6d372 34 {
altb2 5:768e10f6d372 35 float V_ = VT-this->v_max;
altb2 5:768e10f6d372 36 float TX = 2.0f*V_ / this->a_max;
altb2 5:768e10f6d372 37 float DS = .5f*V_ * TX;
altb2 5:768e10f6d372 38 float T_ = TX + DS / this->v_max;
altb2 5:768e10f6d372 39 this->T_end = this->T_end+DS / this->v_max;
altb2 5:768e10f6d372 40 this->T1=(this->T_end-T_)/2.0f; // beschleunige bis hier
altb2 5:768e10f6d372 41 this->T2=(this->T_end+T_)/2.0f; // bis hier konstant
altb2 5:768e10f6d372 42 this->X1=0.5f*this->a_max*this->T1*this->T1;
altb2 5:768e10f6d372 43 this->X2=this->X1 + this->v_max*(this->T2-this->T1);
altb2 5:768e10f6d372 44 this->X3=this->X2 + .5f*a_max*(this->T_end - this->T2)*(this->T_end-this->T2);
altb2 5:768e10f6d372 45 }
altb2 5:768e10f6d372 46 finished = false;
altb2 5:768e10f6d372 47 started = false;
altb2 5:768e10f6d372 48 initialized = true;
altb2 5:768e10f6d372 49 }
altb2 5:768e10f6d372 50 void path_1d::get_x_v(float t, float *x,float *v)
altb2 5:768e10f6d372 51 {
altb2 5:768e10f6d372 52 t = t-t_offset-dT;
altb2 5:768e10f6d372 53 if(t <=T1)
altb2 5:768e10f6d372 54 {
altb2 5:768e10f6d372 55 *x = 0.5f*a_max*t*t;
altb2 5:768e10f6d372 56 *v = a_max * t;
altb2 5:768e10f6d372 57 }
altb2 5:768e10f6d372 58 else if(t<=T2)
altb2 5:768e10f6d372 59 {
altb2 5:768e10f6d372 60 *x = X1+v_max*(t-T1);
altb2 5:768e10f6d372 61 *v = v_max;
altb2 5:768e10f6d372 62 }
altb2 5:768e10f6d372 63 else if(t<=T_end)
altb2 5:768e10f6d372 64 {
altb2 5:768e10f6d372 65 *x = X3-.5*a_max*(t-T_end)*(t-T_end);
altb2 5:768e10f6d372 66 *v = -a_max*(t-T_end);
altb2 5:768e10f6d372 67 }
altb2 5:768e10f6d372 68 else
altb2 5:768e10f6d372 69 {
altb2 5:768e10f6d372 70 *x = s_end;
altb2 5:768e10f6d372 71 *v = 0;
altb2 5:768e10f6d372 72 }
altb2 5:768e10f6d372 73 if(t>=T_end)
altb2 5:768e10f6d372 74 {
altb2 5:768e10f6d372 75 finished = true;
altb2 5:768e10f6d372 76 started = false;
altb2 5:768e10f6d372 77 }
altb2 5:768e10f6d372 78 *x -= .5*v_0*v_0/a_max;
altb2 5:768e10f6d372 79 if((*x) >= s_0)
altb2 5:768e10f6d372 80 {
altb2 5:768e10f6d372 81 finished = true;
altb2 5:768e10f6d372 82 started = false;
altb2 5:768e10f6d372 83 }
altb2 5:768e10f6d372 84 *x *= sig;
altb2 5:768e10f6d372 85 *x += s_start;
altb2 5:768e10f6d372 86 *v *= sig;
altb2 5:768e10f6d372 87 }
altb2 5:768e10f6d372 88 void path_1d::start(float t)
altb2 5:768e10f6d372 89 {
altb2 5:768e10f6d372 90 t_offset = t;
altb2 5:768e10f6d372 91 finished = false;
altb2 5:768e10f6d372 92 started = true;
altb2 5:768e10f6d372 93 }