Updated the trapezoid velocity profile generation.

src/vel_trapezoid.cpp

@@ -20,54 +20,62 @@ void CVelTrapezoid::generate(double start_vel,double end_vel,double max_vel, dou
   this->set_end_vel(end_vel);
   this->set_max_vel(max_vel);
   this->set_max_acc(max_acc);
-  this->acc_time=(max_vel-start_vel)/max_acc;
-  this->dec_time=-(end_vel-max_vel)/max_acc;
-  this->acc_length=start_vel*this->acc_time+(max_vel-start_vel)*this->acc_time/2.0;
-  this->dec_length=end_vel*this->dec_time-(end_vel-max_vel)*this->dec_time/2.0;
+  if(start_vel==max_vel)
+    this->acc_time=0.0;
+  else
+    this->acc_time=fabs(max_vel-start_vel)/max_acc;
+  if(end_vel==max_vel)
+    this->dec_time=0.0;
+  else
+    this->dec_time=fabs(end_vel-max_vel)/max_acc;
+  this->acc_length=std::min(start_vel,max_vel)*this->acc_time+fabs(max_vel-start_vel)*this->acc_time/2.0;
+  this->dec_length=std::min(end_vel,max_vel)*this->dec_time+fabs(end_vel-max_vel)*this->dec_time/2.0;
   this->length=length;
   this->generated=true;
 }
 
 bool CVelTrapezoid::generate(double start_vel,double end_vel,double max_vel, double max_acc,double length,double &new_start_vel,double &new_max_vel)
 {
+  /*
   if(start_vel>max_vel)
     throw CException(_HERE_,"Start velocity must be smaller than the maximum velocity");
   if(end_vel>max_vel)
     throw CException(_HERE_,"End velocity must be smaller than the maximum velocity");
   if(max_acc<=0.0)
     throw CException(_HERE_,"Acceleration must be positive");
+  */
+  if(start_vel>max_vel)
+    start_vel=max_vel;
+  if(end_vel>max_vel)
+    end_vel=max_vel;
+  if(max_acc<=0.0)
+    max_acc=0.0;
   this->generate(start_vel,end_vel,max_vel,max_acc,length);
-  if(length-this->acc_length-this->dec_length>0.0)
+  if((length-this->acc_length-this->dec_length)>0.0)
   {
     this->time=(length-this->acc_length-this->dec_length)/this->max_vel+this->acc_time+this->dec_time;
     new_start_vel=this->start_vel;
     new_max_vel=this->max_vel;
     return true;
   }
-  else 
+  else
   {
-    double value=-pow(this->start_vel-this->end_vel,2.0)*pow(this->max_acc-1.0,2.0)-2.0*this->length*pow(this->max_acc,2.0)*(this->max_acc-1.0);
-    if(value>=0.0)
+    new_max_vel=sqrt(length*this->max_acc+(pow(start_vel,2.0)+pow(end_vel,2.0))/2.0);
+    if(new_max_vel>=start_vel)
     {
-      new_max_vel=((this->max_acc-1.0)*(this->start_vel+this->end_vel)-sqrt(value))/(2.0*(this->max_acc-1.0));
       this->generate(this->start_vel,this->end_vel,new_max_vel,this->max_acc,this->length);
       this->time=(length-this->acc_length-this->dec_length)/new_max_vel+this->acc_time+this->dec_time;
       new_start_vel=this->start_vel;
+      return true;
     }
     else
     {
-      value=-this->length*(this->max_acc-1.0);
-      if(value>0.0)
-      {
-        new_max_vel=((this->max_acc-1.0)*this->end_vel-this->max_acc*sqrt(value))/(this->max_acc-1.0);
-        new_start_vel=new_max_vel;
-        this->generate(new_start_vel,this->end_vel,new_max_vel,this->max_acc,length);
-        this->time=(this->length-this->dec_length)/new_max_vel+this->dec_time;
-      }
-      else
-        throw CException(_HERE_,"Impossible to create trapezoid");
+      new_max_vel=sqrt(pow(end_vel,2.0)+2*length*this->max_acc);
+      this->generate(this->start_vel,this->end_vel,new_max_vel,this->max_acc,this->length);
+      this->time=(length-this->acc_length-this->dec_length)/new_max_vel+this->acc_time+this->dec_time;
+      new_start_vel=this->start_vel;
+      return true;
     }
-    return false;
   }
 }