(Q5) Suppose that the inverted pendulum is initially at equilibrium. Its parameters are given in (5.3). A unit impulse is applied to the system at t = 0. Choose the parameters of a PID controller so that the angle θ remains with [−5 ◦ , 5 ◦ ] and θ is below 0.1 ◦ after 0.4 s.

PendulumQuestion5.py

import sympy as sym
import control as ctrl
import numpy as np
import matplotlib.pyplot as plt
m, ell, x3, x4, M, g, F, m = sym.symbols('m, ell, x3, x4, M, g, F, m')
# φ(F, x3, x4)
phi = 4*m*ell*x4**2*sym.sin(x3) + 4*F - 3*m*g*sym.sin(x3)*sym.cos(x3)
phi /= 4*(M+m) - 3*m*sym.cos(x3)**2
dphi_x3 = phi.diff(x3)
dphi_x4 = phi.diff(x4)
dphi_F = phi.diff(F)

psi = -3*(m*ell*x4**2*sym.sin(x3)*sym.cos(x3) + F*sym.cos(x3) - (M+m)*g*sym.sin(x3))
psi /= (4*(M+m) - 3*m*sym.cos(x3)**2)*ell
dpsi_x3 = psi.diff(x3)
dpsi_x4 = psi.diff(x4)
dpsi_F = psi.diff(F)
# Equilibrium point
Feq = 0
x3eq = 0
x4eq = 0
dphi_F_eq = dphi_F.subs([(F, Feq), (x3, x3eq), (x4, x4eq)])
dphi_x3_eq = dphi_x3.subs([(F, Feq), (x3, x3eq), (x4, x4eq)])
dphi_x4_eq = dphi_x4.subs([(F, Feq), (x3, x3eq), (x4, x4eq)])

dpsi_F_eq = dpsi_F.subs([(F, Feq), (x3, x3eq), (x4, x4eq)])
dpsi_x3_eq = dpsi_x3.subs([(F, Feq), (x3, x3eq), (x4, x4eq)])
dpsi_x4_eq = dpsi_x4.subs([(F, Feq), (x3, x3eq), (x4, x4eq)])

a = dphi_F_eq
b = -dphi_x3_eq
c = 3/(ell*(4*M + m))
d = 3*(M+m)*g/(ell*(4*M + m))
# GIVEN VALUES!


def evaluate_at_given_parameters(z):
    M_value = 0.3
    m_value = 0.1
    g_value = 9.81
    ell_value = 0.35
    return float(z.subs([(M, M_value), (m, m_value), (ell, ell_value), (g, g_value)]))

a_value = evaluate_at_given_parameters(a)
b_value = evaluate_at_given_parameters(b)
c_value = evaluate_at_given_parameters(c)
d_value = evaluate_at_given_parameters(d)

# -----------------------------------
# Control library of Python
# -----------------------------------
transfer_function_F2x3=ctrl.TransferFunction([-c_value],[1,0,-d_value])

def pid(Kp, Ki, Kd):
    Gc = ctrl.TransferFunction([Kp], [1])
    Gc += ctrl.TransferFunction([Kd, 0], [1])
    Gc += ctrl.TransferFunction([Ki], [1, 0])
    return Gc

n_points = 500
t_final = 1
t_span = np.linspace(0, t_final, n_points)
transfer_function_pid = \
    -pid(Kp=200, Ki=0.2, Kd=8)
overall_tf = \
    ctrl.feedback(transfer_function_F2x3,transfer_function_pid)
t_imp, x3_imp = \
    ctrl.impulse_response(overall_tf, t_span)
angle_imp = \
    (x3_imp*(180/np.pi))

plt.plot(t_imp, angle_imp)
plt.xlabel('Time (s)')
plt.ylabel('Angle (degrees)')
plt.grid()
plt.savefig("Question5Angle(degrees)AgainstTime")
plt.show()