T型插补C++实现

此代码完全参考此作者的Matlab代码写得。对此作者表示感谢！https://blog.csdn.net/qq_26565435/article/details/94657852

Tspeed.h程序

#pragma once
#ifndef TSPEED_H
#define TSPEED_H

#include<iostream>
#include<fstream>
#include<cmath>

using namespace std;


void Tspeed();

#endif //TSPEED_H!

Tspeed.cpp程序

#include "Tspeed.h"

ofstream out("Tspeed.txt", ios_base::ate);
void Tspeed()
{
    double v0 = 50, v1 = 20, vmax = 150;
    double a_add = 1000, a_dec = -1500;
    double p0 = 5, p1 = 30;
    double t0 = 2;
    double h = p1 - p0;
    double vf, vv;
    double step = 0.001;
    vf = sqrt((2 * a_add * a_dec * h - a_add * pow(v1, 2) + a_dec * pow(v0, 2)) / (a_dec - a_add));
    if (vf < vmax)
    {
        vv = vf;
    }
    else
    {
        vv = vmax;
    }
    double Ta, Tv, Td;
    double La, Lv, Ld;
    Ta = (vv - v0) / a_add;
    Tv = (h - (pow(vv, 2) - pow(v0, 2)) / (2 * a_add) - (pow(v1, 2) - pow(vv, 2)) / (2 * a_dec)) / vv;
    Td = (v1 - vv) / a_dec;
    double t1;
    t1 = t0 + Ta + Tv + Td;
    La = v0 * Ta + 0.5 * a_add * pow(Ta, 2);
    Lv = vv * Tv;
    Ld = vv * Td + 0.5 * a_dec * pow(Td, 2);
    double p, pd, pdd;
    for (double t = 0; t <= Ta + Tv + Td;)
    {
        if (t >= 0 && t < Ta)
        {
            p = p0 + v0 * t + 0.5 * a_add * pow(t, 2);
            pd = v0 + a_add * t;
            pdd = a_add;
        }
        else if (t >= Ta && t < Ta + Tv)
        {
            p = p0 + La + vv * (t - Ta);
            pd = vv;
            pdd = 0;
        }
        else if (t >= Ta + Tv && t <= Ta + Tv + Td)
        {
            p = p0 + La + Lv + vv * (t - Ta - Tv) + 0.5 * a_dec * pow((t - Ta - Tv), 2);
            pd = vv + a_dec * (t - Ta - Tv);
            pdd = a_dec;
        }
        t = t + step;
        double temp = 2;
        temp = temp + t;
        cout << temp << "  " << p << "  " << pd << "  " << pdd << "  " << endl;
        out << temp << "  " << p << "  " << pd << "  " << pdd << "  " << endl;
        //cout << t << endl;
    }
    out.close();
}

main.cpp程序

#include"Tspeed.h"


int main()
{
    Tspeed();
    system("pause");
    return 0;
}

CMakeLists.txt程序

project(T_CHABU)
include_directories(include)
add_executable(main_cmake main.cpp src/Tspeed.cpp)

Matlab程序

A=load('Tspeed.txt');
t=A( :,1);
p=A( :,2);
pd=A( :,3);
pdd=A( :,4);

figure(2)
subplot(3, 1, 1)
plot(t, p, 'r', 'LineWidth', 1.5)
ylabel('position')
grid on
subplot(3, 1, 2)
plot(t, pd, 'b', 'LineWidth', 1.5)
ylabel('velocity')
grid on
subplot(3, 1, 3)
plot(t, pdd, 'g', 'LineWidth', 1.5)
ylabel('acceleration')
grid on