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