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)
pp.figure(1)
pp.subplot(211)
pp.title("PD regulator")
line_th, = pp.plot(time, th, label='th')
line_dth, = pp.plot(time, w, label='w')
pp.grid(True)
pp.legend([line_th, line_dth])
pp.subplot(212)
line_v, = pp.plot(time, v, label='v')
line_x, = pp.plot(time, x, label='x')
line_f, = pp.plot(time, f, label='u')
pp.grid(True)
pp.legend([line_v, line_x, line_f])
#PID1 = PIDController(P=-40, I=-100, D=-2.5)
#PID1 = PIDController(P=-42, I=-10, D=-5)
#PID1 = PIDController(P=-32, I=-1, D=-5)
#PID1 = PIDController(P=-25, I=-4.5, D=-5)
#PID1 = PIDController(P=-25, I=-0.04, D=-8)
#PID1 = PIDController(P=-15, I=-1, D=-1)
PID1 = PIDController(P=-13, I=-1, D=-1)
with open("./target_value.txt", "r") as file:
targetvalue = float(file.read())
PID1.setTarget(targetvalue)
while True:
try:
# forward(30)
# time.sleep(2)
# backward(30)
# time.sleep(2)
# stand_still()
# time.sleep(2)
accel_data = sensor.get_accel_data()
gyro_data = sensor.get_gyro_data()
ax = accel_data['x']
ay = accel_data['y']
az = accel_data['z']