milnesimpsonacc.m

来自「该程序用PID系统实现差动转向车辆控制」· M 代码 · 共 71 行

%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

function [T,Y,Theta]=MilneSimpsonAcc(t0,tmax,n,y_inn,ar,al,vr,vl,b,theta0,XoY)

if(XoY=='y')
    f='(A*t+B)*sin(C*t.^2+D*t+theta0)';
else
    f='(A*t+B)*cos(C*t.^2+D*t+theta0)';   
end

h=(tmax-t0)/n;
[l,m]=size(y_inn);      
v=zeros(1,m);
T=[t0:h:t0+n*h]';
Y=y_inn;
r=4;

Theta=[theta0];
for i=1:r
       vr=ar*i+vr;
   vl=al*i+vl;
   
   A=(ar+al)/2;
   B=(vr+vl)/2;
   C=(ar-al)/(2*b);
   D=(vr-vl)/b; 
   
   theta=C*i^2+D*i+theta0;
   Theta=[Theta;theta];
    t=T(i);y=Y(i,:);
	for j=1:m
   	    fun(i,j)=eval(f(j,:));
    end
end
fun=[fun;v];

pold=v;
cold=v;

coef1=[0 2 -1 2 0]*4/3;
coef2=[0 0 1 4 1]/3;
for i=r:n
       vr=ar*i+vr;
   vl=al*i+vl;
   
   A=(ar+al)/2;
   B=(vr+vl)/2;
   C=(ar-al)/(2*b);
   D=(vr-vl)/b; 
   
    theta=C*i^2+D*i+theta0;
    Theta=[Theta;theta];
   pnew=Y(i-3,:)+h*coef1*fun;
   %pmod=pnew;
   pmod=pnew+28*(cold-pold)/29;
   t=T(i+1);y=pmod;  
   for j=1:m
    	fun(r+1,j)=eval(f(j,:));
   end
   Y(i+1,:)=Y(i-1,:)+h*coef2*fun;             
   y=Y(i+1,:);
   for j=1:m
   	    fun(r+1,j)=eval(f(j,:));
   end
   fun=[fun(2,:);fun(3,:);fun(4,:);fun(5,:);v];
   pold=pnew;
   cold=Y(i+1,:);
end