工作日志（二）

个人编程作业——轨迹插补完善

轨迹插补中有两个函数模块——指令发送和轨迹生成。指令发送函数发送的数据有：

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;

以上是他们的初始状态。

---

算法原理：

1. 当循环次数达到5次时，将Request置1，即开始发送数据；
2. 轨迹生成函数得到Request为真的信息后，先将Response 置1，然后开始生成轨迹；
3. 要求生成梯形轨迹插补，其特点是Acceleration =Deceleration ，方向相反。
4. 已经从指令中得到目标位置Position，匀速时的速度Velocity，要求的加速度Acceleration及减速度Deceleration，那么轨迹插补时，以距离将插补分为三段：

令：
double p=new_cmd.Position;
double v=new_cmd.Velocity;
double a=new_cmd.Acceleration;
double d=new_cmd.Deceleration;

则
if(axis1_setpoint.Position < vv/(2a))，匀加速段；
else if(axis1_setpoint.Position < p - v*v/d)，匀速段；
else if(axis1_setpoint.Position < p)，匀减速段。

具体插补过程如下：

• 设置插补周期T；
• 在一个插补周期中认为是匀速运动，给出速度和目标位置；
• 当插补周期足够小时，插补轨迹无线接近于梯形轨迹。

代码实现：

double T=10;// 插补周期 
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; 


	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\n", 
		//        (long)(now - previous) / 1000000, 
		//        (long)(now - previous) % 1000000); 
		       previous = now; 

 
		//  Add your code 
		if(new_cmd.Request) 
		{ 
			new_cmd.Response = true; 
			double p=new_cmd.Position; 
			double v=new_cmd.Velocity; 
		 	double a=new_cmd.Acceleration; 
			double d=new_cmd.Deceleration; 
			// printf("p,v,a,d: %f.%f.%f.%f \n", 
			//   p, 
			//   v, 
			//   a, 
			//   d); 

			//accelerate   
			if(axis1_setpoint.Position < v*v/(2*a)) 
			{ 
				axis1_setpoint.Velocity += a*T; 
				axis1_setpoint.Position += axis1_setpoint.Velocity*T; 
				printf("Position1:%f\n",axis1_setpoint.Position); 
			} 

			//constant motion 
			 else if(axis1_setpoint.Position < p - v*v/d) 
			{ 
				axis1_setpoint.Velocity = v; 
				axis1_setpoint.Position += v*T; 
				printf("Position2:%f\n",axis1_setpoint.Position); 
			} 

		  	//Decelerate 
			else if(axis1_setpoint.Position < p) 
			{ 
				axis1_setpoint.Position += axis1_setpoint.Velocity*T; 
				axis1_setpoint.Velocity -= d*T; 
				printf("Position3:%f\n",axis1_setpoint.Position); 
			} 

			new_cmd.Done = true; 
		} 
	} 
}

当到达目标位置时，将Done置1，程序结束。

运行结果图：