program DoublePend * Program to integrate the double pendulum equations * (modified double pendulum) * Ross Bannister, June 2001 implicit none integer Nt,t,Nout real l1,l2,l3,m1,m2,m3,g,dt,y(4),pi * Parameters (SI units) g=10. dt=0.002 pi=4.*atan(1.) * Number of time steps Nt=8000 * Output every how many timesteps? Nout=1 print*,'Please enter lengths l1,l2,l3' read*,l1,l2,l3 print*,'Please enter masses m1,m2,m3' read*,m1,m2,m3 * Initial conditions * Angle 1 y(1)=170.*pi/180. * Angle 2 y(2)=-120.*pi/180. print*,'Please enter initial r.o.c. angle 1, angle 2' print*,'(degrees per second)' read*,y(3),y(4) * Convert to radians * Rate of change angle 1 y(3)=y(3)*pi/180. * Rate of change angle 2 y(4)=y(4)*pi/180. open (12,file='Dpend2') write (12,100)'# Output from double pendulum version 2' write (12,102)'# Lengths : ',l1,l2,l3 write (12,102)'# Masses : ',m1,m2,m3 do t=1,Nt print*,t,Nt call RungeKutt4 (y,m1,m2,m3,l1,l2,l3,g,dt,pi) if (mod(t,Nout).eq.0) : write (12,101) real(t)*dt,180.*y(1)/pi, 180.*y(2)/pi, : 180.*y(3)/pi, 180.*y(4)/pi enddo close (12) 100 format ((A)) 101 format (5F12.2) 102 format ((A),3F8.4) end subroutine matrixA (y,m1,m2,m3,l1,l2,l3,A) * To determine the elements of matrix A implicit none real y(4),m1,m2,m3,l1,l2,l3,A(2,2) real th1,th2,al1,al2,term1,term2,term3 * Angle 1 th1=y(1) * Angle 2 th2=y(2) * Rate of change angle 1 al1=y(3) * Rate of change angle 2 al2=y(4) term1=l2*l3*m3*sin(th1-th2) term2=al2*term1 term3=al1*term1 A(1,1)=-1.*term2 A(1,2)=term2 A(2,1)=-1.*term3 A(2,2)=term3 return end subroutine matrixBinv (y,m1,m2,m3,l1,l2,l3,Binv) * To determine the elements of matrix B-inverse implicit none real y(4),m1,m2,m3,l1,l2,l3,Binv(2,2) real B(2,2),th1,th2,al1,al2,term1,det * Angle 1 th1=y(1) * Angle 2 th2=y(2) * Rate of change angle 1 al1=y(3) * Rate of change angle 2 al2=y(4) term1=m3*l2*l3*cos(th1-th2) * Construct the actual matrix B ... B(1,1)=m1*l1*l1+(m2+m3)*l2*l2 B(1,2)=term1 B(2,1)=term1 B(2,2)=m3*l3*l3 * ... but we need its inverse det=B(1,1)*B(2,2)-B(1,2)*B(2,1) Binv(1,1)=B(2,2)/det Binv(1,2)=-1.*B(1,2)/det Binv(2,1)=-1.*B(2,1)/det Binv(2,2)=B(1,1)/det return end subroutine vectorf (y,m1,m2,m3,l1,l2,l3,g,f) * To determine the elements of vector f implicit none real y(4),m1,m2,m3,l1,l2,l3,g,f(2) real th1,th2,al1,al2,term1,term2 * Angle 1 th1=y(1) * Angle 2 th2=y(2) * Rate of change angle 1 al1=y(3) * Rate of change angle 2 al2=y(4) term1=m3*l2*l3*al1*al2*sin(th1-th2) term2=sin(th1) f(1)=-1.*term1+g*term2*(m1*l1-(m2+m3)*l2) f(2)=term1-g*m3*l3*sin(th2) return end subroutine operate (A,Binv,f,dt,y,kk) * To perform operation k=Ty+g where T(A,Binv) and g(f) * (see notes) implicit none integer i,j,k real A(2,2),Binv(2,2),f(2),dt,y(4),kk(4) real Prod(2,2),total,vec(2) * Calculate -Binv A do i=1,2 do j=1,2 total=0. do k=1,2 total=total-Binv(i,k)*A(k,j) enddo Prod(i,j)=total enddo enddo * Calculate Binv f do i=1,2 total=0. do j=1,2 total=total+Binv(i,j)*f(j) enddo vec(i)=total enddo * Now operate kk(1)=dt*y(3) kk(2)=dt*y(4) kk(3)=dt*(Prod(1,1)*y(3)+Prod(1,2)*y(4)+vec(1)) kk(4)=dt*(Prod(2,1)*y(3)+Prod(2,2)*y(4)+vec(2)) return end subroutine sum (a,b,c,size,flag) * Vector sum c=a+b (flag=1) * Vector sum c=a+b/2 (flag=2) implicit none integer size,i,flag real a(size),b(size),c(size) if (flag.eq.1) then do i=1,size c(i)=a(i)+b(i) enddo else do i=1,size c(i)=a(i)+b(i)/2. enddo endif return end subroutine RungeKutt4 (y,m1,m2,m3,l1,l2,l3,g,dt,pi) * Fourth order Runge-Kutta for double pendulum implicit none integer i real y(4),m1,m2,m3,l1,l2,l3,g,dt,pi real k1(4),k2(4),k3(4),k4(4),A(2,2),Binv(2,2),f(2),ytemp(4),mymod * Stage 1 * ------- call matrixA (y,m1,m2,m3,l1,l2,l3,A) call matrixBinv (y,m1,m2,m3,l1,l2,l3,Binv) call vectorf (y,m1,m2,m3,l1,l2,l3,g,f) call operate (A,Binv,f,dt,y,k1) * Stage 2 * ------- call sum (y,k1,ytemp,4,2) call matrixA (ytemp,m1,m2,m3,l1,l2,l3,A) call matrixBinv (ytemp,m1,m2,m3,l1,l2,l3,Binv) call vectorf (ytemp,m1,m2,m3,l1,l2,l3,g,f) call operate (A,Binv,f,dt,ytemp,k2) * Stage 3 * ------- call sum (y,k2,ytemp,4,2) call matrixA (ytemp,m1,m2,m3,l1,l2,l3,A) call matrixBinv (ytemp,m1,m2,m3,l1,l2,l3,Binv) call vectorf (ytemp,m1,m2,m3,l1,l2,l3,g,f) call operate (A,Binv,f,dt,ytemp,k3) * Stage 4 * ------- call sum (y,k3,ytemp,4,1) call matrixA (ytemp,m1,m2,m3,l1,l2,l3,A) call matrixBinv (ytemp,m1,m2,m3,l1,l2,l3,Binv) call vectorf (ytemp,m1,m2,m3,l1,l2,l3,g,f) call operate (A,Binv,f,dt,ytemp,k4) * Add-up for incremented y-vector do i=1,4 y(i)=y(i) + k1(i)/6. + k2(i)/3. + k3(i)/3. + k4(i)/6. enddo * Put angles back into range (-pi to pi) do i=1,2 y(i)=mymod(y(i),2.*pi) if (y(i).gt.pi) y(i)=y(i)-2.*pi enddo return end real function mymod (x,interval) * This function puts x into the interval 0<=x