1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
|
t1' = t1p
t2' = t2p
t2p' = -mu*t2p+1/(L2*(m1+m2*(sin(t2-t1))^2))*(-(m1+m2)*g*sin(t2)-(m1+m2)*\
L1*t1p^2*sin(t2-t1)+cos(t2-t1)*((m1+m2)*g*sin(t1)-m2*L2*t2p^2*sin(t2-t1)))
t1p' = -mu*t1p+1/(L1*(m1+m2*(sin(t2-t1))^2))*(-(m1+m2)*g*sin(t1)+\
m2*L2*t2p^2*sin(t2-t1)+cos(t2-t1)*(m2*g*sin(t2)+m2*L1*t1p^2*sin(t2-t1)))
# parameters
par L1=4,L2=1,m1=2,m2=1,g=9.8,mu=.01
@ total=200,bound=100000
init t1=1.5,t2=0
th1(x,y)=atan2(x-.5,.5-y)
th1dot(x,y,xp,yp)=(xp*(.5-y)+yp*(x-.5))/((x-.5)^2+(y-.5)^2)
th2(x,y)=atan2(x-.5-.2*sin(t1),-y+.5-.2*cos(t1))
th2dot(x,y,xp,yp)=(xp*(.5-.2*cos(t1)-y)+yp*(x-.5-.2*sin(t1)))/((.5-.2*cos(t1)-y)^2+(x-.5-.2*sin(t1))^2)
done
|
