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]}')