项目开发日志——第三篇

这几天团队对代码整个做了一些局部优化，增加轨迹规划部分的算法流程图，为代码增加注释等工作。由于现在项目都是远程协作，一些问题都是队员通过网上交流讨论的。我个人主要用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。我认为这也证实了自己上述的结论。另外，这可能还与程序没有加入中断机制有关。

后续我将通过更换算法尝试解决这一问题。关于团队的其他进展请参考其他组员的博客。