forked from 1812598631/ARUN
-
Notifications
You must be signed in to change notification settings - Fork 0
/
judgetest-1.py
172 lines (135 loc) · 7.01 KB
/
judgetest-1.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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
import sensor, image, time
from pyb import ADC,Pin
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
#逆时针旋转将球送至射球 顺时针将球抛弃
#所有信号引脚全部高电平为静止 低电平运动
#分球射球逻辑：
# 分球识别颜色
# 识别颜色时 取识别的20次颜色中 三种颜色的最大值作为识别的最终颜色
# 确定颜色后 将确定的颜色信息传递给射球单元
# 射球单元调整炮台角度及转速 准备就绪后 将准备就绪的信号传递给分球结构
# 分球接收到信号执行分球动作
p_out_0 = Pin('P0', Pin.OUT_PP)#设置p_out为输出引脚 低电平逆时针旋转
p_out_1= Pin('P1', Pin.OUT_PP)#设置p_out为输出引脚 低电平顺时针旋转
p_out_2 = Pin('P2', Pin.OUT_PP)#设置p_out为输出引脚 单次低电平顺时针转一圈 将不要的球分至底盘
p_out_3 = Pin('P3', Pin.OUT_PP)#设置p_out为输出引脚 单次低电平逆时针转一圈 将想要的球送至射球弹仓
p_out_4 = Pin('P4', Pin.OUT_PP)#设置p_out为输出引脚 传给射球单元的黑/白信号引脚
p_out_5 = Pin('P5', Pin.OUT_PP)#设置p_out为输出引脚 传给射球单元的粉色信号引脚
p_in_7= Pin('P7', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻 Pin.PULL_DOWN 输入下拉电阻 根据抽签决定分球颜色
p_in_8= Pin('P8', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻 Pin.PULL_DOWN 输入下拉电阻 判断是否开始分球
p_in_9= Pin('P9', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻 Pin.PULL_DOWN 输入下拉电阻 判断是否开始分球判断射球是否就绪执行分球
p_out_0.high()#设置p_out引脚为高
p_out_1.high()#设置p_out引脚为高
p_out_2.high()#设置p_out引脚为高
p_out_3.high()#设置p_out引脚为高
p_out_4.high()#设置p_out引脚为高 传给射球单元黑/白的信号
p_out_5.high()#设置p_out引脚为高 传给射球单元粉色的信号
p=0
w=0
b=0
t=0
def find_initpoint():
adc = ADC("P6") # Must always be "P6". 获取灰度传感器的ADC引脚
location=(adc.read()) #获取灰度传感器传来的模拟量 OPENMV的模拟量最大值为4095
while location>800:
p_out_0.low() #低电平逆时针旋转
location=(adc.read())
print("location=%f"%location)
p_out_0.high() #步进电机停止旋转
def Senddata_Pink():
p_out_4.high()
#p_out_0.low() #步进电机停止旋转
while Ready_value==1:
p_out_0.high() #低电平逆时针旋转
p_out_0.low() #步进电机停止旋转
find_initpoint()
def Senddata_up():
p_out_4.low() #逆时针旋转送球
#p_out_3.high() #步进电机停止旋转
while Ready_value==1:#等待射球就绪
p_out_4.high() #低电平逆时针旋转
p_out_3.high() #步进电机停止旋转
find_initpoint()
def Senddata_down():
p_out_2.low() #顺时针旋转一圈掉球
#p_out_2.high() #步进电机停止旋转
while Ready_value==1:#等待射球就绪
p_out_2.high() #低电平逆时针旋转
p_out_2.high() #步进电机停止旋转
find_initpoint()
def biggest(a,b,c):
# 先比较a和b
if a>b:
maxnum = a
else:
maxnum = b
# 再比较maxnum和c
if c>maxnum:
maxnum=c
return maxnum
find_initpoint() #自定义函数 确定电机回到原点
sensor.set_auto_exposure(False,600) #设置曝光
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
sensor.set_saturation(1) #设置饱和度
clock = time.clock()
while(True):
clock.tick()
img = sensor.snapshot()
color_value = p_in_7.value() # get value, 0 or 1#读入p_in_7引脚的值 抽签确定的球的颜色 0为黑 1为白
judge_value = p_in_8.value() # get value, 0 or 1#读入p_in_8引脚的值 判断是否开始分球
Ready_value = p_in_9.value() # get value, 0 or 1#读入p_in_9引脚的值 射球是否就绪
if color_value == 1:
color_flag=1#白球
else :
color_flag=0#黑球
if judge_value == 1: #定位就绪开始分球射球
for c in img.find_circles(threshold = 1000, x_margin = 40, y_margin = 20, r_margin = 40,
r_min = 40, r_max = 60, r_step = 2):#r_step未明确作用 margin为需要合并的圆的大小及位置
area = (c.x()-c.r(), c.y()-c.r(), 2*c.r(), 2*c.r())
#img.draw_circle(c.x(), c.y(), c.r(), color = (255, 0, 0))#识别到的红色圆形用红色的圆框出来
#print(c.r()) 通过判断圆的半径大小 识别黑球与非球区域的差别
#area为识别到的圆的区域，即圆的外接矩形框
statistics = img.get_statistics(roi=area)#像素颜色统计
#print(statistics)
#l_mode()，a_mode()，b_mode()是L通道，A通道，B通道的众数。
if 60<statistics.l_mode()<80 and 50<statistics.a_mode()<80 and -50<statistics.b_mode()<-30:#if the circle is pink
img.draw_circle(c.x(), c.y(), c.r(), color = (255,0,255))#识别到的粉色圆形用粉色的圆框出来
p=p+1 #p:检测到的分球数量
t=t+1
elif 40<statistics.l_mode()<101 and -15<statistics.a_mode()<25 and -15<statistics.b_mode()<5:#if the circle is white
img.draw_circle(c.x(), c.y(), c.r(), color = (255, 255, 255))#识别到的白色圆形用白色的圆框出来
w=w+1 #w:检测到的白球数量
t=t+1
elif 0<statistics.l_mode()<20 and -10<statistics.a_mode()<0 and 0<statistics.b_mode()<5:#if the circle is white
img.draw_circle(c.x(), c.y(), c.r(), color = (0, 0, 255))#识别到的黑色圆形用蓝色的圆框出来
b=b+1 #b:检测到的黑球数量
t=t+1
if color_flag==1 and t>20:#抽签为白色 且检测颜色超过20次
possible_color=biggest(p,w,b)
if possible_color==p:
Senddata_Pink()
elif possible_color==w:
Senddata_up()
elif possible_color==b:
Senddata_down()
p=0 #计数清零
w=0
b=0
t=0
elif color_flag==0 and t>20:#抽签为黑色 且检测颜色超过20次
if possible_color==p:
Senddata_Pink()
elif possible_color==w:
Senddata_down()
elif possible_color==b:
Senddata_up()
p=0 #计数清零
w=0
b=0
t=0
else:
find_initpoint()