try:
car = Car()
pid = PID(kp=1, ki=0.0, kd=0.1)
pid.SetExpectedOutput(15)
dist_list = []
min_speed = 30
FORWARD = True
while True:
##### perception ######
# ultrasonic sensing
dist_mov_ave = car.DistMeasureMovingAverage()
motor_speed = -pid.UpdateOutput(dist_mov_ave)
if motor_speed >= 0:
FORWARD = True
else:
FORWARD = False
motor_speed = min_speed + min(abs(motor_speed), 100 - min_speed)
if motor_speed < min_speed + 1:
motor_speed = 0
if FORWARD:
car.forward(motor_speed)
else:
car.back(motor_speed)
