linear motion very basic algorithm doesn't work

edit

Hi,

I am controlling a linear robot with it's acceleration. I wan't to go from a point p1 to the point p2, using the maximum acceleration I have which I call "a". With this acceleration, I can know how much time it takes to go from p1 to p2 ( time = sqrt((p2-p1)/a). So I wrote this :

    void My_package::accelerate(double a, double t)                                                                                                                  
     {                                                                                                                                                                
             geometry_msgs::Vector3 acc_cmd;                                                                                                                          

             ros::Duration delta_t = ros::Duration(t/2);                                                                                                              


            ros::Time beginTime = ros::Time::now();
            ros::Time t1 = beginTime + delta_t;                                                                                                                      
            ros::Time t2 = t1 + delta_t;                                                                                                                             
           //change the acceleration command.                                                                                                                       





          while(ros::Time::now() < t1)                                                                                                                                
          {                                                                                                                                                                                                                        
             acc_cmd.x = 0;                                                                                                                                   
             acc_cmd.y = a;                                                                                                                                   
             pub_acc_cmd_.publish(acc_cmd);                                                                                                                   
           }                                                                                                                                                        
          while((ros::Time::now() < t2) && (ros::Time::now() >= t1))                                                                                               
         {                                                                                                                                                        
                 acc_cmd.x = 0;                                                                                                                                   
                 acc_cmd.y = -a;                                                                                                                                  
                 pub_acc_cmd_.publish(acc_cmd);                                                                                                                   
         }                                                                                                                                                        

  }

But unfortunately this result in moving up and then mooving too much down and continue having much speed at the end.

How can I solve this problem?

Thank you for your help!