Ejemplo n.º 1
0
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 :)")
Ejemplo n.º 2
0
    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))
Ejemplo n.º 3
0
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)
Ejemplo n.º 4
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
Ejemplo n.º 5
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="/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