实时控制软件设计第四周作业
一. 二轴桌面冰球游戏的软件设计方案与设计思路
1. 冰球游戏的介绍
以淘宝网某桌面冰球台实物为例，两个玩家通过手移动红色击打器（软件设计中我们通过二轴机械手控制）击打来自对方击回的红色球饼，目标是使球饼进入对方玩家的球洞中并得分。点击超链接可观看提供游戏玩法的视频。
2. 第三方库和工具的选择
首先，设计的游戏能够实现模拟物理碰撞的功能，目前我们考虑使用基于开源的ODE物理引擎。ODE (Open Dynamics Engine) 是一个免费的具有工业品质的刚体动力学的库，一款优秀的开源物理引擎，它能很好地仿真现实环境中的可移动物体，能很好地实现碰撞的仿真。它是快速，强健和可移植的。当然还有其他比较好的库，比如Opcode。
碰撞是指物体（object）间或物体与静止环境（static environment）之间因为接触力而导致物体运动变化的瞬间过程。碰撞由于是物体与物体、物体与环境接触产生的，因此碰撞的发生与当时物体的形状、位置，以及环境形状都有很大关系。
ODE典型的仿真过程：
1）生成一个动力学世界(dynamics world)
2）在该world中创建物体(bodies)
3）设置各body的状态（如质心位置、姿态等）
4）在上面的world中创建约束(joints)
5） 将约束关联到物体上
6） 为各约束设置参数值
7） 生成碰撞世界(collision world)，并为需要作碰撞检测的物体生成碰撞几何体
8） 生成用于容纳接触约束(contact joint)的约束组(joint group)
9） 循环过程
a. 在物体上施加力
b. 根据需要调整约束参数
c. 调用碰撞检测，得到碰撞点和碰撞的物体
d. 为每个碰撞点生成接触约束，并将其放入接触约束组中
e. 执行一个仿真步骤
f. 将接触约束组中的约束清空
10） 销毁dynamics world和collision world。
3. 模块和任务划分
完成整个游戏项目开发需要以下模块：
- ODE碰撞仿真及算法模块
- 二轴平面机械手控制模块
- 图像化界面模块
- 通信模块及其他
考虑到自身情况，我将重点放在实现机械手的控制和碰撞仿真的实现。
4. 关键算法分析
- 碰撞检测算法
常见算法有地图格子划分检测、矩形检测、圆形检测、像素检测和四叉树检测等。显然用圆形检测很适合我们的桌面冰球，优点是算法简单和碰撞精度相对较高，但是存在运算速度慢和效率低的缺点。
圆形碰撞就是判断两个圆是否发生碰撞，也就是判断两个圆圆心距离与两圆半径之和的大小关系，如下图所示，即判断是否满足√((x_1-x_2 )^2+(y_2-y_2 )^2 )<(r1+r2)2，满足则判断发生判断发生了碰撞。
- 碰撞处理算法
当检测到碰撞后，我们将根据所学物理学能量和动量关系，以及力学知识进行求解。
二. 编程作业
1. 轨迹生成器优化
由于上周曲解了题目意图，在这里对上周的任务做了纠正。结合上周的工作，在simple_motion.cpp中增加代码，实现轨迹打印功能，完整代码如下。
#include <stdio.h>
#include <signal.h>
#include <unistd.h>
#include <sys/mman.h>
#include <native/task.h>
#include <native/timer.h>
// Data Type
typedef struct
{
bool Request;
bool Response;
bool Done;
double Position;
double Velocity;
double Acceleration;
double Deceleration;
double Jerk;
} tPosCmd;
typedef struct
{
double Position;
double Velocity;
} tAxisSetpoint;
//Global variables
RT_TASK task_trajectory_generator;
RT_TASK task_command_sender;
tPosCmd new_cmd;
tAxisSetpoint axis1_setpoint;
int cycle_count = 0;
void task_trajectory_generator_proc(void *arg)
{
RTIME now, previous;
/*
* Arguments: &task (NULL=self),
* start time,
* period (here: 1 s)
*/
rt_task_set_periodic(NULL, TM_NOW, 1000000000);
previous = rt_timer_read();
axis1_setpoint.Position = 0;
axis1_setpoint.Velocity = 0;
int temp;
while (1) {
rt_task_wait_period(NULL);
now = rt_timer_read();
/*
* NOTE: printf may have unexpected impact on the timing of
* your program. It is used here in the critical loop
* only for demonstration purposes.
*/
printf("Task A Time since last turn: %ld.%06ld ms
",
(long)(now - previous) / 1000000,
(long)(now - previous) % 1000000);
previous = now;
// Add your code
double Acceleration_time;
double Deceleration_time;
double Uniform_motion_time;
RTIME time_start;
time_start = rt_timer_read();
if(new_cmd.Request)
{
Acceleration_time = new_cmd.Velocity/new_cmd.Acceleration;
Deceleration_time = new_cmd.Velocity/new_cmd.Deceleration;
Uniform_motion_time = (new_cmd.Position-Acceleration_time*new_cmd.Velocity)/new_cmd.Velocity;
printf("Acceleration_time : %f
",Acceleration_time);
printf("Deceleration_time : %f
",Deceleration_time);
printf("Uniform_motion_time : %f
",Uniform_motion_time);
bool flag = true;
while(flag)
{
int t1 = 0 ;
double Speed_new;
double S_new;
while(t1 <= Acceleration_time)
{
Speed_new = t1*new_cmd.Acceleration;
S_new = 0.5*new_cmd.Acceleration*t1*t1;
printf("t= %d ms时，速度v= %f mm/s,位置s= %fmm
",t1,Speed_new,S_new);
t1++;
}
double s=S_new;
while (t1 <= (Acceleration_time + Uniform_motion_time) )
{
Speed_new = Speed_new;
S_new = s + Speed_new*(t1 - Acceleration_time);
printf("t= %d ms时，速度v= %f mm/s,位置s= %fmm
",t1,Speed_new,S_new);
t1++;
}
double sp = Speed_new;
while ( t1 <= ( Acceleration_time + Uniform_motion_time + Deceleration_time ) )
{
Speed_new = sp - (t1 - Acceleration_time - Uniform_motion_time)*new_cmd.Deceleration;
S_new = new_cmd.Position - 0.5*new_cmd.Deceleration*(Acceleration_time + Uniform_motion_time + Deceleration_time - t1)*(Acceleration_time + Uniform_motion_time + Deceleration_time - t1);
printf("t= %d ms时，速度v= %f mm/s,位置s= %fmm
",t1,Speed_new,S_new);
t1++;
}
if(t1 > ( Acceleration_time + Uniform_motion_time + Deceleration_time ))
{
flag = false;
new_cmd.Response = true;
new_cmd.Done = true;
}
}
}
/*
if(new_cmd.Request)
{
new_cmd.Response = true;
new_cmd.Done = true;
}
*/
}
}
void task_command_sender_proc(void *arg)
{
RTIME now, previous;
/*
* Arguments: &task (NULL=self),
* start time,
* period (here: 2 s)
*/
rt_task_set_periodic(NULL, TM_NOW, 2000000000);
previous = rt_timer_read();
new_cmd.Request = false;
new_cmd.Response = false;
new_cmd.Done = false;
new_cmd.Position = 0;
new_cmd.Velocity = 0;
new_cmd.Acceleration = 0;
new_cmd.Deceleration = 0;
new_cmd.Jerk = 0;
while (1) {
rt_task_wait_period(NULL);
now = rt_timer_read();
/*
* NOTE: printf may have unexpected impact on the timing of
* your program. It is used here in the critical loop
* only for demonstration purposes.
*/
printf("Task B Time since last turn: %ld.%06ld ms
",
(long)(now - previous) / 1000000,
(long)(now - previous) % 1000000);
previous = now;
cycle_count = cycle_count + 1;
printf("cycle_count:%d
",cycle_count);
if(cycle_count == 5)
{
new_cmd.Request = true;
new_cmd.Response = false;
new_cmd.Done = false;
new_cmd.Position = 200000;
new_cmd.Velocity = 1000;
new_cmd.Acceleration = 50;
new_cmd.Deceleration = 50;
new_cmd.Jerk = 0;
}
}
}
void catch_signal(int sig)
{
}
int main(int argc, char* argv[])
{
signal(SIGTERM, catch_signal);
signal(SIGINT, catch_signal);
/* Avoids memory swapping for this program */
mlockall(MCL_CURRENT|MCL_FUTURE);
printf("A simple motion control demo
");
/*
* Arguments: &task,
* name,
* stack size (0=default),
* priority,
* mode (FPU, start suspended, ...)
*/
rt_task_create(&task_trajectory_generator, "rttask_trajectory_generator", 0, 98, 0);
rt_task_create(&task_command_sender, "rttask_command_sender", 0, 99, 0);
/*
* Arguments: &task,
* task function,
* function argument
*/
rt_task_start(&task_trajectory_generator, &task_trajectory_generator_proc, NULL);
rt_task_start(&task_command_sender, &task_command_sender_proc, NULL);
while(!new_cmd.Done);
printf("End!
");
rt_task_delete(&task_trajectory_generator);
rt_task_delete(&task_command_sender);
return 0;
}
运行结果截图：
2. c++头文件Axis.h及Axis.cpp
Axis.h代码（包含注释）如下：
#ifndef AXIS_H
#define AXIS_H
// "Axis_h"
struct Axis_state{ //状态
bool Request; //是否接受指指令
bool Done; //是否已完成运动指令
};
struct parameter{ //运动参数
double Position; //位置
double Velocity; //速度
double Acceleration; //加速度
double Deceleration; //减速度
};
struct new_cmd{ //接受命令
double Position;
double Velocity;
double Acceleration;
double Deceleration;
};
class Axis
{
public:
Axis_state State;
parameter Parameter;
new_cmd New_Cmd;
void init_Axis(Axis_state, parameter, new_cmd); //轴初始化函数
Axis_state Get_state(); //返回Axis对象的状态
parameter Get_parameter(); //返回Axis对象的参数
new_cmd Get_new_cmd(); //返回Axis对象的接受命令参数
void take_order(); //执行命令，并及时更新Axis对象
};
#endif
Axis.cpp代码（包含注释）如下：
#include "Axis.h"
#include <stdio.h>
void Axis::init_Axis(Axis_state init_state, parameter init_parameter, new_cmd init_new_cmd)
{
State = init_state;
Parameter = init_parameter;
New_Cmd = init_new_cmd;
}
Axis_state Axis::Get_state()
{
return State;
}
parameter Axis::Get_parameter()
{
return Parameter;
}
new_cmd Axis::Get_new_cmd()
{
return New_Cmd;
}
void Axis::take_order(new_cmd init_new_cmd)
{
double Acceleration_time;
double Deceleration_time;
double Uniform_motion_time;
if (State.Request)
{
Acceleration_time = Parameter.Velocity / New_Cmd.Acceleration;
Deceleration_time = Parameter.Velocity / New_Cmd.Deceleration;
Uniform_motion_time = (Parameter.Position - Acceleration_time*New_Cmd.Velocity) / New_Cmd.Velocity;
printf("Acceleration_time : %f
", Acceleration_time);
printf("Deceleration_time : %f
", Deceleration_time);
printf("Uniform_motion_time : %f
", Uniform_motion_time);
while (State.Request)
{
int t1 = 0;
double Speed_new;
double S_new;
while (t1 <= Acceleration_time)
{
Speed_new = t1*New_Cmd.Acceleration;
S_new = 0.5*New_Cmd.Acceleration*t1*t1;
printf("t= %d ms时，速度v= %f mm/s,位置s= %fmm
", t1, Speed_new, S_new);
t1++;
}
double s = S_new;
while (t1 <= (Acceleration_time + Uniform_motion_time))
{
Speed_new = Speed_new;
S_new = s + Speed_new*(t1 - Acceleration_time);
printf("t= %d ms时，速度v= %f mm/s,位置s= %fmm
", t1, Speed_new, S_new);
t1++;
}
double sp = Speed_new;
double ss = S_new;
while (t1 <= (Acceleration_time + Uniform_motion_time + Deceleration_time))
{
Speed_new = sp - (t1 - Acceleration_time - Uniform_motion_time)*New_Cmd.Deceleration;
S_new = ss + 0.5*New_Cmd.Deceleration*(t1 - Acceleration_time - Uniform_motion_time)*(t1 - Acceleration_time - Uniform_motion_time);
printf("t= %d ms时，速度v= %f mm/s,位置s= %fmm
", t1, Speed_new, S_new);
t1++;
}
if (t1 > (Acceleration_time + Uniform_motion_time + Deceleration_time))
{
State.Request = false;
State.Done = true;
}
}
}
}
参考资料：
[1] ODE典型的仿真过程：
http://blog.csdn.net/jimoshuicao/article/details/8148874
[2] 二维游戏碰撞检测算法的优化及应用：
http://www.doc88.com/p-7498877130679.html