#23_dgl7_neu.py ***30.12.2020***
from numpy import sin,cos,radians,linspace
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
#Pendeldaten
g = 9.81    #Erdbeschleunigung
l1,l2 = 2,1 #Pendellaengen
m1,m2 = 5,1 #Pendelmassen
tmax=20
#DGL-System
def dgl(t,ya,l1, l2, m1, m2):
    phi1,w1,phi2,w2 = ya
    delta=phi2-phi1; m=m1+m2 #Abkuerzung fuer Winkel u. Masse
    phi1_dt = w1 #1. Ableitung Winkel oben
    w1_dt=(m2*l1*w1**2*sin(delta)*cos(delta)\
           +m2*g*sin(phi2)*cos(delta)+m2*l2*w2**2*sin(delta)-m*g*sin(phi1))\
           /(m*l1-m2*l1*cos(delta)**2)
    phi2_dt = w2 #1. Ableitung Winkel unten
    w2_dt=(-m2*l2*w2**2*sin(delta)*cos(delta)\
           + m*(g*sin(phi1)*cos(delta)-l1*w1**2*sin(delta)-g*sin(phi2)))\
           /(m*l2-m2*l2*cos(delta)**2)
    return [phi1_dt, w1_dt, phi2_dt, w2_dt]
#Anfangswerte
phi1 = 120
omega1 = 0
phi2 = 0
omega2 = 0
ti=[0,tmax]
ya =[radians(phi1),omega1,radians(phi2),omega2]
t = linspace(0,tmax,1000)
#Loesung des DGL-Systems
z=solve_ivp(dgl,ti,ya,args=(l1,l2,m1,m2),dense_output=True)
y=z.sol(t).T
#Berechnung der x,y-Koordinaten
#1.Pendel
x1 =  l1*sin(y[:,0])
y1 = -l1*cos(y[:,0])
#2.Pendel
x2 = x1+l2*sin(y[:,2])
y2 = y1-l2*cos(y[:,2])
#drei Unterdiagramme
fig,axes=plt.subplots(3,1,figsize=(8,12))
#Auslenkung l1
axes[0].set_title("Pendel 1")
axes[0].plot(t,x1,"r",lw=2)
axes[0].plot(t,y1,"b",lw=2)
axes[0].set_ylabel("$x_1$,$y_1$")
axes[0].grid(True)
#Auslenkung l2
axes[1].set_title("Pendel 2")
axes[1].plot(t,x2,'r',lw=2)
axes[1].plot(t,y2,'b',lw=2)
axes[1].set_xlabel("Zeit")
axes[1].set_ylabel("$x_2$,$y_2$")
axes[1].grid(True)
#Bahnkurven 
axes[2].set_title("Bahnkurven")
axes[2].plot(x1,y1,'r',lw=2,label="Pendel 1")
axes[2].plot(x2,y2,'b',lw=1,label="Pendel 2")
axes[2].set_xlabel("x")
axes[2].set_ylabel("y")
axes[2].legend(loc="best")
axes[2].grid(True)
fig.tight_layout()
plt.show()
'''
Quelle:
http://www.physics.usyd.edu.au/~wheat/dpend_html/
'''