I have written a discrete-time simulation of a pendulum. It behaves fine when unforced, however I am trying to apply PI(D) control to make it move and reach a particular angle. When I do this and tune my gains, it is able to reach the angle but it won't stay there, and whatever I try seems to make it go wonky.
Beyond playing with the gains for ages, is there a more correct way of doing this PID control to achieve angle control?
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
m = 1.0 # mass (kg)
k = 0.0 # spring constant k_{s}
r = 1.0 # link length (m)
g = 9.81 # gravity term (m/s^{2})
damp = 1.0 # damping term (for unforced pendulum)
start_time = 0.0 # start time
end_time = 50.0 # end time
dt = 0.01 # time increment
num_steps = int((end_time - start_time) / dt)
t = np.linspace(start_time, end_time, num_steps)
theta = np.zeros_like(t)
thetadot = np.zeros_like(t)
thetaddot = np.zeros_like(t)
tau_u = np.zeros_like(t)
error = np.zeros_like(t)
# PID control gains
kp = 1.2
ki = 0.0
kd = 0.0
# ICs
theta[0] = -np.pi/2
thetadot[0] = 0.0
thetaddot[0] = 0.0
tau_u[0] = 0.0
target_angle = 0.0
error[0] = theta[0] - target_angle
for i in range(1, num_steps):
# mod term to keep angle between 0 and 2pi radians
if theta[i] > 2*np.pi:
theta[i] = theta[i] % 2*np.pi
error[i] = theta[i - 1] - target_angle #target_angle - theta[i - 1]
tau_u[i] = (kp * error[i - 1]) - (ki * np.sum(error) * dt) - (kd * thetadot[i - 1])
thetadot[i] = thetadot[i - 1] - ( ((3/(m*r**2))*tau_u[i]) +
(((3*k)/(m*r**2))*theta[i - 1]) +
((3*g*np.cos(theta[i - 1]))/(2*r)) )* dt
theta[i] = (theta[i - 1] + (thetadot[i] * dt) )
# plotting omitted for space
