def Move():
global servo_1
global servo_2
global dis_ok, x_dis, y_dis
while True:
#云台跟踪
if dis_ok is True:
PWMServo.setServo(servo_1, y_dis, 20)
PWMServo.setServo(servo_2, x_dis, 20)
dis_ok = False
else:
time.sleep(0.01)
def adjust():
global x_output
global y_output
global update_ok
global x_dis
while True:
if update_ok:
# print x_output
PWMServo.setServo(1, int(x_dis), 20)
time.sleep(0.02)
PWMServo.setServo(2, int(y_dis), 20)
time.sleep(0.02)
update_ok = False
else:
time.sleep(0.01)
def Move():
global servo_1
global servo_2
global dis_ok, x_dis, y_dis
global action_finish
while True:
#云台跟踪
if dis_ok is True:
dis_ok = False
action_finish = False
PWMServo.setServo(servo_1, y_dis, 20)
PWMServo.setServo(servo_2, x_dis, 20)
time.sleep(0.02)
action_finish = True
else:
time.sleep(0.01)
def track():
global servo1_face_track, servo2_face_track
global dis_ok_face, action_finish_face
global motor, motor_face_track
while True:
#跟踪动作线程
if dis_ok_face:
dis_ok_face = False
action_finish_face = False
PWMServo.setServo(1, servo1_face_track, 20)
PWMServo.setServo(2, servo2_face_track, 20)
motor.setSpeed(motor_face_track) #设置机器人电机速度
time.sleep(0.02)
action_finish_face = True
else:
time.sleep(0.01)
def camera_pos_init():
PWMServo.setServo(1, 1500, 200)
time.sleep(0.2)
PWMServo.setServo(2, 2000, 200)
time.sleep(0.2)
def cam_pos_init():
PWMServo.setServo(1, 1500, 50)
time.sleep(0.05)
PWMServo.setServo(2, 1300, 50)
time.sleep(0.05)
if Running:
if cap.isOpened():
ret, orgFrame = cap.read()
else:
time.sleep(0.01)
else:
time.sleep(0.01)
th1 = threading.Thread(target=get_image)
th1.setDaemon(True)
th1.start()
servo_1 = 1
servo_2 = 2
PWMServo.setServo(servo_1, 1500, 500)
PWMServo.setServo(servo_2, 1500, 500)
time.sleep(0.5)
def Move():
global servo_1
global servo_2
global dis_ok, x_dis, y_dis
while True:
#云台跟踪
if dis_ok is True:
PWMServo.setServo(servo_1, y_dis, 20)
PWMServo.setServo(servo_2, x_dis, 20)
dis_ok = False
def logic():
global xc
global centerX
global centerY
global Running
global step
global stoop
global count
global get_ball
while True:
if Running is True:
if xc is True:
if stoop:
SSR.runAction('0')
time.sleep(0.5)
stoop = False
get_ball = True
step = 5
count = 0
if step == 1:
if 640 >= centerX + (320 -
centerx) > 520: #不在中心，根据方向让机器人转向一步
SSR.runAction('right_move_one_step')
print('1')
elif 0 <= centerX + (320 - centerx) < 120:
SSR.runAction('left_move_one_step')
print('2')
else:
step = 2
xc = False
elif step == 2:
print(centerY)
if 250 < centerY <= 380:
print(3)
SSR.runAction('go_fast')
step = 3
elif 0 <= centerY <= 200:
print(3)
SSR.runAction('go_fast')
SSR.runAction('go_fast')
SSR.runAction('go_fast')
step = 3
else:
step = 4
xc = False
elif step == 3:
if 640 >= centerX + (320 -
centerx) > 450: #不在中心，根据方向让机器人转向一步
SSR.runAction('right_move_one_step')
print('1')
elif 0 <= centerX + (320 - centerx) < 190:
SSR.runAction('left_move_one_step')
print('2')
else:
step = 2
xc = False
elif step == 4:
if (320 - centerx > 0 and centerx + 80 > centerX) or (
320 - centerx < 0
and centerx + 180 > centerX): #不在中心，根据方向让机器人转向一步
if get_ball:
SSR.runAction('stand_')
time.sleep(0.5)
SSR.runAction('left_move_one_step')
print('4')
elif (320 - centerx > 0 and centerx + 180 < centerX) or (
320 - centerx < 0 and centerx - 80 < centerX):
if get_ball:
SSR.runAction('stand_')
time.sleep(0.5)
SSR.runAction('right_move_one_step')
print('5')
else:
SSR.stop_action_group()
get_ball = False
step = 5
if get_ball:
step = 5
else:
xc = False
elif step == 5:
print('ok')
SSR.runAction('stand_')
SSR.runAction('stoop')
time.sleep(0.5)
if get_ball:
step = 4
else:
step = 6
xc = False
elif step == 6:
print(centerY)
if 0 < centerY <= 250:
print('6')
SSR.runAction('stand_')
SSR.runAction('go_fast')
SSR.runAction('go_fast')
step = 5
elif 250 < centerY <= 380:
print('7')
SSR.runAction('stand_')
SSR.runAction('go_fast')
step = 5
elif centerY > 380:
print('8')
print(centerY)
step = 7
else:
step = 1
elif step == 7:
if (320 - centerx) >= 0:
print('shot_right')
SSR.runAction('stand_')
SSR.runAction('shot_right')
step = 1
xc = False
elif (320 - centerx) < 0:
print('shot_left')
SSR.runAction('stand_')
SSR.runAction('shot_left')
step = 1
xc = False
else:
if step == 1:
if count >= 50:
count = 50
if not stoop:
stoop = True
SSR.runAction('0')
SSR.runAction('stoop')
time.sleep(2)
else:
PWMServo.setServo(1, servo1 + 250, 500)
time.sleep(2)
PWMServo.setServo(1, servo1, 500)
time.sleep(2)
else:
time.sleep(0.01)
def diantou():
PWMServo.setServo(1, 1500, 500)
time.sleep(0.5)
PWMServo.setServo(1, 1000, 500)
time.sleep(0.5)
PWMServo.setServo(1, 1500, 500)
PWMServo.setServo(1, 1500, 500)
time.sleep(0.5)
PWMServo.setServo(1, 1000, 500)
time.sleep(0.5)
PWMServo.setServo(1, 1500, 500)
PWMServo.setServo(1, 1500, 500)
time.sleep(0.5)
PWMServo.setServo(1, 1000, 500)
time.sleep(0.5)
PWMServo.setServo(1, 1500, 500)
def handle(self):
global action_num, action_times, inf_flag
conn = self.request
recv_data = b''
Flag = True
stop = False
t2 = 0
while Flag:
try:
buf = conn.recv(128)
if buf == b'':
Flag = False
else:
recv_data = recv_data + buf
if len(recv_data) <= 13:
# 解决断包问题，接收到完整命令后才发送到串口,防止出错
while True:
try:
index = recv_data.index(
b'\x55\x55') # 搜索数据中的0x55 0x55
if len(recv_data) >= index + 3: # 缓存中的数据长度是否足够
recv_data = recv_data[index:]
if recv_data[2] + 2 <= len(
recv_data): # 缓存中的数据长度是否足够
cmd = recv_data[0:(recv_data[2] +
2)] # 取出命令
#print(cmd)
recv_data = recv_data[(
recv_data[2] + 3):] # 去除已经取出的命令
if cmd[0] and cmd[1] is 0x55:
if cmd[2] == 0x08 and cmd[
3] == 0x03: # 数据长度和控制单舵机命令
id = cmd[7]
pos = 0xffff & cmd[
8] | 0xff00 & (cmd[9] << 8)
print('id:', cmd[7], 'pos:',
pos)
if id == 7:
if 1900 < pos:
pos = 1900
elif pos < 1200:
pos = 1200
PWMServo.setServo(
1, 3000 - pos, 20)
elif id == 6:
PWMServo.setServo(
2, 3000 - pos, 20)
else:
pass
elif cmd[2] == 0x05 and cmd[
3] == 0x06:
action_num = cmd[4]
action_times = 0xffff & cmd[
5] | 0xff00 & cmd[6] << 8
if action_num == 0:
SSR.stop_action_group()
SSR.change_action_value(
'stand_slow', 1)
try:
data = get_data.read_data(
)
if data[0] != 'K':
os.system(
'sudo kill -9 '
+ str(data[0]))
PWMServo.setServo(
1, 1500, 300)
PWMServo.setServo(
2, 1500, 300)
get_data.write_data(
"K", "0")
except BaseException as e:
print(e)
elif action_times == 0: # 无限次
print(
'action', action_num,
'times', action_times)
SSR.change_action_value(
str(action_num),
action_times)
stop = True
else:
print(
'action', action_num,
'times', action_times)
if stop:
SSR.stop_action_group()
SSR.change_action_value(
'stand_slow', 1)
stop = False
else:
print(
'action',
action_num,
'times',
action_times)
SSR.change_action_value(
str(action_num),
action_times)
elif cmd[2] == 0x0b and cmd[
3] == 0x03:
PWMServo.setServo(1, 1500, 100)
PWMServo.setServo(2, 1500, 100)
time.sleep(0.1)
elif cmd[2] == 0x02 and cmd[
3] == 0x0F:
if SSR.action_finish():
try:
time.sleep(0.05)
v = math.ceil(
serial_servo_read_vin(
1))
buf = [
0x55, 0x55, 0x04,
0x0F, 0x00, 0x00
]
buf[4] = v & 0xFF
buf[5] = (
v >> 8) & 0xFF
conn.sendall(
bytearray(buf))
except BaseException as e:
print(e)
if DEBUG is True:
for i in cmd:
print(hex(i))
print('*' * 30)
else:
break
else:
break
except Exception as e: # 在recv_data 中搜不到 '\x55\x55'子串
break
recv_data = b''
action_times = None
action_num = None
else:
recv_data = b''
pass
except Exception as e:
print(e)
Flag = False
break
def runAction():
global get_color, action_finish
global color_max
while True:
if get_color:
get_color = False
action_finish = False
if color_max == 'red':
PWMServo.setServo(1, 1800, 200)
time.sleep(0.2)
PWMServo.setServo(1, 1200, 200)
time.sleep(0.2)
PWMServo.setServo(1, 1800, 200)
time.sleep(0.2)
PWMServo.setServo(1, 1200, 200)
time.sleep(0.2)
PWMServo.setServo(1, 1500, 100)
SSR.runAction('pick')
time.sleep(1)
action_finish = True
elif color_max == 'green' or color_max == 'blue':
PWMServo.setServo(2, 1800, 200)
time.sleep(0.2)
PWMServo.setServo(2, 1200, 200)
time.sleep(0.2)
PWMServo.setServo(2, 1800, 200)
time.sleep(0.2)
PWMServo.setServo(2, 1200, 200)
time.sleep(0.2)
PWMServo.setServo(2, 1500, 100)
SSR.runAction('0')
time.sleep(1)
action_finish = True
else:
get_color = False
time.sleep(0.01)
else:
time.sleep(0.01)
def setcamera(state):
global check
if check == 0:
if state == 4:
PWMServo.setServo(1, 1700, 500)
#time.sleep(0.6)
PWMServo.setServo(2, 1500, 500)
#time.sleep(0.6)
else:
PWMServo.setServo(1, 2100, 500)
#time.sleep(0.6)
PWMServo.setServo(2, 1500, 500)
#time.sleep(0.6)
elif check == 1:
print('check', check)
PWMServo.setServo(1, 2100, 500)
PWMServo.setServo(2, 1200, 500)
check = 2
elif check == 2:
print('check', check)
PWMServo.setServo(1, 2100, 500)
PWMServo.setServo(2, 1700, 500)
check = 3
else:
print('check', check)
PWMServo.setServo(1, 2100, 500)
#time.sleep(0.6)
PWMServo.setServo(2, 1500, 500)
#time.sleep(0.6)
check = 0
def runAction():
global get_color
global color_max
while True:
if get_color:
if color_max == 'red':
PWMServo.setServo(1, 2000, 500)
time.sleep(0.6)
PWMServo.setServo(1, 1000, 500)
time.sleep(0.6)
PWMServo.setServo(1, 1500, 500)
time.sleep(0.6)
get_color = False
elif color_max == 'green' or color_max == 'blue':
PWMServo.setServo(2, 2000, 500)
time.sleep(0.6)
PWMServo.setServo(2, 1000, 500)
time.sleep(0.6)
PWMServo.setServo(2, 1500, 500)
time.sleep(0.6)
get_color = False
else:
get_color = False
time.sleep(0.01)
else:
time.sleep(0.01)
import time import PWMServo PWMServo.setServo(2, 1200, 200) time.sleep(0.2) PWMServo.setServo(2, 1700, 200) time.sleep(0.2) PWMServo.setServo(2, 1200, 200) time.sleep(0.2) PWMServo.setServo(2, 1700, 200) time.sleep(0.2) PWMServo.setServo(2, 1500, 100) time.sleep(0.1)
def camera_pos_init():
PWMServo.setServo(1, 1500, 100)
time.sleep(0.1)
PWMServo.setServo(2, 1500, 100)
time.sleep(0.1)
def yaotou():
PWMServo.setServo(2, 1500, 400)
time.sleep(0.4)
PWMServo.setServo(2, 1900, 400)
time.sleep(0.4)
PWMServo.setServo(2, 1500, 400)
time.sleep(0.4)
PWMServo.setServo(2, 1100, 400)
time.sleep(0.4)
PWMServo.setServo(2, 1500, 400)
time.sleep(0.4)
PWMServo.setServo(2, 1900, 400)
time.sleep(0.4)
PWMServo.setServo(2, 1500, 400)
time.sleep(0.4)
PWMServo.setServo(2, 1100, 400)
time.sleep(0.4)
PWMServo.setServo(2, 1500, 400)
def handle(self):
global action_num, action_times, inf_flag
conn = self.request
recv_data = b''
Flag = True
while Flag:
try:
buf = conn.recv(1024)
if buf == b'':
Flag = False
else:
recv_data = recv_data + buf
# 解决断包问题，接收到完整命令后才发送到串口,防止出错
while True:
try:
index = recv_data.index(b'\x55\x55') # 搜索数据中的0x55 0x55
if len(recv_data) >= index+3: # 缓存中的数据长度是否足够
recv_data = recv_data[index:]
if ord(recv_data[2]) + 2 <= len(recv_data): # 缓存中的数据长度是否足够
cmd = recv_data[0:ord(recv_data[2])+2] # 取出命令
recv_data = recv_data[ord(recv_data[2])+3:] # 去除已经取出的命令
if ord(cmd[0]) and ord(cmd[1]) is 0x55:
if ord(cmd[2]) == 0x08 and ord(cmd[3]) == 0x03: # 数据长度和控制单舵机命令
print('id', ord(cmd[7]))
id = ord(cmd[7])
pos = 0xffff & ord(cmd[8]) | 0xff00 & (ord(cmd[9]) << 8)
print('pos', pos)
if id == 6:
PWMServo.setServo(1, pos, 20)
elif id == 7:
PWMServo.setServo(2, pos, 20)
else:
pass
elif ord(cmd[2]) == 0x05 and ord(cmd[3]) == 0x06:
action_num = ord(cmd[4])
action_times = 0xffff & ord(cmd[5]) | 0xff00 & (ord(cmd[6]) << 8)
print 'action', action_num
print 'times', action_times
if action_times == 0: # 无限次
SSR.change_action_value(str(action_num), action_times)
inf_flag = True
else:
if inf_flag:
SSR.stop_action_group()
inf_flag = False
else:
SSR.change_action_value(str(action_num), action_times)
if DEBUG is True:
for i in cmd:
print hex(ord(i))
print('*' * 30)
else:
break
else:
break
except Exception as e: # 在recv_data 中搜不到 '\x55\x55'子串
break
recv_data = b''
action_times = None
action_num = None
except Exception as e:
print(e)
Flag = False
break
import threading
import time
import smbus
import LeCmd
import PWMServo
import device
PWMServo.setServo(1, 1500, 500)
PWMServo.setServo(2, 1500, 500)
LeCmd.cmd_i001([
500, 18, 1, 500, 2, 388, 3, 500, 4, 594, 5, 500, 6, 575, 7, 800, 8, 725, 9,
500, 10, 612, 11, 500, 12, 406, 13, 500, 14, 425, 15, 200, 16, 275, 17,
500, 18, 445
]) # 立正
RED = 1
GREEN = 2
BLUE = 3
WHITE = 4
R_F = 550
G_F = 950
B_F = 1200
r_f = 70
g_f = 100
b_f = 130
r = 0
g = 0
b = 0
def run_action(type):
if type == 0: # The camera echo
PWMServo.setServo(1, 1500, 100)
time.sleep(0.1)
PWMServo.setServo(2, 1500, 100)
time.sleep(0.1)
elif type == 1: # Nod
PWMServo.setServo(2, 1200, 200)
time.sleep(0.2)
PWMServo.setServo(2, 1700, 200)
time.sleep(0.2)
elif type == 2: # Shake head
PWMServo.setServo(1, 1200, 200)
time.sleep(0.2)
PWMServo.setServo(1, 1700, 200)
time.sleep(0.2)
print('''
**********************************************************
*****颜色跟踪:识别对应颜色的小球,让摄像头跟随小球转动*****
**********************************************************
----------------------------------------------------------
Official website:http://www.lobot-robot.com/pc/index/index
Online mall:https://lobot-zone.taobao.com/
----------------------------------------------------------
Version: --V3.2 2019/11/09
----------------------------------------------------------
''')
x_dis = 1500
y_dis = 1500
PWMServo.setServo(1, y_dis, 500)
PWMServo.setServo(2, x_dis, 500)
debug = True
target_color = "blue"
#数值范围映射
def leMap(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
#获取面积最大的轮廓
def getAreaMaxContour(contours):
contour_area_temp = 0
contour_area_max = 0
import Serial_Servo_Running as SSR
import signal
import PWMServo
debug = 1
Running = True
stream = "http://127.0.0.1:8080/?action=stream?dummy=param.mjpg"
cap = cv2.VideoCapture(stream)
orgFrame = None
# 机器人应该转的角度
deflection_angle = 0
# The value must be modified according to how the camera base was placed
PWMServo.setServo(1, 1200, 600) #up and down up-little
time.sleep(0.8)
PWMServo.setServo(2, 1500, 600) # Same as above
time.sleep(0.8)
#SSR.running_action_group('1', 5)
if Running:
if cap.isOpened():
ret, orgFrame = cap.read()
else:
time.sleep(0.01)
print('cap not open\n')
if orgFrame is not None and ret:
filename = 'captured/' + datetime.now().strftime('%y-%m-%d_%H_%M_%S')
np.save(filename, orgFrame)
import numpy
import PWMServo
import Serial_Servo_Running as SSR
import cv2
import time
import math
import sys
import socket
import signal
import threading
import PWMServo
import numpy as np
from cv_ImgAddText import *
from config import color_range
PWMServo.setServo(1, 1000, 1000)
PWMServo.setServo(2, 1450, 1000)
SSR.runAction('0')
SSR.start_action_thread()
stream = "http://127.0.0.1:8080/?action=stream?dummy=param.mjpg"
cap = cv2.VideoCapture(stream)
cap.release()
cap = cv2.VideoCapture(stream)
HOST = '192.168.149.1'
PORT = 8001
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.bind((HOST, PORT))
def fine_Apple():
global color
global RED, GREEN, BLUE, WHITE
while True:
if color == RED:
color = None
PWMServo.setServo(1, 1200, 200)
time.sleep(0.2)
PWMServo.setServo(1, 1700, 200)
time.sleep(0.2)
PWMServo.setServo(1, 1200, 200)
time.sleep(0.2)
PWMServo.setServo(1, 1700, 200)
time.sleep(0.2)
PWMServo.setServo(1, 1500, 100)
time.sleep(0.1)
elif color == GREEN or color == BLUE:
color = None
PWMServo.setServo(2, 1200, 200)
time.sleep(0.2)
PWMServo.setServo(2, 1700, 200)
time.sleep(0.2)
PWMServo.setServo(2, 1200, 200)
time.sleep(0.2)
PWMServo.setServo(2, 1700, 200)
time.sleep(0.2)
PWMServo.setServo(2, 1500, 100)
time.sleep(0.1)
else:
time.sleep(0.01)
def handle(self):
global action_num, action_times, inf_flag
conn = self.request
recv_data = b''
Flag = True
while Flag:
try:
buf = conn.recv(1024)
if buf == b'':
Flag = False
else:
recv_data = recv_data + buf
# Send the command to the serial port until receiving the complete command to prevent error
while True:
try:
index = recv_data.index(
b'\x55\x55') # Search 0x55 0x55 in the data
if len(
recv_data
) >= index + 3: # Whether the length of the data in the cache is sufficient
recv_data = recv_data[index:]
if recv_data[2] + 2 <= len(
recv_data
): # Whether the length of the data in the cache is enough
cmd = recv_data[0:(recv_data[2] +
2)] # Take the command
recv_data = recv_data[(
recv_data[2] +
3):] # Remove the taken command
if cmd[0] and cmd[1] is 0x55:
if cmd[2] == 0x08 and cmd[
3] == 0x03: # Data length and control single servo command
print('id', cmd[7])
id = cmd[7]
pos = 0xffff & cmd[
8] | 0xff00 & cmd[9] << 8
print('pos', pos)
if id == 19:
PWMServo.setServo(1, pos, 20)
elif id == 20:
PWMServo.setServo(2, pos, 20)
else:
pass
elif cmd[2] == 0x05 and cmd[3] == 0x06:
action_num = cmd[4]
action_times = 0xffff & cmd[
5] | 0xff00 & cmd[6] << 8
print('action', action_num)
print('times', action_times)
if action_times == 0: # Infinite imes
print('action_times:',
action_times)
SSR.change_action_value(
str(action_num),
action_times)
inf_flag = True
else:
if inf_flag:
SSR.stop_action_group()
inf_flag = False
else:
SSR.change_action_value(
str(action_num),
action_times)
elif cmd[2] == 0x0b and cmd[3] == 0x03:
PWMServo.setServo(1, 1500, 100)
PWMServo.setServo(2, 1500, 100)
time.sleep(0.1)
data = get_data.read_data()
print(int(data[0]))
os.kill(int(data[0]), signal.SIGKILL)
get_data.write_data("K", "0")
if DEBUG is True:
for i in cmd:
print(hex(i))
print('*' * 30)
else:
break
else:
break
except Exception as e: # The '\x55\x55' substring cannot be found in recv_data
break
recv_data = b''
action_times = None
action_num = None
except Exception as e:
print(e)
Flag = False
break
sys.exit(0)
import cv2
import time
import math
import socket
import signal
import threading
import PWMServo
import check_camera
import numpy as np
import timeout_decorator
from config import *
from cv_ImgAddText import *
import Serial_Servo_Running as SSR
PWMServo.setServo(1, servo1, 500)
PWMServo.setServo(2, servo2, 500)
debug = True
def read_data():
in_file = open("/home/pi/human_code/share.txt", "r")
state = in_file.readline()
in_file.close()
return state
#数值映射
def leMap(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
#! /usr/bin/env python
import time
import Serial_Servo_Running as SSR
import PWMServo
if __name__ == '__main__':
# while True:
# print "Child Running"
# time.sleep(1)
PWMServo.setServo(1, 1500, 200)