a = -(B + k * k / R) / (J + m * r * r) b = k / ((J + m * r * r) * R) dt = 0.005 t = np.arange(0.0, 30.0, dt) x = np.zeros(len(t)) w = np.zeros(len(t)) control = np.zeros(len(t)) w_ref = 1.0 pid = PIDController(Kp, Kd, Ki, w_ref, w[0]) if __name__ == "__main__": for i in range(0, len(t) - 1): u = limit_control(pid.getControl(w[i], dt), 12) control[i] = u dx = w[i] * r dw = a * w[i] + b * u x[i + 1] = x[i] + dx * dt w[i + 1] = w[i] + dw * dt plot_x = pp.subplot(311) plot_x.title.set_text("Position") plot_x.grid(True) x_line, = pp.plot(t, x, 'r', label="x") plot_w = pp.subplot(312)
time = np.arange(0, 10, dt) Kp = 60.0 Kd = 14.0 Ki = 0.0 xKp = 2.0 xKd = 3.0 xKi = 0.0 pid_th = PIDController(Kp, Kd, Ki, 0.0, th[0]) pid_x = PIDController(xKp, xKd, xKi, 0.0, x[0]) for i in range(0, len(time) - 1): u = pid_th.getControl(th[i], dt) + pid_x.getControl(x[i], dt) f.append(-u) d2th = (g * sin(th[i]) - f[i] * cos(th[i])) / L dth1 = w[i] + d2th * dt v.append(v[i] + f[i] * dt) x.append(x[i] + v[i] * dt) th.append(th[i] + dth1 * dt) w.append(dth1) if i == len(time) / 2: x0 = 0.2 pid_x.setTarget(x0)
a = -(B + k * k / R) / (J + m * r * r) b = k / ((J + m * r * r) * R) dt = 0.05 t = np.arange(0.0, 30.0, dt) x = np.zeros(len(t)) w = np.zeros(len(t)) control = np.zeros(len(t)) x[0] = 0.1 pid = PIDController(Kp, Kd, Ki, 0.0, x[0]) if __name__ == "__main__": for i in range(0, len(t) - 1): u = limit_control(pid.getControl(x[i], dt), 24) control[i] = u dx = w[i] * r dw = a * w[i] + b * u x[i + 1] = x[i] + dx * dt w[i + 1] = w[i] + dw * dt plot_x = pp.subplot(311) plot_x.title.set_text("Position") plot_x.grid(True) x_line, = pp.plot(t, x, 'r', label="x") plot_w = pp.subplot(312)