|
| 1 | +/* |
| 2 | + Double Pendulum |
| 3 | + |
| 4 | +A double pendulum implementation, uses |
| 5 | +RK4 to integrate each time step, which |
| 6 | +requires that the user have loaded |
| 7 | +rk4.bas into the library using |
| 8 | +LIBRARY SAVE prior to running this. |
| 9 | + |
| 10 | +The initial conditions and system |
| 11 | +parameters start after the function |
| 12 | +and subroutine definitions. |
| 13 | + |
| 14 | +*/ |
| 15 | + |
| 16 | +option angle radians |
| 17 | +option base 1 |
| 18 | +framebuffer create |
| 19 | +framebuffer write f |
| 20 | +cls rgb(black) |
| 21 | + |
| 22 | +'system of equations dy/dt=rhs(t,y) |
| 23 | +function rhs(t,y(),dy()) as integer |
| 24 | + local float o1=y(1) '\theta_1 |
| 25 | + local float w1=y(2) '\omega_1 |
| 26 | + local float o2=y(3) '\theta_2 |
| 27 | + local float w2=y(4) '\omega_2 |
| 28 | + |
| 29 | + local float del=o2-o1 |
| 30 | + local float den=(m1+m2)-m2*cos(del)^2 |
| 31 | + dy(1)=y(2) '\dot{\theta_1} = \omega_1 |
| 32 | + dy(2)=((m2*l1*w1^2)*sin(del)*cos(del)+ m2*g*sin(o2)*cos(del)+ m2*l2*w2^2*sin(del)- (m1+m2)*g*sin(o1))/(l1*den) |
| 33 | + dy(3)=y(4) |
| 34 | + dy(4)=(-m2*l2*w2^2*sin(del)*cos(del)+ (m1+m2)*(g*sin(o1)*cos(del)- l1*w1^2*sin(del) - g*sin(o2)))/(l2*den) |
| 35 | + |
| 36 | + rhs = 1 |
| 37 | +end function |
| 38 | + |
| 39 | +sub plot_pen x1,y1,x2,y2 |
| 40 | + static integer h = MM.VRES |
| 41 | + static integer w = MM.HRES |
| 42 | + static integer xo = w\2 |
| 43 | + static integer yo = h\2 |
| 44 | + static float scl = h/2/(l1+l2) |
| 45 | + static float rad = 10/(m1+m2) |
| 46 | + static integer r1 = fix(m1*rad) |
| 47 | + static integer r2 = fix(m2*rad) |
| 48 | + local integer px1 = fix(x1*scl)+xo |
| 49 | + local integer py1 = yo-fix(y1*scl) |
| 50 | + local integer px2 = fix(x2*scl)+xo |
| 51 | + local integer py2 = yo-fix(y2*scl) |
| 52 | + |
| 53 | + cls rgb(black) |
| 54 | + line xo,yo,px1,py1,1,rgb(green) |
| 55 | + circle px1,py1,r1,1,,rgb(blue),-1 |
| 56 | + line px1,py1,px2,py2,1,rgb(green) |
| 57 | + circle px2,py2,r2,1,,rgb(blue),-1 |
| 58 | + framebuffer copy f,n |
| 59 | +end sub |
| 60 | + |
| 61 | +'system parameters |
| 62 | +dim float g = 9.81'm/s^2 |
| 63 | +dim float l1 = 1.0 'm |
| 64 | +dim float m1 = 1.0 'kg |
| 65 | +dim float l2 = 1.0 'm |
| 66 | +dim float m2 = 1.0 'kg |
| 67 | + |
| 68 | +'initial conditions and integration |
| 69 | +dim float dt=0.05 |
| 70 | +dim float y(4) = (pi/2,0,pi/2,0) |
| 71 | +dim float th1,th2,x1,y1,x2,y2 |
| 72 | +dim integer f,i |
| 73 | +do |
| 74 | + f = rk4step("rhs",y(),0,dt) |
| 75 | + th1 = y(1) |
| 76 | + x1 = l1*sin(th1) |
| 77 | + y1 = -l1*cos(th1) |
| 78 | + th2 = y(3) |
| 79 | + x2 = x1+l2*sin(th2) |
| 80 | + y2 = y1-l2*cos(th2) |
| 81 | + plot_pen x1,y1,x2,y2 |
| 82 | +loop |
0 commit comments