项目开发日志——第二篇

时间：2022-08-05 17:46:38

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

  • 目前解决的问题

在团队项目仓库的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\n";
            //再输出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\n";
                std::cout << "next cycle: \n";
            }
            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\n";
                std::cout << "next cycle: \n";
            }

            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\n";
            //再输出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\n";
                std::cout << "next cycle: \n";
            }
            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\n";
                std::cout << "next cycle: \n";
            }
            Timer++;
        }
        if (Timer  == targ_time )
        {
            Request = false;
            Done = true;
            position_x = targ_position_x;
        }
    }
}

运行截图（对于某一给定命令）：
项目开发日志——第二篇

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

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

标签：

相关文章