linear motion very basic algorithm doesn't work
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!