C++实现T型插补详解

这篇文章主要介绍了C++实现T型插补,本文给大家介绍的非常详细，对大家的学习或工作具有一定的参考借鉴价值，需要的朋友可以参考下

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