Preston Ernst / Mbed OS mirror_actuator_VT

-data logging revision

Dependencies:   FastPWM

Lib_Misc/path_1d.cpp@2:92c25cb669f4, 2021-08-24 (annotated)

Committer:
ernstpre
Date:
Tue Aug 24 08:51:13 2021 +0000
Revision:
2:92c25cb669f4
Parent:
0:d2e117716219 
Publish Commit 24/8/21

Who changed what in which revision?

UserRevisionLine numberNew 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 }