thym.go = 1 # Begin testbench pos_cam = False update_cam = False while thym.go: tps1 = time.monotonic() if thym.state == 'ASTOLFI': thym.ASTOLFI(th, Ts, filter, pos_cam, update_cam) elif thym.state == 'TURN': thym.TURN(th, Ts) elif thym.state == 'LOCAL': thym.LOCAL(th, Ts, filter, pos_cam) elif thym.state == 'INIT': thym.INIT(global_path, Init_pos) tps2 = time.monotonic() Ts = tps2 - tps1 print(thym.Pos) # check if we arrive at the goal if thym.p < 3 and thym.node == len(thym.global_path) - 2: th.set_var("motor.left.target", 0) th.set_var("motor.right.target", 0) print('FININSH!!!!') tfinal = time.monotonic() thym.go = 0