ramp_timebase.cpp

#include <iostream>
#include <cmath>
using namespace std;

class Ramp {
public:
    Ramp(float v_start, float v_max, float v_end, float acceleration, float L, float T)
    {
        /* 加速時間と減速時間を計算
         http://keisan.casio.jp/exec/system/1164005431 */
        float t_start = fabs(v_max - v_start) / acceleration;
        float t_end = fabs(v_max - v_end) / acceleration;
        /* 絶対値を求めておく */
        float w_start = fabs(v_start);
        float w_max = fabs(v_max);
        float w_end = fabs(v_end);
        /* 加速距離、減速距離を求める */
        float l1 = (w_start + w_max) * t_start / 2;
        float l2 = (w_max + w_end) * t_end / 2;
        /* 台形にならない場合 */
        if(l1 + l2 > L) {
            /*
              加速・減速時の距離を足すと距離Lになる速度Vを求める.

            　公式 V^2 - V0^2 = 2aL より距離Lは以下の式で表すことができる.
              (V^2 - V0^2) / 2a + (V'^2 - V0'^2) / 2a' = L

              速度Vについて解くと以下のようになる.
              2a'(V^2 - V0^2) + 2a(V'^2 - V0'^2) = L * 2a * 2a'
              2a'V^2 - 2a'V0^2 + 2aV'^2 - 2aV0^2 = L * 2a * 2a'
              2a'V^2 + 2aV'^2 = L * 2a * 2a' - 2a'V0^2 - 2aV0^2
              V^2(2a' + 2a) = L * 2a * 2a' - 2a'V0^2 - 2aV0^2
              V^2 = (L * 2a * 2a' - 2a'V0^2 - 2aV0^2) / (2a' + 2a)
            */
            float a_s = (w_max > w_start ? acceleration : -acceleration);
            float a_e = (w_max > w_end ? acceleration : -acceleration);
            float v = sqrtf(((L * 2 * a_s * 2 * a_e) + (2 * a_e * v_start * v_start) + (2 * a_s * v_end * v_end)) / (2 * a_e + 2 * a_s));
            t_start = fabs(v - w_start) / acceleration;
            t_end = fabs(v - w_end) / acceleration;
            _n1 = t_start;
            _n2 = 0;
            _n3 = _n1 + t_end;
        } else {
            _n1 = t_start;
            _n2 = _n1 + (L - l1 + l2) / w_max;
            _n3 = _n2 + t_end;
        }
        _v_start = v_start;
        _v_max = v_max;
        _v_end = v_end;
        _a_start = (v_max > v_start ? acceleration : -acceleration);
        _a_end = (v_max > v_end ? acceleration : -acceleration);
        _dt = T;
        _t = 0;
        cout << _n1 << '\t' << _n2 << '\t' << _n3 << '\t' << _v_start << '\t' << v_max << '\t' << _v_end << '\t' << _a_start << '\t' << _a_end << endl;
    }

    float next(void)
    {
        float v;
        if(_t < _n1) {
            v = _v_start + _t * _a_start;
            _t += _dt;
        } else if(_t < _n2) {
            v = _v_max;
            _t += _dt;
        } else if(_t < _n3) {
            v = _v_end + (_n3 - _t) * _a_end;
            _t += _dt;
        } else {
            v = _v_end;
        }
        return v;
    }

private:
    float _n1;
    float _n2;
    float _n3;
    float _v_start;
    float _v_max;
    float _v_end;
    float _a_start;
    float _a_end;
    float _t;
    float _dt;
};

int main(void){
    // Here your code !
    #if 1
    Ramp r0(  2,  10,   2, 0.5, 100, 0.5); // ok
    Ramp r1( 10,   5,  10, 0.5, 100, 0.5); // ok
    Ramp r2( -5, -10,  -5, 0.5, 100, 0.5); // ok
    Ramp r3(-10,  -5, -10, 0.5, 100, 0.5); // ok
    Ramp r4(  2,   5,  10, 0.5, 100, 0.5); // ok
    Ramp r5( -2,  -5, -10, 0.5, 100, 0.5); // ok
    #else
    Ramp r0(  2,  10,   2, 2, 100, 0.5); // ok
    Ramp r1( 10,   8,  10, 2, 100, 0.5); // ok
    Ramp r2( -2, -10,  -2, 2, 100, 0.5); // ok
    Ramp r3(-10,  -8, -10, 2, 100, 0.5); // ok
    Ramp r4(  2,   5,  10, 2, 100, 0.5); // ok
    Ramp r5( -2,  -5, -10, 2, 100, 0.5); // ok
    #endif

    for(int i=0; i<105; i++){
        cout << i << "\t" << r0.next() << "\t" << r1.next() << "\t" << r2.next() << "\t" << r3.next() << "\t" << r4.next() << "\t" << r5.next() <<endl;
    }
}