max_size=0
for blob in blobs:
if blob[2]*blob[3] > max_size:
max_blob=blob
max_size = blob[2]*blob[3]
return max_blob
while(True):
clock.tick() # Track elapsed milliseconds between snapshots().
img = sensor.snapshot() # Take a picture and return the image.
faces = img.find_features(face_cascade, threshold=0.5, scale=1.25)
if faces:
face = find_max(faces)
cx = int(face[0]+face[2]/2)
cy = int(face[1]+face[3]/2)
pan_error = cx-img.width()/2
tilt_error = cy-img.height()/2
print("pan_error: ", pan_error)
img.draw_rectangle(face) # rect
img.draw_cross(cx, cy) # cx, cy
pan_output=pan_pid.get_pid(pan_error,1)
tilt_output=tilt_pid.get_pid(tilt_error,1)
print("pan_output",pan_output)
pan_servo.angle(pan_servo.angle()+pan_output)
tilt_servo.angle(tilt_servo.angle()+tilt_output)
while (True):
clock.tick() # Track elapsed milliseconds between snapshots().
img = sensor.snapshot() # Take a picture and return the image.
blobs = img.find_apriltags()
if blobs:
max_blob = find_max(blobs)
x_error = max_blob[6] - img.width() / 2
h_error = max_blob[2] * max_blob[3] - size_threshold
print("x error: ", x_error)
'''
for b in blobs:
# Draw a rect around the blob.
img.draw_rectangle(b[0:4]) # rect
img.draw_cross(b[5], b[6]) # cx, cy
'''
img.draw_rectangle(max_blob[0:4]) # rect
img.draw_cross(max_blob[6], max_blob[7]) # cx, cy
x_output = x_pid.get_pid(x_error, 1)
h_output = h_pid.get_pid(h_error, 1)
print("h_output", h_output)
if -h_output - x_output < 0:
car.run(35, -h_output + x_output)
if -h_output + x_output < 0:
car.run(-h_output - x_output, 35)
#car.run(-h_output-x_output,-h_output+x_output)
else:
car.run(100, 100)
theta_err = line.theta()-180
else:
theta_err = line.theta()
img.draw_line(line.line(), color = 127)
print(rho_err,line.magnitude(),rho_err)
if line.magnitude()>8:
#if -40<b_err<40 and -30<t_err<30:
rho_output = rho_pid.get_pid(rho_err,1)
theta_output = theta_pid.get_pid(theta_err,1)
output = rho_output+theta_output
car.run(50+output, 50-output)
else:
car.run(0,0)
else:
car.run(50,-50)
frame_center = (frame.shape[1] / 2, frame.shape[0] / 2)
trackletsData = track.tracklets
for t in trackletsData:
roi = t.roi.denormalize(frame.shape[1], frame.shape[0])
x1 = int(roi.topLeft().x)
y1 = int(roi.topLeft().y)
x2 = int(roi.bottomRight().x)
y2 = int(roi.bottomRight().y)
horizontal_deviation = (frame_center[0]) - ((x2 - x1) / 2 + x1)
vertical_deviation = (frame_center[1]) - ((y2 - y1) / 2 + y1)
if str(t.id) == id:
count = count + 1 if old_horizontal_deviation == horizontal_deviation and old_vertical_deviation == vertical_deviation else 0
if count < 5:
if abs(horizontal_deviation) > 20:
pan = pan_pid.get_pid(horizontal_deviation, 1)
focuser.set(Focuser.OPT_MOTOR_X,focuser.get(Focuser.OPT_MOTOR_X) + int(pan))
if abs(vertical_deviation) > 15:
tilt = tilt_pid.get_pid(vertical_deviation, 1)
focuser.set(Focuser.OPT_MOTOR_Y,focuser.get(Focuser.OPT_MOTOR_Y) + int(tilt))
old_horizontal_deviation = horizontal_deviation
old_vertical_deviation = vertical_deviation
print("偏离中心:{0}, 图像中心坐标:{1}, 检测框中心坐标:{2}".format((horizontal_deviation, vertical_deviation), frame_center, (((x2 - x1) / 2 + x1), ((y2 - y1) / 2 + y1))))
elif key == ord('q'):
break
cv2.putText(frame, f"ID: {[t.id]}", (x1 + 10, y1 + 35), cv2.FONT_HERSHEY_TRIPLEX, 0.5, color)
cv2.rectangle(frame, (x1, y1), (x2, y2), color, cv2.FONT_HERSHEY_SIMPLEX)
cv2.putText(frame, "NN fps: {:.2f}".format(fps), (2, frame.shape[0] - 4), cv2.FONT_HERSHEY_TRIPLEX, 0.4, color)
#a += uart_out.write(bytes(cx_hexlist))
#print('send %d' %a)
#a = img.draw_image(reimg, 0, 0)
reline = reimg.get_regression([(50, 255)], x_stride=1)
if reline:
#a = img.draw_line(reline.line(), color = (0, 255, 0), thickness = 4)
#a = img.draw_line(40, 15, reline[0] + abs(reline[0] - reline[2]), 15, color = (255, 0, 0), thickness = 2)
rho_err = abs(reline.rho()) - 40
if reline.theta() > 90:
theta_err = reline.theta() - 180
else:
theta_err = reline.theta()
rho_u = rho_pid.get_pid(rho_err, 1)
theta_u = theta_pid.get_pid(theta_err, 1)
int_rho_u = struct.unpack('<I', struct.pack('<f', rho_u))[0]
int_theta_u = struct.unpack('<I', struct.pack('<f', theta_u))[0]
rho_u_hexlist = [
int_rho_u & 0xFF, (int_rho_u >> 8) & 0xFF,
(int_rho_u >> 16) & 0xFF, (int_rho_u >> 24) & 0xFF
]
theta_u_hexlist = [
int_theta_u & 0xFF, (int_theta_u >> 8) & 0xFF,
(int_theta_u >> 16) & 0xFF, (int_theta_u >> 24) & 0xFF
]
line_hexlist = [reline[0], reline[1], reline[2], reline[3]]
while (True):
clock.tick() # Track elapsed milliseconds between snapshots().
img = sensor.snapshot() # Take a picture and return the image.
blobs = img.find_blobs([green_threshold])
if blobs:
max_blob = find_max(blobs)
x_error = max_blob.cx() - img.width() / 2 #色块的外框的中心x坐标blob[5]
#h_error = max_blob[2]*max_blob[3]-size_threshold
#色块的外框的宽度blob[2],色块的外框的高度blob[3]
print("x error: ", x_error) #打印 x 轴误差 用于转弯
#print("h error: ", h_error) #打印 距离误差 用于速度
'''
for b in blobs:
# Draw a rect around the blob.
img.draw_rectangle(b[0:4]) # rect
img.draw_cross(b[5], b[6]) # cx, cy
'''
img.draw_rectangle(max_blob.rect()) # rect
img.draw_cross(max_blob.cx(), max_blob.cy()) # cx, cy
x_output = x_pid.get_pid(x_error, 1)
#h_output=h_pid.get_pid(h_error,1) #h_error调整后的值
print("x_output", x_output)
#print("h_output",h_output)
#run(-h_output-x_output,-h_output+x_output)
run(-x_output, x_output)
#print(-h_output-x_output,-h_output+x_output)
else:
run(0, 0)
fifoCount -= packetSize
fifoBuffer = mpu.getFIFOBytes(packetSize)
yaw, rol, pit = mpu.dmpGetEuler(*mpu.dmpGetQuaternion(fifoBuffer))
g_pit, g_rol, g_yaw = mpu.dmpGetGyro(fifoBuffer)
yaw -= 6
#print(rol, pit, yaw, g_rol, g_pit, g_yaw)
# RC
#print('%s %s %s %s' % (rc_thr.get_width(), rc_rol.get_width(), rc_pit.get_width(), rc_yaw.get_width()))
rc_thr_width = rc_thr.get_width()
rc_rol_width = _map(rc_rol.get_width(), 995, 1945, -45, 45)
rc_pit_width = _map(rc_pit.get_width(), 995, 1945, -45, 45)
rc_yaw_width = _map(rc_yaw.get_width(), 995, 1945, -180, 180)
#print('%s %s %s' % (rc_rol_width, rc_pit_width, rc_yaw_width))
if rc_thr_width > 1200:
# Stablise PIDS
rol_stab_out = max(min(rs_pid.get_pid(rc_rol_width + rol, 1), 250),
-250)
pit_stab_out = max(min(ps_pid.get_pid(rc_pit_width + pit, 1), 250),
-250)
yaw_stab_out = max(
min(ys_pid.get_pid(wrap_180(yaw_target + yaw), 1), 360), -360)
#print('%s %s %s' % (rol_stab_out, pit_stab_out, yaw_stab_out))
if abs(rc_yaw_width) > 5:
yaw_stab_out = rc_yaw_width
yaw_target = yaw
# rate PIDS
rol_out = max(min(rr_pid.get_pid(rol_stab_out + g_rol, 1), 500), -500)
pit_out = max(min(pr_pid.get_pid(pit_stab_out + g_pit, 1), 500), -500)
yaw_out = max(min(yr_pid.get_pid(yaw_stab_out + g_yaw, 1), 500), -500)
#print('%s %s %s' % (rol_out, pit_out, yaw_out))
# ESC
blobs = img.find_blobs([threshold], pixels_threshold=250, area_threshold=250, merge=True, margin=10)
if blobs:
blob = find_max(blobs)
roi = blob[0:4]
if roi is not None:
_cx_=roi[0]+roi[2]//2
_cy_=roi[1]+roi[3]//2
img.draw_rectangle(roi)
img.draw_cross(_cx_,_cy_)
#添加滤波列表
cx_array.append(_cx_)
cy_array.append(_cy_)
h_array.append(roi[2])
#调用滤波函数,可加权重
cx_ = recursion_filter(cx_array,0.2)
cy_ = recursion_filter(cy_array,0.2)
h_area = recursion_filter(h_array)
#获取PID输出
x_error = cx_ - img.width()//2
y_error = cy_ - img.height()//2
h_error = h_area - h_threshold
x_out = cx_pid.get_pid(x_error,-1)
y_out = cy_pid.get_pid(y_error,1)
h_out = h_pid.get_pid(h_error,1)
#获取动作字典
actions=myarm.action(x_out,y_err=y_out,h_err=h_out)
print(roi[2],h_threshold,h_out)
#执行动作
myarm.execute(actions)
h_threshold = h_area
class NXP_Motor:
def __init__(self, port):
self.i2c = i2c_bus.get(i2c_bus.M_BUS)
self._available()
self.port = port - 1
self.prev_enc_val = 0
self.interval = 0
self.sample_interval = 10
self.enc_zero = 0
self.input_value = 0
self.speed_pid = PID(p=50, i=0, d=60)
self.speed_point = None
self.angle_pid = PID(p=8, i=0, d=30)
self.angle_point = None
self.angle_max_pwm = 255
self.encode_zero = 0
self.encode_clear()
def _available(self):
if self.i2c.is_ready(LEGO_BOARD_I2C_ADDR) or self.i2c.is_ready(
LEGO_BOARD_I2C_ADDR):
pass
else:
raise module.Module("Lego module maybe not connect")
def reset(self):
self.stop()
self.encoder_incr()
def stop(self):
self.set_pwm(0)
self.angle_point = None
self.speed_point = None
def brake(self):
# self.set_pwm(0)
self.set_speed(0)
def write(self, value):
self.angle_point = None
self.speed_point = None
self.set_pwm(value)
def read(self):
return self.encoder_read()
def encoder_incr(self):
enc_val = self.encoder_read()
incr_val = enc_val - self.prev_enc_val
self.prev_enc_val = enc_val
return incr_val
def set_pwm(self, pwm):
_pwm = constrain(pwm, -255, 255)
buf = ustruct.pack('<h', int(_pwm))
self.i2c.writeto_mem(LEGO_BOARD_I2C_ADDR,
MOTOR_CTRL_ADDR + (self.port * MOTOR_CTRL_LEN),
buf)
def encoder_read(self):
return self._encoder_read() - self.encode_zero
def encode_clear(self):
self.encode_zero = self._encoder_read()
def _encoder_read(self):
buf = bytearray(4)
self.i2c.readfrom_mem_into(
LEGO_BOARD_I2C_ADDR,
ENCODER_READ_ADDR + (self.port * ENCODER_READ_LEN), buf)
return (tuple(ustruct.unpack('<l', buf))[0])
def set_speed(self, speed):
# 0-27
# self.speed_point = speed / (1000/self.sample_interval*60)
self.angle_point = None
self.speed_point = speed
self.encoder_incr()
def set_angle_zero(self):
self.enc_zero = self.encoder_read()
self.input_value = 0
def set_angle(self, angle, max_pwm=255):
self.speed_point = None
self.angle_point = angle * 2
self.angle_pid._min_output = -max_pwm
self.angle_pid._max_output = max_pwm
def update(self):
if time.ticks_us() >= self.interval:
self.interval = time.ticks_us() + 10000
if not self.angle_point == None:
self.input_value = self.encoder_read() - self.enc_zero
output_value = self.angle_pid.get_pid(self.input_value,
self.angle_point)
self.set_pwm(output_value)
if not self.speed_point == None:
# Encoder filter
self.input_value *= 0.75
self.input_value += self.encoder_incr() * 0.25
# self.input_value = constrain(self.input_value, -30, 30)
# Output pwm
output_value = self.speed_pid.get_pid(self.input_value,
self.speed_point)
self.set_pwm(output_value)
# print("in:%0.2f, out:%0.2f\r\n" % (self.input_value, output_value))
def update_no_block(self):
if not self.angle_point == None:
self.input_value = self.encoder_read() - self.enc_zero
output_value = self.angle_pid.get_pid(self.input_value,
self.angle_point)
print(self.input_value - self.angle_point, end="\t")
print(output_value)
self.set_pwm(output_value)
if not self.speed_point == None:
# Encoder filter
self.input_value *= 0.75
self.input_value += self.encoder_incr() * 0.25
# self.input_value = constrain(self.input_value, -30, 30)
# Output pwm
output_value = self.speed_pid.get_pid(self.input_value,
self.speed_point)
self.set_pwm(output_value)
def test(self):
while True:
self.update()
self.set_pwm(0)
def deinit(self):
pass
pass
ti = time_counter(ti)
print('找球ti=', ti)
if ti > 600:
car_adjust()
p = 1
break
max_blob = find_max(blobs)
x1 = max_blob[0] - 5
y1 = max_blob[1] - 5
w1 = max_blob[2] + 10
h1 = max_blob[3] + 10
area = (x1, y1, w1, h1)
x_error = max_blob[5] - 80
y_error = max_blob[3] - ball_high
x_output = x_pid.get_pid(x_error, 1)
y_output = y_pid.get_pid(y_error, 1)
img.draw_rectangle(max_blob[0:4]) # rect
img.draw_cross(max_blob[5], max_blob[6]) # cx, cy
print('x_error,y_error,max_blob[3]', x_error, y_error, max_blob[3])
if 30 > max_blob[3] > 15:
x_range = 30
elif max_blob[3] >= 30:
x_range = 20
else:
x_range = 40
pass
if x_error < -x_range or x_error > x_range: #先调方向
car.run(-x_output, x_output)
else: #方向正确后if x_error<-x_range or x_error>x_range:#先调方向
if -y_range < y_error < y_range:
fifoCount -= packetSize
fifoBuffer = mpu.getFIFOBytes(packetSize)
yaw, rol, pit = mpu.dmpGetEuler(*mpu.dmpGetQuaternion(fifoBuffer))
g_pit, g_rol, g_yaw = mpu.dmpGetGyro(fifoBuffer)
yaw -= 6
#print(rol, pit, yaw, g_rol, g_pit, g_yaw)
# RC
#print('%s %s %s %s' % (rc_thr.get_width(), rc_rol.get_width(), rc_pit.get_width(), rc_yaw.get_width()))
rc_thr_width = rc_thr.get_width()
rc_rol_width = _map(rc_rol.get_width(), 995, 1945, -45, 45)
rc_pit_width = _map(rc_pit.get_width(), 995, 1945, -45, 45)
rc_yaw_width = _map(rc_yaw.get_width(), 995, 1945, -180, 180)
#print('%s %s %s' % (rc_rol_width, rc_pit_width, rc_yaw_width))
if rc_thr_width > 1200:
# Stablise PIDS
rol_stab_out = max(min(rs_pid.get_pid(rc_rol_width + rol, 1), 250), -250)
pit_stab_out = max(min(ps_pid.get_pid(rc_pit_width + pit, 1), 250), -250)
yaw_stab_out = max(min(ys_pid.get_pid(wrap_180(yaw_target + yaw), 1), 360), -360)
#print('%s %s %s' % (rol_stab_out, pit_stab_out, yaw_stab_out))
if abs(rc_yaw_width) > 5:
yaw_stab_out = rc_yaw_width
yaw_target = yaw
# rate PIDS
rol_out = max(min(rr_pid.get_pid(rol_stab_out + g_rol, 1), 500), -500)
pit_out = max(min(pr_pid.get_pid(pit_stab_out + g_pit, 1), 500), -500)
yaw_out = max(min(yr_pid.get_pid(yaw_stab_out + g_yaw, 1), 500), -500)
#print('%s %s %s' % (rol_out, pit_out, yaw_out))
# ESC
yaw_out = rc_yaw_width
esc_0.move(rc_thr_width - (0.866 * pit_out) + (0.5 * rol_out) - yaw_out)
esc_1.move(rc_thr_width - (0.866 * pit_out) - (0.5 * rol_out) + yaw_out)
