计算机科学论坛--[求助]运动插补程序

贴子主题： [求助]运动插补程序

举报打印推荐 IE收藏夹

本主题类别:

怎样用C语言改写下面代码:
原来网址:[URL=http://www.orocos.org/documentation/interpolation-api.html]

Figure 7 :

% initial position, velocity and acceleration:

p0 = 30; v0 = -2; a0 = -1;

% initial time:
t0 = 0;

% the profile has seven distinct phases, ending at time t7.
% final position and velocity:
p7 = 100;
v7 = 0;

% (Absolute values of) maximum velocity, acceleration and jerk:
vm = 7; am = 2; jm = 1;

niter = 8 % maximum number of iterations
dv=(vmax-v0)/niter;
d34=0;

% first try fast slow down to zero velocity:
vi=0;
[t1,t2,t3,j0,dist1]=trapacc_vel(vi,v0,a0,vmax,amax,jmax);
if (dist1>abs(dist))
error("\nInitial conditions and constraints do not allow solution!\n");
exit;
endif

% "else": use double pulse trajectory, starting with highest velocity
vi=vmax;

for i=1:niter
  % first pulse starts from beginning:
  [t1,t2,t3,j1,dist1]=trapacc_vel(vi,v0,a0,vi,amax,jmax);
  % second pulse starts from end, and runs "backwards":
  [dt45,dt56,dt67,j2,dist2]=trapacc_vel(vi,0,0,vi,amax,jmax);
  d34=abs(dist)-dist1-dist2;
  if (d34>0)
    % ok, gap to be filled by cruise with v=vi:
    t4=t3+d34/vi;
    t5=t4+dt45;
    t6=t4+dt56;
    t7=t4+dt67;
    break;
  else
    % not ok, still moving too far:
    vi=vi-dv;
  endif
endfor

非常感谢!!


	W 3 C h i n a ( since 2003 ) 旗下站点苏ICP备05006046号《全国人大常委会关于维护互联网安全的决定》《计算机信息网络国际联网安全保护管理办法》	78.125ms