class Controller:
def __init__(self, kp, ki, kd, limit, sigma, Ts, flag=True):
self.kp = kp # Proportional control gain
self.ki = ki # Integral control gain
self.kd = kd # Derivative control gain
self.limit = limit # The output saturates at this limit
self.sigma = sigma # dirty derivative bandwidth is 1/sigma
self.beta = (2.0 * sigma - Ts) / (2.0 * sigma + Ts)
self.Ts = Ts # sample rate
self.flag = flag
self.PID_object = PIDControl(self.kp, self.ki, self.kd, self.limit,
self.sigma, self.Ts, self.flag)
def update(self, r, y):
return self.PID_object.PID(r, y)
class Controller:
def __init__(self):
self.ctrl = PIDControl(P.kp, P.ki, P.kd, P.umax, P.sigma, P.Ts)
def update(self, r, y):
return self.ctrl.PID(r,y)
def update(self, r, y):
PID_object = PIDControl(self.kp, self.ki, self.kd, self.limit,
self.sigma, self.Ts, self.flag)
return PID_object.PID(r, y)
C = PIDControl(P.kp, P.ki, P.kd, P.umax, P.sigma, P.Ts, flag=True)
plant = System(P.K, P.tau, omega0, P.Ts)
integrator_RK4 = intg.get_integrator(P.Ts, System.f)
# Simulate step response
t_history = [t0]
x_history = [x0]
u_history = [0]
r = r0
x = np.array([theta0, omega0])
t = t0
for i in range(P.nsteps):
u = C.PID(r, x[0])
t += P.Ts
x = integrator_RK4.step(t, x, u)
t_history.append(t)
x_history.append(x)
u_history.append(u)
'''
# Plot response theta due to step change in r
plt.figure(figsize=(8,6))
plt.plot(t_history,x_history, label="$\omega$")
plt.xlabel("Time [s]")
plt.ylabel("Speed [rad/s]")
plt.legend()
plt.show()
# print(f'Final speed = {omega_history[-1]}')