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