实时控制软件机器人子系统
团队工作
- 针对机器人子系统对线性规划算法进行了改进
- 针对机器人子系统减速功能，在算法基础上添加了代码
减速功能
- 添加了减速功能
- 暂定减速为匀减速，减速度可以通过函数进行设定
代码
if (Timer > targ_time)
{
if ((Timer/1000.0) <= ((targ_time/1000.0) + Deceleration_time_x) || (Timer/1000.0) <= ((targ_time/1000.0) + Deceleration_time_y))
{
//减速
if (Velocity_new_X > Deceleration_x)
{
Velocity_new_X = Velocity_new_X - Deceleration_x*((Timer/1000.0) - (targ_time/1000.0));
Position_new_X = Position_new_X + 0.5*Deceleration_x*((Timer/1000.0) - (targ_time/1000.0))*((Timer/1000.0) - (targ_time/1000.0));
std::cout << "when t= " << Timer << " ms,Velocity_X= " << Velocity_new_X << " mm/s,Position_X= " << Position_new_X << " mm
";
if (Velocity_new_Y <= Deceleration_y)
{
Position_new_Y = Position_new_Y + 0.5*Velocity_new_Y*Velocity_new_Y / Deceleration_y;
Velocity_new_Y = 0;
std::cout << "when t= " << Timer << " ms,Velocity_Y= " << Velocity_new_Y << " mm/s,Position_Y= " << Position_new_Y << " mm
";
std::cout << "next cycle:
";
}
if (Velocity_new_Y > Deceleration_y)
{
Velocity_new_Y = Velocity_new_Y - Deceleration_y*((Timer/1000.0) - (targ_time/1000.0));
Position_new_Y = Position_new_Y + 0.5*Deceleration_y*((Timer/1000.0) - (targ_time/1000.0))*((Timer/1000.0) - (targ_time/1000.0));
std::cout << "when t= " << Timer << " ms,Velocity_Y= " << Velocity_new_Y << " mm/s,Position_Y= " << Position_new_Y << " mm
";
std::cout << "next cycle:
";
}
}
if (Velocity_new_X <= Deceleration_x)
{
Position_new_X = Position_new_X + 0.5*Velocity_new_X*Velocity_new_X / Deceleration_x;
Velocity_new_X = 0;
std::cout << "when t= " << Timer << " ms,Velocity_X= " << Velocity_new_X << " mm/s,Position_X= " << Position_new_X << " mm
";
if (Velocity_new_Y <= Deceleration_y)
{
Position_new_Y = Position_new_Y + 0.5*Velocity_new_Y*Velocity_new_Y / Deceleration_y;
Velocity_new_Y = 0;
std::cout << "when t= " << Timer << " ms,Velocity_Y= " << Velocity_new_Y << " mm/s,Position_Y= " << Position_new_Y << " mm
";
std::cout << "next cycle:
";
Request = false;
Done = true;
position_x = targ_position_x;
position_y = targ_position_y;
velocity_y = targ_velocity_x;
velocity_y = targ_velocity_y;
}
if (Velocity_new_Y > Deceleration_y)
{
Velocity_new_Y = Velocity_new_Y - Deceleration_y*((Timer / 1000.0) - (targ_time/1000.0));
Position_new_Y = Position_new_Y + 0.5*Deceleration_y*((Timer / 1000.0) - (targ_time/1000.0))*((Timer / 1000.0) - (targ_time/1000.0));
std::cout << "when t= " << Timer << " ms,Velocity_Y= " << Velocity_new_Y << " mm/s,Position_Y= " << Position_new_Y << " mm
";
std::cout << "next cycle:
";
}
}
}
}
参考
完整代码已经上传至github中，github仓库链接