/
main.py
118 lines (109 loc) · 3.46 KB
/
main.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
import sys
import time
from threading import Thread
from random import Random
from drive import Drive
from grab import Grab
from distance_detector import DistanceDetector
from color_detector import ColorDetector
class Main:
drive_proc = True
detect_boundary = True
detecting = True
detect_cube_found = False
continue_driving = True
def __init__(self, color):
self.random = Random()
self.drive = Drive()
self.grab = Grab()
self.dd = DistanceDetector()
self.cd = ColorDetector(color)
def turn_ninety_degrees(self):
self.drive.turn(Drive.DIRECTION_LEFT, 88)
def detect_outer_boundary(self):
while self.detecting:
while self.detect_boundary:
detect = False
if not self.detect_cube_found:
detect = self.cd.detect_boundary()
if detect:
break
time.sleep(0.01)
self.continue_driving = False
self.drive.stop()
time.sleep(1)
self.drive.reverse(70)
time.sleep(2)
self.drive.turn(Drive.DIRECTION_LEFT, self.random.randint(60, 180))
self.drive.drive_continuous()
self.continue_driving = True
# def drive_process(self):
# self.drive.drive_forward(275)
# time.sleep(1)
# self.turn_ninety_degrees()
# for i in range(250, 0, -20):
# if not self.drive_proc:
# break
# for j in range(1, 4):
# time.sleep(0.5)
# if not self.drive_proc:
# break
# self.drive.drive_forward(i)
# time.sleep(1)
# if not self.drive_proc:
# break
# self.turn_ninety_degrees()
# self.drive.stop()
def proc(self):
self.drive.drive_continuous()
self.dd.detect()
self.detect_cube_found = True
self.drive.stop()
time.sleep(0.5)
self.grab.lift()
time.sleep(1)
detected = False
attempts = 0
max_attempts = 6
while not detected:
detected = self.cd.detect()
if not self.continue_driving:
time.sleep(0.02)
detected = False
if detected:
break
else:
attempts += 1
if attempts >= max_attempts:
self.drive.reverse(20)
self.grab.sink()
time.sleep(2)
self.drive.turn(Drive.DIRECTION_RIGHT, 80)
self.detect_cube_found = False
return
self.drive.turn(Drive.DIRECTION_LEFT, 5)
time.sleep(0.5)
self.drive.reverse(5)
time.sleep(1)
self.grab.sink()
time.sleep(1)
self.drive.turn(Drive.DIRECTION_RIGHT, attempts * 5)
time.sleep(1)
self.drive.drive_continuous()
self.continue_driving = False
self.detect_cube_found = False
if __name__ == '__main__':
color = sys.argv[1]
m = Main(color)
try:
m.drive.turn(Drive.DIRECTION_LEFT, 40)
m.drive.drive_continuous()
t_detect = Thread(target=m.detect_outer_boundary)
t_detect.start()
while True:
if m.continue_driving:
m.proc()
time.sleep(0.05)
except KeyboardInterrupt:
m.drive.stop()
sys.exit()