cylin7.m

有用的动态仿真技术

function cylin3
clc;
clear all;
[t,Y] = ode45(@equ3,[0 10],[-10 0 0 0]);

subplot(2,2,1);
plot(t,Y(:,1));
title('x');
grid;
subplot(2,2,2);
plot(t,Y(:,2));
title('x dot');
grid;

subplot(2,2,3);
plot(t,Y(:,3)); 
title('teta');
grid;
subplot(2,2,4);
plot(t,Y(:,4)); 
title('teta dot');
grid;


function dy = equ3(t,y)
dy = zeros(4,1);

g=9.81;
T=1000000;
I0=10000;
muk=0;
l=20000000000000;
m0=10;
ro=m0/l;
r0=0.5;
R=1;
mic=0;
b=(R-r0)/m0;
h=2;



dy(1) = y(2);
dy(2) = (((abs(R*y(4)-y(2))<=mic)*(m*R*(T*(R*cos(alfa(r(r0,b,m),h,x)) + r(r0,b,m)) - dy(4)*((0.5*(-ro*r(r0,b,m)*y(4))*r(r0,b,m)^2 + m*r(r0,b,m)*(b*(-ro*r(r0,b,m)*y(4)))) + (-ro*r(r0,b,m)*y(4))*R^2)))/(m*R^2 + I) + (-ro*r(r0,b,m)*y(4))*R*dy(4) - T*cos(alfa(r(r0,b,m),h,x)))   +   (abs(R*y(4)-y(2))>mic)*(-muk*(m*g - T*sin(alfa(r(r0,b,m),h,x)))*sign(y(2)-R*y(4)))) + T*cos(alfa(r(r0,b,m),h,x))-(-ro*r(r0,b,m)*y(4))*y(2)/m;
dy(3) = y(4);
dy(4) = (T*r(r0,b,m) - ((abs(R*y(4)-y(2))<=mic)*(m*R*(T*(R*cos(alfa(r(r0,b,m),h,x)) + r(r0,b,m)) - dy(4)*(Id + (-ro*r(r0,b,m)*y(4))*R^2)))/(m*R^2 + I) + (-ro*r(r0,b,m)*y(4))*R*dy(4) - T*cos(alfa(r(r0,b,m),h,x)))   +   (abs(R*y(4)-y(2))>mic)*(-muk*(m*g - T*sin(alfa(r(r0,b,m),h,x)))*sign(y(2)-R*y(4))))*R - Id*y(4)/I;




function a = alfa(r,h,x)
a = atan2((r-1/2*h/(x^2+h^2)*(2*h*r - 2*sign(x)*(x^4-x^2*r^2+h^2*x^2)^(1/2)))/x, 1/2/(x^2+h^2)*(2*h*r - 2*sign(x)*(x^4-x^2*r^2+h^2*x^2)^(1/2)));

function f = friction(mic,x,tet,xd,tetd,m,R,r,h)
f = ((abs(R*tetd-xd)<=mic)*(m*R*(T*(R*cos(alfa(r,h,x)) + r) - dtetd*(Id + md*R^2)))/(m*R^2 + I) + md*R*tetdd - T*cos(alfa(r,h,x))   +   (abs(R*tetd-xd)>mic)*(-muk*(m*g - T*sin(atan2((r-1/2*h./(x^2+h^2).*(2*h*r - 2*sign(y(1)).*(x^4-x^2*r^2+h^2*x^2).^(1/2)))./y(1), 1/2./(x^2+h^2).*(2*h*r - 2*sign(y(1)).*(x^4-x^2*r^2+h^2*x^2).^(1/2)))))*sign(xd-R*tetd)));

function radiuse = r(r0,b,m)
radiuse = (r0+b*m);

function mass = m(m0,ro,b,tet)
mass = ((m0-ro^2*tet)/(1+b*ro*tet));

function massdot = md(m0,ro,b,tet,tetd)
massdot = (-ro*(r0+b*((m0-ro^2*tet)/(1+b*ro*tet)))*tetd);

function Inetia = I(I0,m0,ro,tet,b)
Inetia = (I0 + 0.5*((m0-ro^2*tet)/(1+b*ro*tet))*(r0+b*((m0-ro^2*tet)/(1+b*ro*tet)))^2);

function Inertiadot = Id(m0,ro,tet,b,tetd)
Inertiadot = (0.5*(-ro*(r0+b*((m0-ro^2*tet)/(1+b*ro*tet)))*tetd)*(r0+b*((m0-ro^2*tet)/(1+b*ro*tet)))^2 + ((m0-ro^2*tet)/(1+b*ro*tet))*(r0+b*((m0-ro^2*tet)/(1+b*ro*tet)))*(b*(-ro*(r0+b*((m0-ro^2*tet)/(1+b*ro*tet)))*tetd)))