这几天团队对代码整个做了一些局部优化，增加轨迹规划部分的算法流程图，为代码增加注释等工作。由于现在项目都是远程协作，一些问题都是队员通过网上交流讨论的。我个人主要用Xenoami为轨迹输出部分增加了一个时间周期函数，目的是保证每单位ms有一个输出，以模拟脉冲信号，但是程序实现效果目前还不太理想。下面将做具体介绍。
首先，对个人“项目开发日志——第二篇”做一些补充。主要是：
-
函数void Planning( )算法流程图
-
函数void LinearInterpolation()算法流程图
下面介绍一下我用Xenomai实现定时器的功能。我直接在函数中增加了如下代码（用/* */标记的代码部分）：
void TrajectoryPlan::LinearInterpolation()
{
double Velocity_new_X, Velocity_new_Y;
double Position_new_X, Position_new_Y;
long Timer = 0;
double Position_flag_x, Position_flag_y = 0;
/*
bool flag;
RTIME time_start,time_now;
*/
while (Request)
{
while (Acceleration_time_x - (Timer / 1000.0) + 0.0001 > 0)
{
flag = true;
time_start = rt_timer_read();
Velocity_new_X = (Timer / 1000.0)*ActualAccelerration_x;
Position_new_X = 0.5*ActualAccelerration_x*(Timer / 1000.0)*(Timer / 1000.0);
std::cout << "when t= " << Timer << " ms,Velocity_X= " << Velocity_new_X << " mm/s,Position_X= " << Position_new_X<<" mm
";
//再输出Y
if (Acceleration_time_y - (Timer / 1000.0) + 0.0001 > 0)
{
Velocity_new_Y = (Timer / 1000.0)*ActualAccelerration_y;
Position_new_Y = 0.5*ActualAccelerration_y*(Timer / 1000.0)*(Timer / 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 (abs(Acceleration_time_y - (Timer / 1000.0)) < 0.0005)
{
Position_flag_y = Position_new_Y;
}
if (((Acceleration_time_y + Uniform_motion_time_y) - (Timer / 1000.0) + 0.0001 > 0) && (Acceleration_time_y - (Timer / 1000.0) + 0.0001 < 0))
{
Velocity_new_Y = Velocity_new_Y;
Position_new_Y = Position_flag_y + Velocity_new_Y*((Timer / 1000.0) - Acceleration_time_y);
std::cout << "when t= " << Timer << " ms,Velocity_Y= " << Velocity_new_Y << " mm/s,Position_Y= " << Position_new_Y << " mm
";
std::cout << "next cycle:
";
/*
//延时1ms
while(flag){
time_now = rt_timer_read();
if((long)(time_now - time_start) / 1000000 >=1)
flag = false;
}
/*
Timer++;
}
Position_flag_x = Position_new_X;
while ((Acceleration_time_x + Uniform_motion_time_x) - (Timer / 1000.0) + 0.0001 > 0)
{
/*
flag = true;
time_start = rt_timer_read();
/*
Velocity_new_X = Velocity_new_X;
Position_new_X = Position_flag_x + Velocity_new_X*((Timer / 1000.0) - Acceleration_time_x);
std::cout << "when t= " << Timer << " ms,Velocity_X= " << Velocity_new_X << " mm/s,Position_X= " << Position_new_X << " mm
";
//再输出Y
if (Acceleration_time_y - (Timer / 1000.0) + 0.0001 > 0)
{
Velocity_new_Y = (Timer / 1000.0)*ActualAccelerration_y;
Position_new_Y = 0.5*ActualAccelerration_y*(Timer / 1000.0)*(Timer / 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 (abs(Acceleration_time_y - (Timer / 1000.0)) < 0.0001)
{
Position_flag_y = Position_new_Y;
}
if (((Acceleration_time_y + Uniform_motion_time_y) - (Timer / 1000.0) + 0.0001 > 0) && (Acceleration_time_y - (Timer / 1000.0) + 0.0001 < 0))
{
Velocity_new_Y = Velocity_new_Y;
Position_new_Y = Position_flag_y + Velocity_new_Y*((Timer / 1000.0) - Acceleration_time_y);
std::cout << "when t= " << Timer << " ms,Velocity_Y= " << Velocity_new_Y << " mm/s,Position_Y= " << Position_new_Y << " mm
";
std::cout << "next cycle:
";
}
/*
//延时1ms
while(flag){
time_now = rt_timer_read();
if((long)(time_now - time_start) / 1000000 >= 1)
flag = false;
}
*/
Timer++;
}
if (Timer == targ_time )
{
Request = false;
Done = true;
position_x = targ_position_x;
}
}
}
程序通过简单的while语句和Xenomai的RTIME变量想达到的1ms的插补周期的效果，即在开始读取时间，进入while循环，不断读取新的时间，直到新的时间减去开始的时间大于或等于1ms，便结束循环，这种方法比较简单粗暴便不补充算法流程图了。而实际程序的运行效果显示，实际插补周期明显大于插补周期。我分析是否是在计算并打印轨迹那部分程序的运行时间是否超过1ms，当我把定时周期设置为1s（方法具体是将"if((long)(time_now - time_start) / 1000000 >= 1)"条件中不等式右边的1（单位：ms）改为1000（单位：ms））,经多次用手机计时器核对，程序运行时间与计时器始终同步，500s的累计误差不超过0.5s。我认为这也证实了自己上述的结论。另外，这可能还与程序没有加入中断机制有关。
后续我将通过更换算法尝试解决这一问题。关于团队的其他进展请参考其他组员的博客。