def connexion_thymio(com): """ This function should be called in order to connect Thymio "Thymio is connected :)" will be sent once the connexion is successful """ global th th = Thymio.serial(port=com, refreshing_rate=0.1) time.sleep(10) # To make sure the Thymio has had time to connect print("Thymio is connected :)")
end, _ = get_video.match_template(warped, template, False) # end = np.array([1020, 200]).astype(int) # Read saved map image img = 'map2.jpg' gray = cv2.imread(img, cv2.IMREAD_GRAYSCALE) show_animation = True # Shows voronoi path planning process path = voronoi_road_map.get_path(gray, show_animation, start, end) ''' INITIALIZE THYMIO ''' th = Thymio.serial( port='COM5', refreshing_rate=0.1) #/dev/ttyACM0 for linux, #/dev/cu.usbmodem141101 my_th = Robot(th) my_th.set_position([start[0], start[1], np.rad2deg(pos[2])]) my_th.set_speed(100) # To make sure the Thymio has had time to connect time.sleep(3) variables = th.variable_description() print(variables[0]) ''' INITIALIZE VARIABLES ''' #initialize ekf variables xEst = np.zeros((5, 1))
from Thymio import Thymio import time th = Thymio.serial(port="COM7", refreshing_rate=0.1) time.sleep(10) # To make sure the Thymio has had time to connect print("Thymio is connected :)") th.set_var("motor.left.target", 100) th.set_var("motor.right.target", 100) for i in range(100): time.sleep(0.1) value_speed = [th['motor.left.speed'], th['motor.right.speed']] print(value_speed) th.set_var("motor.left.target", 0) th.set_var("motor.right.target", 0)
if state == 1: #go straigth Pos = pos_predict(Pos, NextPosition, Ts) thymio_straight(Pos, NextPosition) time.sleep(Ts) state = estimate_straight_state(Pos, NextPosition) if state == 0 and next < 4: #len(a) NextAngle = calculate_angle(a[next], a[next + 1]) print("turning") elif state == 0 and next == 4: #len(a) print("arrive at destination") th.set_var("motor.left.target", 0) th.set_var("motor.right.target", 0) return 0 x = np.array(thymio["prox.horizontal"]) if sum(x[0:4]) > 2000: # threshold a modifié #enter in avoid local obstacle mode Pos = avoid_obstacle(thymio, Angle, Pos, NextPosition, Ts) else: print("straight") a = [(0, 0), (50.0, 0), (50, 50), (0, 50), (0, 0)] #points for the direction Ts = 0.1 #sampling time th = Thymio.serial(port="\\.\COM3", refreshing_rate=0.1) time.sleep(3) # To make sure the Thymio has had time to connect Tymio_run(th, a, Ts) #test if it works
if state == 1: #go straigth Pos = pos_predict(Pos, NextPosition, Ts) thymio_straight(Pos, NextPosition) time.sleep(Ts) state = estimate_straight_state(Pos, NextPosition) if state == 0 and next < 4: #len(a) NextAngle = calculate_angle(a[next], a[next + 1]) print("turning") elif state == 0 and next == 4: #len(a) print("arrive at destination") th.set_var("motor.left.target", 0) th.set_var("motor.right.target", 0) return 0 x = np.array(thymio["prox.horizontal"]) if sum(x[0:4]) > 2000: # threshold a modifié #enter in avoid local obstacle mode Pos = avoid_obstacle(thymio, Angle, Pos, NextPosition, Ts) else: print("straight") a = [(0, 0), (50.0, 0), (50, 50), (0, 50), (0, 0)] #points for the direction Ts = 0.1 #sampling time th = Thymio.serial(port="/dev/cu.usbmodem141401", refreshing_rate=0.1) time.sleep(3) # To make sure the Thymio has had time to connect Tymio_run(th, a, Ts) #test if it works