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