swingup.asv

倒立摆模型建立

function F=swingup(u)

REF=16;

F=REF;
if (abs(u(3))<0.2)
F=0;

%elseif abs(pi-u(3))<pi/2
%F=-1*F;

elseif ((abs(u(4))<0.1)&&(pi-u(3)>0)&&(F>0))||(u(1)>9)
%F=-1*REF;

%elseif ((abs(u(4))<0.1)&&(pi-u(3)<0)&(F<0))||(u(1)<-9)
%F=REF;

%else
%end