gain = GAIN_MIN + abs(int(KP*(pos[0] - ref_pos[0])))
if gain > GAIN_MAX:
gain = GAIN_MAX
if gain < GAIN_MIN:
gain = GAIN_MIN
print("Gain is "+str(gain));
roboroach._set_gain(gain)
saved_pos.append([round(elem,4) for elem in pos])
saved_ref.append([round(elem,4) for elem in ref_pos])
saved_gain.append(gain)
# Acting
if pos[0] - ref_pos[0] > POS_THRESH_POS:
roboroach._turn("right")
elif pos[0] - ref_pos[0] < POS_THRESH_NEG:
roboroach._turn("left")
time.sleep(0.01);
except KeyboardInterrupt:
pass
print("\nEnd of control")
print("Finishing tracker...")
polaris_driver.stopTracking()
polaris_driver.close()
time_now = strftime("%Y-%m-%d %H:%M:%S", gmtime())
np.savetxt('position.txt'+time_now, saved_pos)
np.savetxt('reference.txt'+time_now, saved_ref)
