-
Notifications
You must be signed in to change notification settings - Fork 0
/
test-motor.py
79 lines (57 loc) · 1.21 KB
/
test-motor.py
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
from adafruit_servokit import ServoKit
kit = ServoKit(channels=16)
import time
def move_forward(angle, channel):
for i in range(0, angle, 5):
kit.servo[channel].angle = i
time.sleep(0.04)
def move_backwards(angle, channel):
for i in reversed(range(0, angle, 5)):
kit.servo[channel].angle = i
time.sleep(0.04)
def move_left(angle):
move_forward(angle, 0)
def move_right(angle):
move_backwards(angle, 0)
def drop(angle):
move_forward(angle, 1)
def pick(angle):
move_backwards(angle, 1)
def seek(angle):
move_forward(angle, 2)
def deseek(angle):
move_backwards(angle, 2)
def grab(angle):
move_forward(angle, 4)
def release(angle):
move_backwards(angle, 4)
def collect_object_steps():
# collect the object
seek(90)
drop(150)
grab(90)
pick(150)
deseek(90)
def drop_object_steps():
# drop the object
drop(150)
release(90)
pick(150)
def collect_paper():
collect_object_steps()
move_left(90)
drop_object_steps()
move_right(90)
def collect_metal():
collect_object_steps()
move_left(135)
drop_object_steps()
move_right(135)
def collect_plastic():
collect_object_steps()
move_left(180)
drop_object_steps()
move_right(180)
#collect_paper()
#collect_metal()
#collect_plastic()