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)