swingup.m

倒立摆模型建立

function F=swingup(u)

REF=-0.1;

F=REF;
if (abs(u(3))<0.2)||(abs(2*pi-u(3))<0.2)
F=0;

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

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

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

end