-
Notifications
You must be signed in to change notification settings - Fork 0
/
drive_motor.py
137 lines (97 loc) · 2.54 KB
/
drive_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
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
132
133
134
135
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(pos_y):
# collect the object
seek(pos_y)
drop(160)
grab(90)
pick(160)
deseek(pos_y)
def drop_object_steps():
# drop the object
drop(150)
release(90)
pick(150)
def getPosX(x_center):
if x_center < 160:
pos_x = 90
elif x_center >= 160 and x_center < 320:
pos_x = 67
elif x_center >= 320 and x_center < 480:
pos_x = 45
elif x_center >= 480 and x_center < 640:
pos_x = 22
print(pos_x)
print(x_center)
return pos_x
def getPosY(y_center):
if y_center < 120:
pos_y = 22
elif y_center >= 120 and y_center < 240:
pos_y = 45
elif y_center >= 240 and y_center < 360:
pos_y = 67
elif y_center >= 360 and y_center < 480:
pos_y = 90
return pos_y
def collect_paper(x_center, y_center):
pos_x = getPosX(x_center)
pos_y = getPosY(y_center)
# approach an object
move_left(pos_x)
# collect the object
collect_object_steps(pos_y)
move_right(pos_x)
#drop the object
move_left(90)
drop_object_steps()
move_right(90)
def collect_metal(x_center, y_center):
pos_x = getPosX(x_center)
pos_y = getPosY(y_center)
# approach an object
move_left(pos_x)
# collect the object
collect_object_steps(pos_y)
move_right(pos_x)
#drop the object
move_left(135)
drop_object_steps()
move_right(135)
def collect_plastic(x_center, y_center):
pos_x = getPosX(x_center)
pos_y = getPosY(y_center)
# approach an object
move_left(pos_x)
# collect the object
collect_object_steps(pos_y)
move_right(pos_x)
#drop the object
move_left(180)
drop_object_steps()
move_right(180)