FirstBot/go_to_plot.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
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
import time

import pypot.dynamixel

from robot import *

################################ DESTINATION ################################

X = 1
Y = 1
THETA = 0 # - np.pi / 2

################################# VARIABLES #################################

FRAMERATE = 1 / 30

LINEAR_FACTOR = 0.4
ANGULAR_FACTOR = 2
DELTA = 0.03

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.enable_torque(ids)

#array of coordinates
coordinates = ([],[])

try:
    robot = Robot()
    current_time = time.time()
    initial_distance = np.sqrt(X ** 2 + Y ** 2)
    # initial_angle = np.arctan2(X + robot.x, Y - robot.y)
    # sign = 1
    # if abs(initial_angle) > np.pi / 2:
    #     sign = - 1
    #     if initial_angle > 0:
    #         initial_angle -= np.pi / 2
    #     else:
    #         initial_angle += np.pi / 2

    distance = initial_distance
    while distance > DELTA:
        distance = np.sqrt((X + robot.x) ** 2 + (Y - robot.y) ** 2)
        # print(distance)
        print("x, y :", robot.x, robot.y)
        v = LINEAR_FACTOR * distance

        angle = np.arctan2(X + robot.x, Y - robot.y) - robot.theta
        w = ANGULAR_FACTOR * angle
        print("angle :", angle)
        print("v, w :", v, w)

        w_l, w_r = inverse_kinematics(v, w)
        speed = {1: - w_l * 180 / np.pi, # * (initial_distance - distance),
                 2: w_r * 180 / np.pi} # * (initial_distance - distance)}
        # print("Speed :", speed)
        dxl_io.set_moving_speed(speed)

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

        robot.odom(v, w, dt)
        coordinates[0].append(robot.x)
        coordinates[1].append(robot.y)
        # print("x, y, theta :", robot.x, robot.y, robot.theta)
        time.sleep(FRAMERATE)

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

    plt.plot(coordinates[0],coordinates[1])
    plt.savefig('trace_go_to.png')

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