def actuator(w, gain):
# Map turn rate -> motor commands. myro.motors(left, right)
if w >= 0:
u1 = 1
u2 = 1 - 2 * w
elif w == 0:
u1 = 1
u2 = 1
else:
u1 = 1 + 2 * w
u2 = 1
myro.motors(gain * u2, gain * u1)
def keydown(e):
# print('down', e.char)
key = e.char
if key == "q":
exit(0)
elif key == "w":
motors(5, 5)
elif key == "s":
motors(-5, -5)
elif key == "a":
motors(-5, 5)
elif key == "d":
motors(5, -5)
# Calculate target error. If updated, send to controller
new_error = getError(im)
if new_error != None:
e = new_error
elif e < 0:
e = -1
else:
e = 1
eps = 0.1
# Logic to consider change in control law
updateCLaw(t0, new_error)
if e < eps and e > -eps:
e = 0
myro.motors(1, 1)
continue
# Control logic and motor command
control(e)
# datasetlocation = "./Dataset/final_test_images/frontRoom%02d.jpg"
# imageWidth = 1280
# imageHeight = 800
# numParticles = 1000
# initialScale = 50
# predictionSigma = 150
#
# for i in range(11, 19):
# im = cv2.imread(datasetlocation%i)
# blobCenter = getError(im)
