-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathcontrol.py
84 lines (71 loc) · 2.58 KB
/
control.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
from collections import deque
import logging
import time
logger = logging.getLogger()
class Controller:
def __init__(self, num_green, window_width):
self.num_green = num_green
self.chase_green = True
self.last_radious = deque
self.window_width = window_width
self.captures = False
self.green_pop_count = 0
self.pop_until = 0
self.recent_pop = 0
self.key = "green"
self.detected = False
self.serching_count = 0
self.last_turn = "R"
def control(self, con, center_info):
self.control_motor(con, center_info[0], center_info[1])
if self.key == "green":
self.pop_detection(con, center_info[1])
def pop_detection(self, con, radius):
if radius is None:
radius = 0.0
if (not self.captures) and radius > self.window_width*0.4:
self.captures = True
self.pop_until = 10
logger.critical("balloon captured")
if (self.pop_until > 0):
self.pop_until -= 1
if self.pop_until == 0:
self.captures = False
if self.captures and radius < self.window_width*0.1:
# poped
self.captures = False
self.green_pop_count += 1
self.detected = False
logger.critical("%d-th balloon poped"%self.green_pop_count)
if self.green_pop_count == self.num_green:
self.key = "red"
def control_motor(self, con, center, radius):
center_threshold = 0.3 * (1.1 ** self.serching_count)
if center is None and (not self.detected):
con.write(b"S")
elif center is None and self.detected:
logger.info("enter reverse")
if self.last_turn == "R":
con.write(b"L")
logger.info("L")
else:
con.write(b"R")
logger.info("R")
self.serching_count += 1
elif center[0] + 0.25 * radius < self.window_width * (0.5 - center_threshold/2):
con.write(b"L")
self.detected = True
self.serching_count = 0
self.last_turn = "L"
elif center[0] - 0.25 * radius > self.window_width * (0.5 + center_threshold/2):
con.write(b"R")
self.detected = True
self.serching_count = 0
self.last_turn = "R"
else:
con.write(b"F")
self.detected = True
self.serching_count = 0
if self.serching_count > 7:
self.serching_count = 0
self.detected = False