梯形加减速速度规划

/*+
 *Author zero.zhao
 *E-mail make9-11@126.com
 */
#include “open_loop_control/trapezoidal.h“
#include “cmath“
#include 
#include 
#include 

namespace trapezoidal
{
  Trapezoidal::Trapezoidal（）
  {
	  jerk_ = 2.0;
	  a_max_ = 1.0;
	  v_max_ = 0.95;
	  rate_ = 0.01;
  }

  Trapezoidal::~Trapezoidal（）
  {
  }
  
  void Trapezoidal::SetAcc（float &a_max） 
  {
	  a_max_ = a_max;
  }
  
  float Trapezoidal::GetAcc（void）
  {
	  return a_max_;
  }
  
  void Trapezoidal::SetVel（float &v）
  {
	  v_max_ = v;
  }
  
  float Trapezoidal::GetVel（void）
  {
	  return v_max_;
  }
  
  void Trapezoidal::SetRate（float &rate）
  {
	  rate_ = rate;
  }
  
  float Trapezoidal::GetReat（void）
  {
	  return rate_;
  }
  
  int Trapezoidal::GetCounts（void）
  {
    return T / rate_ + 1;
  }
  
  void Trapezoidal::CalculateTime（float &v_maxfloat &a_maxfloat &q0float &q1）
  {
    float h = q1 - q0;
    float s = std::pow（v_max2） / a_max;
    a_max_ = a_max;
    if（h - s > 0）
    {
      Ta = v_max / a_max;
      Td = Ta;
      T = h / v_max + Ta;
    }
    else
    {
      Ta = std::sqrt（h / a_max）;
      Td = Ta;
      T = 2 * Ta;
      v_max = a_max * Ta;
    }
     v_max_ = v_max;
     std::cout << “Ta : “ << Ta << std::endl;
     std::cout << “Td : “ << Td << std::endl;
     std::cout << “T  : “ << T  << std::endl;
     std::cout << “v_max : “ << v_max_ << std::endl;
  }
  
  
  float Trapezoidal::CalculateVel（float  &t）
  {
    float v = 0;
    if（t <= Ta）
    {
 //     v = v_max_ * t/Ta;
      v = a_max_ * t;
    }
    else if （t <= T - Td）
    {
      v = v_max_;
    }
    else if （t <= T）
    {
 //     v = v_max_ * （ T- t） / Td;
      v =  a_max_ * （ T- t）;
    }
    else
    {
      v = 0;
    }
    return v;
  }
  
  
  void Trapezoidal::CalculateTime（float &v0float &v1float &v_maxfloat &a_maxfloat &q0float &q1）
  {
    v0_ = v0;
    v1_ = v1;
    a_max_ = a_max;
    float h = q1 - q0;
    float judge = h * a_max  - v_max * v_max + （v0_ * v0_ - v1_ * v1_） *0.5;
    if（judge < 0）
    {
      //can‘t reach v_max
      v_max = std::sqrt（h * a_max + （v0_ * v0_ + v1_ * v1_） * 0.5 ）;
      Ta = （v_max - v0_） / a_max;
      Td = （v_max - v1_） / a_max;
      T = Ta + Td;
    }
    else
    {
      //can reach v_max
      Ta = （v_max - v0_） / a_max;
      Td = （v_max - v1_） / a_max;
      T = h / v_max + v_max / a_max * 0.5* std::pow（1 - v0_ / v_max 2） + \
          v_max / a_max * 0.5* std::pow（1 - v1_ / v_max 2）;
    }
    v_max_ = v_max;
    std::cout << “ --> judge : “ << judge << std::endl;
    std::cout << “Ta : “ << Ta << std::endl;
    std::cout << “Td : “ << Td << std::endl;
    std::cout << “T  : “ << T  << std::endl;
    std::cout << “ --> v_max : “ << v_max_ << std::endl;
  }
  
  float Trapezoidal::CalculateVel（float &v0float &v1float  &t）
  {
    float v = 0;
    if（t <= Ta）
    {
      v = v0 + （v_max_ - v0） * t / Ta;
    }
    else if（t <= T - Td）
    {
      v = v_max_;
    }
    else if（t <= T）
    {
      v = v1