根据第一篇日志以及整个团队的任务分工，这两天我主要完成轨迹规划算法及测试程序。
- 目前解决的问题
在团队项目仓库的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. 优化算法，优化类的设计