CubeBot/test_controller.py at main · VGlukhov-git/CubeBot · 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
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
from models.arm import Arm
from devices.servo_uart import ServoUart
import time
import gpiod

servo_device = ServoUart()
A = 82.8 + 5
B = 48
C = 25
delta_a = 25
# delta_a = 0

LED1 = 17
LED2 = 27
LED3 = 22

LED_PINS = [LED1, LED2, LED3]

request = gpiod.request_lines(
    "/dev/gpiochip0",
    consumer="leds",
    config={
        pin: gpiod.LineSettings(
            direction=gpiod.line.Direction.OUTPUT
        )
        for pin in LED_PINS
    },
)


FrontLeftArm = Arm({
    "chA": 3,
    "chB": 4,
    "chC": 5,
    "delta_a": delta_a,
    "delta_b": 0,
    "device": servo_device,
    "A": A,
    "B": B,
    "C": C
})

FrontRightArm = Arm({
    "chA": 2,
    "chB": 0,
    "chC": 1,
    "delta_a": delta_a,
    "device": servo_device,
    "A": A,
    "B": B,
    "C": C,
    "invert": True
})

BackLeftArm = Arm({
    "chA": 11,
    "chB": 10,
    "chC": 12,
    "delta_a": delta_a,
    "device": servo_device,
    "A": A,
    "B": B,
    "C": C,
    "invert": True
})

BackRightArm = Arm({
    "chA": 13,
    "chB": 15,
    "chC": 14,
    "delta_a": delta_a,
    "device": servo_device,
    "A": A,
    "B": B,
    "C": C,
})

# FrontLeftArm.set_angle(90, 90, 180)
# FrontRightArm.set_angle(0, 140, 90)
# BackLeftArm.set_angle(0, 140, 90)
# BackRightArm.set_angle(0, 140, 90)
count = 0
# while count < 50:
    # count += 0.1
    # time.sleep(0.01)
# FrontLeftArm.set_coord(B, -A, 0)
# FrontLeftArm.set_angle(90, 90, 180)
# FrontLeftArm.set_coord(1, -40, 0)
# time.sleep(1)
count = 35

Z = 60
# FrontLeftArm.set_coord(count, -40, Z)
# FrontRightArm.set_coord(count, -40, Z)
# BackLeftArm.set_coord(count, -40, Z)
# BackRightArm.set_coord(count, -40, Z)

request.set_value(17, gpiod.line.Value.ACTIVE)
request.set_value(27, gpiod.line.Value.INACTIVE)
request.set_value(22, gpiod.line.Value.INACTIVE)
time.sleep(1)

request.set_value(17, gpiod.line.Value.INACTIVE)
request.set_value(27, gpiod.line.Value.ACTIVE)
request.set_value(22, gpiod.line.Value.INACTIVE)
time.sleep(1)

request.set_value(17, gpiod.line.Value.INACTIVE)
request.set_value(27, gpiod.line.Value.INACTIVE)
request.set_value(22, gpiod.line.Value.ACTIVE)
time.sleep(1)

# while count < 35:
#     FrontLeftArm.set_coord(count, -40, Z)
#     FrontRightArm.set_coord(count, -40, Z)
#     BackLeftArm.set_coord(count, -40, Z)
#     BackRightArm.set_coord(count, -40, Z)
#     count += 1
#     time.sleep(0.01)

count = 0
time.sleep(0.3)

while count < B:
    FrontLeftArm.set_coord(55, -40 - count, Z)
    FrontRightArm.set_coord(55, -40 - count, Z)
    BackLeftArm.set_coord(55, -40 - count, Z)
    BackRightArm.set_coord(55, -40 - count, Z)

    count += 1
    time.sleep(0.1)