adamsbmacc.m

该程序用PID系统实现差动转向车辆控制

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

function [T,Y,Theta]=Adamsbm(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];

coef1=[-9 37 -59 55 0]/24;
coef2=[0 1 -5 19 9]/24;
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];
    p=Y(i,:)+h*coef1*fun;                    %predictor
    
    t=T(i+1);y=p;                            %evaluation
    for j=1:m
   	    fun(r+1,j)=eval(f(j,:));
    end
    Y(i+1,:)=Y(i,:)+h*coef2*fun;             %corrector
    
    y=Y(i+1,:);                              %evaluation
    for j=1:m
   	    fun(r+1,j)=eval(f(j,:));
    end
    fun=[fun(2,:);fun(3,:);fun(4,:);fun(5,:);v];
end