class CVThread(threading.Thread):
font = cv2.FONT_HERSHEY_SIMPLEX
kalman_filter_X = Kalman_filter.Kalman_filter(0.01, 0.1)
kalman_filter_Y = Kalman_filter.Kalman_filter(0.01, 0.1)
P_direction = 1
T_direction = -1
P_servo = 0
T_servo = 1
P_anglePos = 0
T_anglePos = 0
cameraDiagonalW = 64
cameraDiagonalH = 48
videoW = 640
videoH = 480
Y_lock = 0
X_lock = 0
tor = 17
scGear = RPIservo.ServoCtrl()
scGear.moveInit()
move.setup()
switch.switchSetup()
def __init__(self, *args, **kwargs):
self.CVThreading = 0
self.CVMode = 'none'
self.imgCV = None
self.mov_x = None
self.mov_y = None
self.mov_w = None
self.mov_h = None
self.radius = 0
self.box_x = None
self.box_y = None
self.drawing = 0
self.findColorDetection = 0
self.left_Pos1 = None
self.right_Pos1 = None
self.center_Pos1 = None
self.left_Pos2 = None
self.right_Pos2 = None
self.center_Pos2 = None
self.center = None
super(CVThread, self).__init__(*args, **kwargs)
self.__flag = threading.Event()
self.__flag.clear()
self.avg = None
self.motionCounter = 0
self.lastMovtionCaptured = datetime.datetime.now()
self.frameDelta = None
self.thresh = None
self.cnts = None
def mode(self, invar, imgInput):
self.CVMode = invar
self.imgCV = imgInput
self.resume()
def elementDraw(self, imgInput):
if self.CVMode == 'none':
pass
elif self.CVMode == 'findColor':
if self.findColorDetection:
cv2.putText(imgInput, 'Target Detected', (40, 60),
CVThread.font, 0.5, (255, 255, 255), 1,
cv2.LINE_AA)
self.drawing = 1
else:
cv2.putText(imgInput, 'Target Detecting', (40, 60),
CVThread.font, 0.5, (255, 255, 255), 1,
cv2.LINE_AA)
self.drawing = 0
if self.radius > 10 and self.drawing:
cv2.rectangle(imgInput, (int(self.box_x - self.radius),
int(self.box_y + self.radius)),
(int(self.box_x + self.radius),
int(self.box_y - self.radius)), (255, 255, 255),
1)
elif self.CVMode == 'findlineCV':
if frameRender:
imgInput = cv2.cvtColor(imgInput, cv2.COLOR_BGR2GRAY)
retval_bw, imgInput = cv2.threshold(imgInput, 0, 255,
cv2.THRESH_OTSU)
imgInput = cv2.erode(imgInput, None, iterations=6)
try:
if lineColorSet == 255:
cv2.putText(imgInput, ('Following White Line'), (30, 50),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (128, 255, 128),
1, cv2.LINE_AA)
else:
cv2.putText(imgInput, ('Following Black Line'), (30, 50),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (128, 255, 128),
1, cv2.LINE_AA)
cv2.line(imgInput, (self.left_Pos1, (linePos_1 + 30)),
(self.left_Pos1, (linePos_1 - 30)), (255, 128, 64), 1)
cv2.line(
imgInput,
(self.right_Pos1, (linePos_1 + 30)),
(self.right_Pos1, (linePos_1 - 30)),
(64, 128, 255),
)
cv2.line(imgInput, (0, linePos_1), (640, linePos_1),
(255, 255, 64), 1)
cv2.line(imgInput, (self.left_Pos2, (linePos_2 + 30)),
(self.left_Pos2, (linePos_2 - 30)), (255, 128, 64), 1)
cv2.line(imgInput, (self.right_Pos2, (linePos_2 + 30)),
(self.right_Pos2, (linePos_2 - 30)), (64, 128, 255),
1)
cv2.line(imgInput, (0, linePos_2), (640, linePos_2),
(255, 255, 64), 1)
cv2.line(imgInput,
((self.center - 20), int(
(linePos_1 + linePos_2) / 2)),
((self.center + 20), int(
(linePos_1 + linePos_2) / 2)), (0, 0, 0), 1)
cv2.line(
imgInput,
((self.center), int((linePos_1 + linePos_2) / 2 + 20)),
((self.center), int((linePos_1 + linePos_2) / 2 - 20)),
(0, 0, 0), 1)
except:
pass
elif self.CVMode == 'watchDog':
if self.drawing:
cv2.rectangle(
imgInput, (self.mov_x, self.mov_y),
(self.mov_x + self.mov_w, self.mov_y + self.mov_h),
(128, 255, 0), 1)
return imgInput
def watchDog(self, imgInput):
timestamp = datetime.datetime.now()
gray = cv2.cvtColor(imgInput, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (21, 21), 0)
if self.avg is None:
print("[INFO] starting background model...")
self.avg = gray.copy().astype("float")
return 'background model'
cv2.accumulateWeighted(gray, self.avg, 0.5)
self.frameDelta = cv2.absdiff(gray, cv2.convertScaleAbs(self.avg))
# threshold the delta image, dilate the thresholded image to fill
# in holes, then find contours on thresholded image
self.thresh = cv2.threshold(self.frameDelta, 5, 255,
cv2.THRESH_BINARY)[1]
self.thresh = cv2.dilate(self.thresh, None, iterations=2)
self.cnts = cv2.findContours(self.thresh.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
self.cnts = imutils.grab_contours(self.cnts)
# print('x')
# loop over the contours
for c in self.cnts:
# if the contour is too small, ignore it
if cv2.contourArea(c) < 5000:
continue
# compute the bounding box for the contour, draw it on the frame,
# and update the text
(self.mov_x, self.mov_y, self.mov_w,
self.mov_h) = cv2.boundingRect(c)
self.drawing = 1
self.motionCounter += 1
#print(motionCounter)
#print(text)
self.lastMovtionCaptured = timestamp
switch.switch(1, 1)
switch.switch(2, 1)
switch.switch(3, 1)
light.setColor(255, 64, 0)
if (timestamp - self.lastMovtionCaptured).seconds >= 0.5:
self.drawing = 0
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
light.setColor(0, 64, 255)
self.pause()
def findLineCtrl(self, posInput, setCenter): #2
if posInput and setCenter:
print(posInput, setCenter)
if posInput > (setCenter + findLineError):
if CVRun:
move.move(80, 'no', 'right', 0.5)
else:
move.move(80, 'no', 'no', 0.5)
pass
elif posInput < (setCenter - findLineError):
if CVRun:
move.move(80, 'no', 'left', 0.5)
else:
move.move(80, 'no', 'no', 0.5)
pass
else:
if CVRun:
move.move(80, 'forward', 'no', 0.5)
else:
move.move(80, 'no', 'no', 0.5)
pass
else:
move.motorStop()
def findlineCV(self, frame_image):
frame_findline = cv2.cvtColor(frame_image, cv2.COLOR_BGR2GRAY)
retval, frame_findline = cv2.threshold(frame_findline, 0, 255,
cv2.THRESH_OTSU)
frame_findline = cv2.erode(frame_findline, None, iterations=6)
colorPos_1 = frame_findline[linePos_1]
colorPos_2 = frame_findline[linePos_2]
try:
lineColorCount_Pos1 = np.sum(colorPos_1 == lineColorSet)
lineColorCount_Pos2 = np.sum(colorPos_2 == lineColorSet)
lineIndex_Pos1 = np.where(colorPos_1 == lineColorSet)
lineIndex_Pos2 = np.where(colorPos_2 == lineColorSet)
if lineColorCount_Pos1 == 0:
lineColorCount_Pos1 = 1
if lineColorCount_Pos2 == 0:
lineColorCount_Pos2 = 1
self.left_Pos1 = lineIndex_Pos1[0][lineColorCount_Pos1 - 1]
self.right_Pos1 = lineIndex_Pos1[0][0]
self.center_Pos1 = int((self.left_Pos1 + self.right_Pos1) / 2)
self.left_Pos2 = lineIndex_Pos2[0][lineColorCount_Pos2 - 1]
self.right_Pos2 = lineIndex_Pos2[0][0]
self.center_Pos2 = int((self.left_Pos2 + self.right_Pos2) / 2)
self.center = int((self.center_Pos1 + self.center_Pos2) / 2)
except:
center = None
move.motorStop()
pass
self.findLineCtrl(self.center, 320)
self.pause()
def servoMove(ID, Dir, errorInput):
if ID == CVThread.P_servo:
errorGenOut = CVThread.kalman_filter_X.kalman(errorInput)
CVThread.P_anglePos += 0.15 * (
errorGenOut * Dir) * CVThread.cameraDiagonalW / CVThread.videoW
if abs(errorInput) > CVThread.tor:
CVThread.scGear.moveAngle(ID, CVThread.P_anglePos)
CVThread.X_lock = 0
else:
CVThread.X_lock = 1
elif ID == CVThread.T_servo:
errorGenOut = CVThread.kalman_filter_Y.kalman(errorInput)
CVThread.T_anglePos += 0.15 * (
errorGenOut * Dir) * CVThread.cameraDiagonalH / CVThread.videoH
if abs(errorInput) > CVThread.tor:
CVThread.scGear.moveAngle(ID, CVThread.T_anglePos)
CVThread.Y_lock = 0
else:
CVThread.Y_lock = 1
else:
print('No servoPort %d assigned.' % ID)
def findColor(self, frame_image):
hsv = cv2.cvtColor(frame_image, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, colorLower, colorUpper) #1
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
if len(cnts) > 0:
self.findColorDetection = 1
c = max(cnts, key=cv2.contourArea)
((self.box_x, self.box_y), self.radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
X = int(self.box_x)
Y = int(self.box_y)
error_Y = 240 - Y
error_X = 320 - X
CVThread.servoMove(CVThread.P_servo, CVThread.P_direction, error_X)
CVThread.servoMove(CVThread.T_servo, CVThread.T_direction, error_Y)
if CVThread.X_lock == 1 and CVThread.Y_lock == 1:
switch.switch(1, 1)
switch.switch(2, 1)
switch.switch(3, 1)
light.setColor(255, 64, 0)
else:
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
light.setColor(0, 64, 255)
else:
self.findColorDetection = 0
move.motorStop()
self.pause()
def pause(self):
self.__flag.clear()
def resume(self):
self.__flag.set()
def run(self):
while 1:
self.__flag.wait()
if self.CVMode == 'none':
continue
elif self.CVMode == 'findColor':
self.CVThreading = 1
self.findColor(self.imgCV)
self.CVThreading = 0
elif self.CVMode == 'findlineCV':
self.CVThreading = 1
self.findlineCV(self.imgCV)
self.CVThreading = 0
elif self.CVMode == 'watchDog':
self.CVThreading = 1
self.watchDog(self.imgCV)
self.CVThreading = 0
pass
if not functionMode: if OLED_connection: screen.screen_show(5,'Functions OFF') else: pass print(data) response = json.dumps(response) await websocket.send(response) async def main_logic(websocket, path): await check_permit(websocket) await recv_msg(websocket) if __name__ == '__main__': switch.switchSetup() switch.set_all_switch_off() HOST = '' PORT = 10223 #Define port serial BUFSIZ = 1024 #Define buffer size ADDR = (HOST, PORT) global flask_app flask_app = app.webapp() flask_app.startthread() try: RL=robotLight.RobotLight() RL.start() RL.breath(70,70,255)
def server_setup():
global PWM_0, PWM_1, PWM_2, PWM_3, socket_speed, PWM_0_OLD, PWM_1_OLD, PWM_2_OLD, PWM_3_OLD
HOST = ''
PORT = 10223 #Define port serial
BUFSIZ = 1024 #Define buffer size
ADDR = (HOST, PORT)
def ap_thread():
os.system("sudo create_ap wlan0 eth0 AdeeptCar 12345678")
def get_cpu_tempfunc():
""" Return CPU temperature """
result = 0
mypath = "/sys/class/thermal/thermal_zone0/temp"
with open(mypath, 'r') as mytmpfile:
for line in mytmpfile:
result = line
result = float(result) / 1000
result = round(result, 1)
return str(result)
def get_gpu_tempfunc():
""" Return GPU temperature as a character string"""
res = os.popen('/opt/vc/bin/vcgencmd measure_temp').readline()
return res.replace("temp=", "")
def get_cpu_use():
""" Return CPU usage using psutil"""
cpu_cent = psutil.cpu_percent()
return str(cpu_cent)
def get_ram_info():
""" Return RAM usage using psutil """
ram_cent = psutil.virtual_memory()[2]
return str(ram_cent)
def get_swap_info():
""" Return swap memory usage using psutil """
swap_cent = psutil.swap_memory()[3]
return str(swap_cent)
def info_get():
global cpu_t, cpu_u, gpu_t, ram_info
while 1:
cpu_t = get_cpu_tempfunc()
cpu_u = get_cpu_use()
ram_info = get_ram_info()
time.sleep(3)
try:
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.connect(("1.1.1.1", 80))
ipaddr_check = s.getsockname()[0]
s.close()
print(ipaddr_check)
except:
ap_threading = threading.Thread(
target=ap_thread) #Define a thread for data receiving
ap_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
ap_threading.start() #Thread starts
tcpSerSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
tcpSerSock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
tcpSerSock.bind(ADDR)
tcpSerSock.listen(5) #Start server,waiting for client
print('waiting for connection...')
tcpCliSock, addr = tcpSerSock.accept()
print('...connected from :', addr)
switch.switchSetup()
def info_send_client():
SERVER_IP = addr[0]
SERVER_PORT = 2256 #Define port serial
SERVER_ADDR = (SERVER_IP, SERVER_PORT)
Info_Socket = socket.socket(
socket.AF_INET,
socket.SOCK_STREAM) #Set connection value for socket
Info_Socket.connect(SERVER_ADDR)
print(SERVER_ADDR)
while 1:
Info_Socket.send((get_cpu_tempfunc() + ' ' + get_cpu_use() + ' ' +
get_ram_info()).encode())
time.sleep(1)
info_threading = threading.Thread(
target=info_send_client) #Define a thread for FPV and OpenCV
info_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
info_threading.start() #Thread starts
while True:
data = ''
data = str(tcpCliSock.recv(BUFSIZ).decode())
if not data:
continue
elif '0+' == data:
PWM_select = 0
PWM_0 += socket_speed
pwm.set_pwm(0, 0, PWM_0)
PWM_0_OLD = PWM_0
elif '0-' == data:
PWM_select = 0
PWM_0 -= socket_speed
pwm.set_pwm(0, 0, PWM_0)
PWM_0_OLD = PWM_0
elif '1+' == data:
PWM_select = 1
PWM_1 += socket_speed
pwm.set_pwm(1, 0, PWM_1)
PWM_1_OLD = PWM_1
elif '1-' == data:
PWM_select = 1
PWM_1 -= socket_speed
pwm.set_pwm(1, 0, PWM_1)
PWM_1_OLD = PWM_1
elif '2+' == data:
PWM_select = 2
PWM_2 += socket_speed
pwm.set_pwm(2, 0, PWM_2)
PWM_2_OLD = PWM_2
elif '2-' == data:
PWM_select = 2
PWM_2 -= socket_speed
pwm.set_pwm(2, 0, PWM_2)
PWM_2_OLD = PWM_2
elif '3+' == data:
PWM_select = 3
PWM_3 += socket_speed
pwm.set_pwm(3, 0, PWM_3)
PWM_3_OLD = PWM_3
elif '3-' == data:
PWM_select = 3
PWM_3 -= socket_speed
pwm.set_pwm(3, 0, PWM_3)
PWM_3_OLD = PWM_3
elif 'Switch_1_on' == data:
switch.switch(1, 1)
elif 'Switch_2_on' == data:
switch.switch(2, 1)
elif 'Switch_3_on' == data:
switch.switch(3, 1)
elif 'Switch_1_off' == data:
switch.switch(1, 0)
elif 'Switch_2_off' == data:
switch.switch(2, 0)
elif 'Switch_3_off' == data:
switch.switch(3, 0)
elif 'wsB' in data:
try:
set_B = data.split()
socket_speed = int(set_B[1])
except:
pass
LCD_change()
print(data)
