FirstBot/odometry.py at master · MarcDcls/FirstBot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
import time

import pypot.dynamixel

from robot import *

################################### TIMER ###################################

T = 20

#############################################################################

FRAMERATE = 1 / 120

ports = pypot.dynamixel.get_available_ports()
if not ports:
    exit('No port')

port = ports[0]
print('Using the first on the list', port)

dxl_io = pypot.dynamixel.DxlIO(port)
print('Connected!')

found_ids = dxl_io.scan([1, 2])
print('Found ids:', found_ids)

ids = found_ids[:2]
dxl_io.disable_torque(ids)

try:
    robot = Robot()
    current_time = time.time()

    i = 0
    j = 1
    while T > 0:
        i += 1
        w_r, w_l = dxl_io.get_present_speed(ids)
        w_r, w_l = - w_r * (np.pi / 180), w_l * (np.pi / 180)
        v, w = direct_kinematics(w_l, w_r)
        # print("SPEED :", w_r, w_l)

        dt = time.time() - current_time
        current_time += dt
        T -= dt

        robot.odom(v, w, dt)

        if i / 100 == j:
            j += 1
            # print("v, w :", v, w)
            print("X :", robot.x, "\nY :", robot.y, "\nTHETA :", robot.theta)

        time.sleep(FRAMERATE)

    # print("X :", X, "\nY :", Y, "\nTHETA :", THETA)

except KeyboardInterrupt:
    speed = {1:0, 2:0}
    dxl_io.set_moving_speed(speed)
    exit()