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)