项目开发日志——第二篇

根据第一篇日志以及整个团队的任务分工，这两天我主要完成轨迹规划算法及测试程序。

• 目前解决的问题

在团队项目仓库的drcs/include文件路径下，我们已经定义了一个TrajectoryPlanning类，它继承了已定义的RobotCommand类，而RobotCommand类继承了RobotSystem类。这部分工作主要是团队盛玉庭完成的，其他人慢慢优化。之前我个人一直使用Axis和Robot类，放在GitHub团队仓库的text_code文件夹下并把在Robot类中创建Axis类的两个对象，因为成员访问等问题，程序bug很多，也不好扩展。现在重新定义的类已完全包括原来的功能，但更便于后续程序扩展。

在TrajectoryPlanning类中，我定义了两个函数分别实现计算加速度、时间等参数和打印输出每个周期的轨迹。之前把两个功能放在一个函数中，那样显然不合理，因为出现了太多重复计算，这对于小系统而言可能无关痛痒，但在庞大的系统上必定会存在问题。

void Planning();
void LinearInterpolation();

在Planning()成员函数中，输入是目标位置、速度矢量、时间，当然还包括了初始点位置。输出则是，通过计算分别得到X和Y方向的实际加速度、加速时间、匀速时间等，并把值返回给类的数据成员。我们采取先匀加速后匀速的方法沿直线到达目标位置。在此我们统统假定加速度满足最大加速度的要求。代码如下：

void TrajectoryPlan::Planning(void)
{
	if (Request)
	{
		Uniform_motion_time_x = 2 * (targ_position_x - position_x) / targ_velocity_x - (targ_time / 1000.0);
		Acceleration_time_x = (targ_time / 1000.0) - Uniform_motion_time_x;
		ActualAccelerration_x = targ_velocity_x / Acceleration_time_x;

		std::cout << "ActualAccelerration_x : " << ActualAccelerration_x << std::endl;
		std::cout << "Acceleration_time_x : " << Acceleration_time_x << std::endl;
		std::cout << "Uniform_motion_time_x : " << Uniform_motion_time_x << std::endl;

		Uniform_motion_time_y = 2 * (targ_position_y - position_y) / targ_velocity_y - (targ_time / 1000.0);
		Acceleration_time_y = (targ_time / 1000.0) - Uniform_motion_time_y;
		ActualAccelerration_y = targ_velocity_y / Acceleration_time_y;

		std::cout << "ActualAccelerration_y : " << ActualAccelerration_y << std::endl;
		std::cout << "Acceleration_time_y : " << Acceleration_time_y << std::endl;
		std::cout << "Uniform_motion_time_y : " << Uniform_motion_time_y << std::endl;
	}
}

在LinearInterpolation()成员函数中，则相对简单，只需根据之前得到的结果，计算X和Y两个分量上的速度和位置。目前仅仅是没有时间周期的打印，接下来考虑用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;

	while (Request)
	{
		while (Acceleration_time_x - (Timer / 1000.0) + 0.0001 > 0)
		{
			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: 
";
			}

			Timer++;
		}
		Position_flag_x = Position_new_X;
		while ((Acceleration_time_x + Uniform_motion_time_x) - (Timer / 1000.0) + 0.0001 > 0)
		{
			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: 
";
			}
			Timer++;
		}
		if (Timer  == targ_time )
		{
	        Request = false;
			Done = true;
			position_x = targ_position_x;
		}
	}
}

运行截图（对于某一给定命令）：

• 待解决问题及下一阶段计划

由于编程基础较弱，目前边学边做，比如复杂类的创建等。且目前对Xenoami一些函数的使用经常会出错，要么程序不能运行要么运行后效果与目标差异很大。具体问题如下：
1. 用Xenomai为程序增加中断、定时器的功能，能运行成功
2. 为代码增加单元测试模块
3. 优化算法，优化类的设计