cylin6.m

matlab中用于解决高等动力学hamilton系统的相关程序

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 br=gt(b,ro)
br=b*ro;

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)*(((m0-ro^2*y(3))/(1+gt(b,ro)*y(3)))*R*(T*(R*cos(atan2(((r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))-1/2*h./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))./y(1), 1/2./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))) + (r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))) - dy(4)*((0.5*(-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2 + ((m0-ro^2*y(3))/(1+b*ro*y(3)))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*(b*(-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4)))) + (-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))*R^2)))/(((m0-ro^2*y(3))/(1+b*ro*y(3)))*R^2 + (I0 + 0.5*((m0-ro^2*y(3))/(1+b*ro*y(3)))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2)) + (-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))*R*dy(4) - T*cos(atan2(((r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))-1/2*h./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))./y(1), 1/2./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))))   +   (abs(R*y(4)-y(2))>mic)*(-muk*(((m0-ro^2*y(3))/(1+b*ro*y(3)))*g - T*sin(atan2(((r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))-1/2*h./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))./y(1), 1/2./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))))*sign(y(2)-R*y(4)))) + T*cos(atan2(((r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))-1/2*h./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))./y(1), 1/2./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2))))-(-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))*y(2)/((m0-ro^2*y(3))/(1+b*ro*y(3)));
dy(3) = y(4);
dy(4) = (T*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - ((abs(R*y(4)-y(2))<=mic)*(((m0-ro^2*y(3))/(1+b*ro*y(3)))*R*(T*(R*cos(atan2(((r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))-1/2*h./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))./y(1), 1/2./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))) + (r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))) - dy(4)*((0.5*(-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2 + ((m0-ro^2*y(3))/(1+b*ro*y(3)))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*(b*(-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4)))) + (-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))*R^2)))/(((m0-ro^2*y(3))/(1+b*ro*y(3)))*R^2 + (I0 + 0.5*((m0-ro^2*y(3))/(1+b*ro*y(3)))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2)) + (-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))*R*dy(4) - T*cos(atan2(((r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))-1/2*h./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))./y(1), 1/2./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))))   +   (abs(R*y(4)-y(2))>mic)*(-muk*(((m0-ro^2*y(3))/(1+b*ro*y(3)))*g - T*sin(atan2(((r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))-1/2*h./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))./y(1), 1/2./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))))*sign(y(2)-R*y(4))))*R - (0.5*(-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2 + ((m0-ro^2*y(3))/(1+b*ro*y(3)))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*(b*(-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))))*y(4)/(I0 + 0.5*((m0-ro^2*y(3))/(1+b*ro*y(3)))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2);




%alfa = atan2(((r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))-1/2*h./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))./y(1), 1/2./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))
%f = ((abs(R*y(4)-y(2))<=mic)*(((m0-ro^2*y(3))/(1+b*ro*y(3)))*R*(T*(R*cos(atan2(((r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))-1/2*h./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))./y(1), 1/2./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))) + (r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))) - dy(4)*((0.5*(-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2 + ((m0-ro^2*y(3))/(1+b*ro*y(3)))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*(b*(-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4)))) + (-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))*R^2)))/(((m0-ro^2*y(3))/(1+b*ro*y(3)))*R^2 + (I0 + 0.5*((m0-ro^2*y(3))/(1+b*ro*y(3)))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2)) + (-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))*R*dy(4) - T*cos(atan2(((r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))-1/2*h./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))./y(1), 1/2./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))))   +   (abs(R*y(4)-y(2))>mic)*(-muk*(((m0-ro^2*y(3))/(1+b*ro*y(3)))*g - T*sin(atan2(((r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))-1/2*h./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))./y(1), 1/2./(y(1).^2+h^2).*(2*h*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3)))) - 2*sign(y(1)).*(y(1).^4-y(1).^2*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2+h^2*y(1).^2).^(1/2)))))*sign(y(2)-R*y(4))))

%m=((m0-ro^2*y(3))/(1+b*ro*y(3)))
%md=(-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))
%I=(I0 + 0.5*((m0-ro^2*y(3))/(1+b*ro*y(3)))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2)
%Id=(0.5*(-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))^2 + ((m0-ro^2*y(3))/(1+b*ro*y(3)))*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*(b*(-ro*(r0+b*((m0-ro^2*y(3))/(1+b*ro*y(3))))*y(4))))