# Written by:

# Kelly Roos

# Engineering Physics

# Bradley University

# rooster@bradley.edu | 309.677.2997

import numpy as np

import matplotlib.pyplot as plt

# Input parameters for model

g=9.8 # accel due to gravity (m/s^2)

m=1 # mass of pendulum in kg

l=1 # length of pendulum in meters

b=0.2 # damping strength (kg m^2/s)

omega=3 # driving frequency in rad/sec

tau_d=1 # driving torque in Nm

dt=0.01 # time step (s)

t_steps=100000 # total number of iterations

theta_i=120*3.14159/180 # convert to radians

theta_dot_i=0

# Defines the 1D arrays to be used in the computation and

# sets all values in the arrays to zero

time = np.zeros(t_steps)

theta = np.zeros(t_steps)

theta_dot = np.zeros(t_steps)

alpha = np.zeros(t_steps)

theta_sa = np.zeros(t_steps)

# Initial conditions

time[1] = 0

theta[1] = theta_i

theta_dot[1] = theta_dot_i

theta_sa[1] = theta_i*np.cos(np.sqrt(2*g/l)*time[1])

alpha[1] = -2*g*np.sin(theta[1])/l-4*b*theta_dot[1]/(m*l*l)+4*tau_d*np.cos(omega*time[1])/(m*l*l)

# Main loop: Euler algorithm, euler-Cromer, and evaluation of exact solutions for v and y

for i in range(2, t_steps):

time[i] = time[i-1] + dt

# Euler-Cromer

theta_dot[i]=theta_dot[i-1]+alpha[i-1]*dt

theta[i]=theta[i-1]+theta_dot[i]*dt

alpha[i]=-2*g*np.sin(theta[i])/l-4*b*theta_dot[i]/(m*l*l)+4*tau_d*np.cos(omega*time[i])/(m*l*l)

# small angle approximation

theta_sa[i]=theta_i*np.cos(np.sqrt(2*g/l)*time[i])

# constrain theta between -pi and +pi

if theta[i] > 3.14159265:

theta[i]=theta[i]-2*3.14159265

elif theta[i] < -3.14159265:

theta[i]=theta[i]+2*3.14159265

else:

theta[i]=theta[i]

# Plotting Results

#plt.plot(theta,theta_dot)

plt.plot(time,theta,time,theta_sa)

plt.ylim((-3, 3))

plt.xlim((0, 20))

plt.xlabel('time (s)')

plt.ylabel('ang. position (rad)')

plt.title('ang. position vs. time')

plt.show()