def main():
global FREQ1, FREQ2
range_init()
pwm1, pwm2 = servo.servo_init(FREQ1, FREQ2)
while (1):
try:
dist = get_dist()
print dist
if dist < 100:
call(['./take_pic.sh'])
vect = image_capture.get_feature_vector('image.jpg')
print vect
clss = classify(vect)
# Move laundry into correct bin
print 'Moving laundry to ' + str(clss) + '\'s bin'
servo.moveLaundry(clss, pwm1, pwm2)
except KeyboardInterrupt:
GPIO.cleanup()
# File name : server.py # Production : Gtank # Website : www.gewbot.com # E-mail : [email protected] # Author : William # Date : 2019/10/28 import socket import threading import time import os import LED import move import servo import switch servo.servo_init() switch.switchSetup() switch.set_all_switch_off() LED = LED.LED() LED.colorWipe(80, 255, 0) step_set = 1 speed_set = 100 rad = 0.6 direction_command = 'no' turn_command = 'no' servo_command = 'no' pos_input = 1 catch_input = 1 cir_input = 6
def run():
global servo_move, speed_set, servo_command, functionMode, init_get, R_set, G_set, B_set, SR_mode
servo.servo_init()
move.setup()
findline.setup()
direction_command = 'no'
turn_command = 'no'
servo_command = 'no'
speed_set = 100
rad = 0.5
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
servo_move = Servo_ctrl()
servo_move.start()
servo_move.pause()
findline.setup()
while True:
data = ''
data = str(tcpCliSock.recv(BUFSIZ).decode())
if not data:
continue
elif 'forward' == data:
direction_command = 'forward'
move.move(speed_set, direction_command)
elif 'backward' == data:
direction_command = 'backward'
move.move(speed_set, direction_command)
elif 'DS' in data:
direction_command = 'no'
move.move(speed_set, direction_command)
elif 'left' == data:
# turn_command = 'left'
servo.turnLeft()
elif 'right' == data:
# turn_command = 'right'
servo.turnRight()
elif 'TS' in data:
# turn_command = 'no'
servo.turnMiddle()
elif 'Switch_1_on' in data:
fpv.Sounds(1)
tcpCliSock.send(('Switch_1_on').encode())
elif 'Switch_1_off' in data:
fpv.Sounds(0)
tcpCliSock.send(('Switch_1_off').encode())
elif 'Switch_2_on' in data:
#switch.switch(2,1)
fpv.Radar(1)
tcpCliSock.send(('Switch_2_on').encode())
elif 'Switch_2_off' in data:
#switch.switch(2,0)
fpv.Radar(0)
tcpCliSock.send(('Switch_2_off').encode())
elif 'Switch_3_on' in data:
switch.switch(3, 1)
tcpCliSock.send(('Switch_3_on').encode())
elif 'Switch_3_off' in data:
switch.switch(3, 0)
tcpCliSock.send(('Switch_3_off').encode())
elif 'Switch_A_on' in data:
tcpCliSock.send(('Switch_A_on').encode())
elif 'Switch_A_off' in data:
tcpCliSock.send(('Switch_A_off').encode())
elif 'Switch_B_on' in data:
tcpCliSock.send(('Switch_B_on').encode())
elif 'Switch_B_off' in data:
tcpCliSock.send(('Switch_B_off').encode())
elif 'Switch_C_on' in data:
fpv.HaarJJ(1)
tcpCliSock.send(('Switch_C_on').encode())
elif 'Switch_C_off' in data:
fpv.HaarJJ(0)
tcpCliSock.send(('Switch_C_off').encode())
elif 'Face_Track_on' in data:
# functionMode = 3
fpv.FaceTrack(1)
tcpCliSock.send(('Face_Track_on').encode())
elif 'Face_Track_off' in data:
# functionMode = 0
fpv.FaceTrack(0)
tcpCliSock.send(('Face_Track_off').encode())
elif 'function_1_on' in data:
servo.ahead()
time.sleep(0.2)
tcpCliSock.send(('function_1_on').encode())
radar_send = servo.radar_scan()
tcpCliSock.sendall(radar_send.encode())
print(radar_send)
time.sleep(0.3)
tcpCliSock.send(('function_1_off').encode())
elif 'function_2_on' in data:
functionMode = 2
fpv.FindColor(1)
tcpCliSock.send(('function_2_on').encode())
elif 'function_2_off' in data:
functionMode = 0
fpv.FindColor(0)
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
tcpCliSock.send(('function_2_off').encode())
elif 'function_3_on' in data:
functionMode = 3
fpv.WatchDog(1)
tcpCliSock.send(('function_3_on').encode())
elif 'function_3_off' in data:
functionMode = 0
fpv.WatchDog(0)
tcpCliSock.send(('function_3_off').encode())
elif 'function_4_on' in data:
functionMode = 4
servo_move.resume()
tcpCliSock.send(('function_4_on').encode())
elif 'function_4_off' in data:
functionMode = 0
servo_move.pause()
move.motorStop()
tcpCliSock.send(('function_4_off').encode())
elif 'function_5_on' in data:
functionMode = 5
servo_move.resume()
tcpCliSock.send(('function_5_on').encode())
elif 'function_5_off' in data:
functionMode = 0
servo_move.pause()
move.motorStop()
tcpCliSock.send(('function_5_off').encode())
elif 'function_6_on' in data:
if MPU_connection:
functionMode = 6
servo_move.resume()
tcpCliSock.send(('function_6_on').encode())
elif 'function_6_off' in data:
functionMode = 0
servo_move.pause()
move.motorStop()
init_get = 0
tcpCliSock.send(('function_6_off').encode())
#elif 'function_1_off' in data:
# tcpCliSock.send(('function_1_off').encode())
elif 'lookleft' == data:
servo_command = 'lookleft'
servo_move.resume()
elif 'lookright' == data:
servo_command = 'lookright'
servo_move.resume()
elif 'up' == data:
servo_command = 'up'
servo_move.resume()
elif 'down' == data:
servo_command = 'down'
servo_move.resume()
elif 'stop' == data:
if not functionMode:
servo_move.pause()
servo_command = 'no'
pass
elif 'home' == data:
servo.ahead()
elif 'CVrun' == data:
if not FPV.CVrun:
FPV.CVrun = 1
tcpCliSock.send(('CVrun_on').encode())
else:
FPV.CVrun = 0
tcpCliSock.send(('CVrun_off').encode())
elif 'wsR' in data:
try:
set_R = data.split()
R_set = int(set_R[1])
led.colorWipe(R_set, G_set, B_set)
except:
pass
elif 'wsG' in data:
try:
set_G = data.split()
G_set = int(set_G[1])
led.colorWipe(R_set, G_set, B_set)
except:
pass
elif 'wsB' in data:
try:
set_B = data.split()
B_set = int(set_B[1])
led.colorWipe(R_set, G_set, B_set)
except:
pass
elif 'pwm0' in data:
try:
set_pwm0 = data.split()
pwm0_set = int(set_pwm0[1])
servo.setPWM(0, pwm0_set)
except:
pass
elif 'pwm1' in data:
try:
set_pwm1 = data.split()
pwm1_set = int(set_pwm1[1])
servo.setPWM(1, pwm1_set)
except:
pass
elif 'pwm2' in data:
try:
set_pwm2 = data.split()
pwm2_set = int(set_pwm2[1])
servo.setPWM(2, pwm2_set)
except:
pass
elif 'Speed' in data:
try:
set_speed = data.split()
speed_set = int(set_speed[1])
except:
pass
elif 'Save' in data:
try:
servo.saveConfig()
except:
pass
elif 'CVFL' in data:
if not FPV.FindLineMode:
FPV.FindLineMode = 1
tcpCliSock.send(('CVFL_on').encode())
else:
move.motorStop()
FPV.FindLineMode = 0
tcpCliSock.send(('CVFL_off').encode())
elif 'Render' in data:
if FPV.frameRender:
FPV.frameRender = 0
else:
FPV.frameRender = 1
elif 'WBswitch' in data:
if FPV.lineColorSet == 255:
FPV.lineColorSet = 0
else:
FPV.lineColorSet = 255
elif 'lip1' in data:
try:
set_lip1 = data.split()
lip1_set = int(set_lip1[1])
FPV.linePos_1 = lip1_set
except:
pass
elif 'lip2' in data:
try:
set_lip2 = data.split()
lip2_set = int(set_lip2[1])
FPV.linePos_2 = lip2_set
except:
pass
elif 'err' in data:
try:
set_err = data.split()
err_set = int(set_err[1])
FPV.findLineError = err_set
except:
pass
elif 'FCSET' in data:
FCSET = data.split()
fpv.colorFindSet(int(FCSET[1]), int(FCSET[2]), int(FCSET[3]))
elif 'setEC' in data: #Z
ECset = data.split()
try:
fpv.setExpCom(int(ECset[1]))
except:
pass
elif 'defEC' in data: #Z
fpv.defaultExpCom()
elif 'police' in data:
if LED.ledfunc != 'police':
tcpCliSock.send(('rainbow_off').encode())
LED.ledfunc = 'police'
ledthread.resume()
tcpCliSock.send(('police_on').encode())
elif LED.ledfunc == 'police':
LED.ledfunc = ''
ledthread.pause()
tcpCliSock.send(('police_off').encode())
elif 'rainbow' in data:
if LED.ledfunc != 'rainbow':
tcpCliSock.send(('police_off').encode())
LED.ledfunc = 'rainbow'
ledthread.resume()
tcpCliSock.send(('rainbow_on').encode())
elif LED.ledfunc == 'rainbow':
LED.ledfunc = ''
ledthread.pause()
tcpCliSock.send(('rainbow_off').encode())
elif 'sr' in data:
if not SR_mode:
if SR_dect:
SR_mode = 1
tcpCliSock.send(('sr_on').encode())
sr.resume()
elif SR_mode:
SR_mode = 0
sr.pause()
move.motorStop()
tcpCliSock.send(('sr_off').encode())
else:
pass
print(data)
def run():
global servo_move, speed_set, servo_command, functionMode, init_get, R_set, G_set, B_set, SR_mode
servo.servo_init()
move.setup()
findline.setup()
direction_command = "no"
turn_command = "no"
servo_command = "no"
speed_set = 100
rad = 0.5
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
servo_move = Servo_ctrl()
servo_move.start()
servo_move.pause()
findline.setup()
while True:
data = ""
data = str(tcpCliSock.recv(BUFSIZ).decode())
if not data:
continue
elif "forward" == data:
direction_command = "forward"
move.move(speed_set, direction_command)
elif "backward" == data:
direction_command = "backward"
move.move(speed_set, direction_command)
elif "DS" in data:
direction_command = "no"
move.move(speed_set, direction_command)
elif "left" == data:
# turn_command = 'left'
servo.turnLeft()
elif "right" == data:
# turn_command = 'right'
servo.turnRight()
elif "TS" in data:
# turn_command = 'no'
servo.turnMiddle()
elif "Switch_1_on" in data:
switch.switch(1, 1)
tcpCliSock.send(("Switch_1_on").encode())
elif "Switch_1_off" in data:
switch.switch(1, 0)
tcpCliSock.send(("Switch_1_off").encode())
elif "Switch_2_on" in data:
switch.switch(2, 1)
tcpCliSock.send(("Switch_2_on").encode())
elif "Switch_2_off" in data:
switch.switch(2, 0)
tcpCliSock.send(("Switch_2_off").encode())
elif "Switch_3_on" in data:
switch.switch(3, 1)
tcpCliSock.send(("Switch_3_on").encode())
elif "Switch_3_off" in data:
switch.switch(3, 0)
tcpCliSock.send(("Switch_3_off").encode())
elif "function_1_on" in data:
servo.ahead()
time.sleep(0.2)
tcpCliSock.send(("function_1_on").encode())
radar_send = servo.radar_scan()
tcpCliSock.sendall(radar_send.encode())
print(radar_send)
time.sleep(0.3)
tcpCliSock.send(("function_1_off").encode())
elif "function_2_on" in data:
functionMode = 2
fpv.FindColor(1)
tcpCliSock.send(("function_2_on").encode())
elif "function_3_on" in data:
functionMode = 3
fpv.WatchDog(1)
tcpCliSock.send(("function_3_on").encode())
elif "function_4_on" in data:
functionMode = 4
servo_move.resume()
tcpCliSock.send(("function_4_on").encode())
elif "function_5_on" in data:
functionMode = 5
servo_move.resume()
tcpCliSock.send(("function_5_on").encode())
elif "function_6_on" in data:
if MPU_connection:
functionMode = 6
servo_move.resume()
tcpCliSock.send(("function_6_on").encode())
# elif 'function_1_off' in data:
# tcpCliSock.send(('function_1_off').encode())
elif "function_2_off" in data:
functionMode = 0
fpv.FindColor(0)
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
tcpCliSock.send(("function_2_off").encode())
elif "function_3_off" in data:
functionMode = 0
fpv.WatchDog(0)
tcpCliSock.send(("function_3_off").encode())
elif "function_4_off" in data:
functionMode = 0
servo_move.pause()
move.motorStop()
tcpCliSock.send(("function_4_off").encode())
elif "function_5_off" in data:
functionMode = 0
servo_move.pause()
move.motorStop()
tcpCliSock.send(("function_5_off").encode())
elif "function_6_off" in data:
functionMode = 0
servo_move.pause()
move.motorStop()
init_get = 0
tcpCliSock.send(("function_6_off").encode())
elif "lookleft" == data:
servo_command = "lookleft"
servo_move.resume()
elif "lookright" == data:
servo_command = "lookright"
servo_move.resume()
elif "up" == data:
servo_command = "up"
servo_move.resume()
elif "down" == data:
servo_command = "down"
servo_move.resume()
elif "stop" == data:
if not functionMode:
servo_move.pause()
servo_command = "no"
pass
elif "home" == data:
servo.ahead()
elif "CVrun" == data:
if not FPV.CVrun:
FPV.CVrun = 1
tcpCliSock.send(("CVrun_on").encode())
else:
FPV.CVrun = 0
tcpCliSock.send(("CVrun_off").encode())
elif "wsR" in data:
try:
set_R = data.split()
R_set = int(set_R[1])
led.colorWipe(R_set, G_set, B_set)
except:
pass
elif "wsG" in data:
try:
set_G = data.split()
G_set = int(set_G[1])
led.colorWipe(R_set, G_set, B_set)
except:
pass
elif "wsB" in data:
try:
set_B = data.split()
B_set = int(set_B[1])
led.colorWipe(R_set, G_set, B_set)
except:
pass
elif "pwm0" in data:
try:
set_pwm0 = data.split()
pwm0_set = int(set_pwm0[1])
servo.setPWM(0, pwm0_set)
except:
pass
elif "pwm1" in data:
try:
set_pwm1 = data.split()
pwm1_set = int(set_pwm1[1])
servo.setPWM(1, pwm1_set)
except:
pass
elif "pwm2" in data:
try:
set_pwm2 = data.split()
pwm2_set = int(set_pwm2[1])
servo.setPWM(2, pwm2_set)
except:
pass
elif "Speed" in data:
try:
set_speed = data.split()
speed_set = int(set_speed[1])
except:
pass
elif "Save" in data:
try:
servo.saveConfig()
except:
pass
elif "CVFL" in data:
if not FPV.FindLineMode:
FPV.FindLineMode = 1
tcpCliSock.send(("CVFL_on").encode())
else:
move.motorStop()
FPV.FindLineMode = 0
tcpCliSock.send(("CVFL_off").encode())
elif "Render" in data:
if FPV.frameRender:
FPV.frameRender = 0
else:
FPV.frameRender = 1
elif "WBswitch" in data:
if FPV.lineColorSet == 255:
FPV.lineColorSet = 0
else:
FPV.lineColorSet = 255
elif "lip1" in data:
try:
set_lip1 = data.split()
lip1_set = int(set_lip1[1])
FPV.linePos_1 = lip1_set
except:
pass
elif "lip2" in data:
try:
set_lip2 = data.split()
lip2_set = int(set_lip2[1])
FPV.linePos_2 = lip2_set
except:
pass
elif "err" in data:
try:
set_err = data.split()
err_set = int(set_err[1])
FPV.findLineError = err_set
except:
pass
elif "FCSET" in data:
FCSET = data.split()
fpv.colorFindSet(int(FCSET[1]), int(FCSET[2]), int(FCSET[3]))
elif "setEC" in data: # Z
ECset = data.split()
try:
fpv.setExpCom(int(ECset[1]))
except:
pass
elif "defEC" in data: # Z
fpv.defaultExpCom()
elif "police" in data:
if LED.ledfunc != "police":
tcpCliSock.send(("rainbow_off").encode())
LED.ledfunc = "police"
ledthread.resume()
tcpCliSock.send(("police_on").encode())
elif LED.ledfunc == "police":
LED.ledfunc = ""
ledthread.pause()
tcpCliSock.send(("police_off").encode())
elif "rainbow" in data:
if LED.ledfunc != "rainbow":
tcpCliSock.send(("police_off").encode())
LED.ledfunc = "rainbow"
ledthread.resume()
tcpCliSock.send(("rainbow_on").encode())
elif LED.ledfunc == "rainbow":
LED.ledfunc = ""
ledthread.pause()
tcpCliSock.send(("rainbow_off").encode())
elif "sr" in data:
if not SR_mode:
if SR_dect:
SR_mode = 1
tcpCliSock.send(("sr_on").encode())
sr.resume()
elif SR_mode:
SR_mode = 0
sr.pause()
move.motorStop()
tcpCliSock.send(("sr_off").encode())
else:
pass
print(data)
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 = 11
T_servo = 11
P_anglePos = 0
T_anglePos = 0
cameraDiagonalW = 64
cameraDiagonalH = 48
videoW = 640
videoH = 480
Y_lock = 0
X_lock = 0
tor = 17
scGear = servo.servo_init()
#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)
if (timestamp - self.lastMovtionCaptured).seconds >= 0.5:
self.drawing = 0
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
self.pause()
def findLineCtrl(self, posInput, setCenter): #2
# if posInput:
if posInput > (setCenter + findLineError):
# move.motorStop()
#turnRight
error = (posInput - 320) / 3
outv = int(round((pid.GenOut(error)), 0))
CVThread.scGear.moveAngle(0, -outv)
if CVRun:
move.move(80, 'no', 'right', 0.5)
else:
move.move(80, 'no', 'no', 0.5)
pass
elif posInput < (setCenter - findLineError):
# move.motorStop()
#turnLeft
error = (320 - posInput) / 3
outv = int(round((pid.GenOut(error)), 0))
CVThread.scGear.moveAngle(0, outv)
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)
#forward
pass
# else:
# pass
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
pass
self.findLineCtrl(self.center, 320)
self.pause()
def servoMove(ID, Dir, errorInput):
if ID == 1:
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 == 11:
errorGenOut = CVThread.kalman_filter_Y.kalman(errorInput)
CVThread.T_anglePos += 0.25 * (
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)
else:
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
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
from BaseHTTPServer import BaseHTTPRequestHandler, HTTPServer
import sys, os
import signal
import subprocess
import threading
import time
from subprocess import call
import servo
import RPi as GPIO
FREQ = 50
pwm1, pwm2, pwm3 = servo.servo_init(FREQ)
class ServerHandler(BaseHTTPRequestHandler):
global pwm1, pwm2, pwm3
def _set_headers(self, content_type="text/plain"):
self.send_response(200)
self.end_headers()
def do_HEAD(self):
self._set_headers()
def do_GET(self):
print("in do get")
action = self.path.split("/")
action = action[1]
print("here is the action",action)
if(action == "waterup"):
servo.moveMotor(1,pwm1,pwm2,pwm3)
def run():
global servo_move, speed_set, servo_command, functionMode, init_get
servo.servo_init()
move.setup()
findline.setup()
direction_command = 'no'
turn_command = 'no'
servo_command = 'no'
speed_set = 100
rad = 0.5
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
servo_move = Servo_ctrl()
servo_move.start()
servo_move.pause()
findline.setup()
while True:
data = ''
data = str(tcpCliSock.recv(BUFSIZ).decode())
if not data:
continue
elif 'forward' == data:
direction_command = 'forward'
move.move(speed_set, direction_command, turn_command, rad)
elif 'backward' == data:
direction_command = 'backward'
move.move(speed_set, direction_command, turn_command, rad)
elif 'DS' in data:
direction_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'left' == data:
turn_command = 'left'
move.move(speed_set, direction_command, turn_command, rad)
elif 'right' == data:
turn_command = 'right'
move.move(speed_set, direction_command, turn_command, rad)
elif 'TS' in data:
turn_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'Switch_1_on' in data:
switch.switch(1, 1)
tcpCliSock.send(('Switch_1_on').encode())
elif 'Switch_1_off' in data:
switch.switch(1, 0)
tcpCliSock.send(('Switch_1_off').encode())
elif 'Switch_2_on' in data:
switch.switch(2, 1)
tcpCliSock.send(('Switch_2_on').encode())
elif 'Switch_2_off' in data:
switch.switch(2, 0)
tcpCliSock.send(('Switch_2_off').encode())
elif 'Switch_3_on' in data:
switch.switch(3, 1)
tcpCliSock.send(('Switch_3_on').encode())
elif 'Switch_3_off' in data:
switch.switch(3, 0)
tcpCliSock.send(('Switch_3_off').encode())
elif 'function_1_on' in data:
servo.ahead()
time.sleep(0.2)
tcpCliSock.send(('function_1_on').encode())
radar_send = servo.radar_scan()
tcpCliSock.sendall(radar_send.encode())
print(radar_send)
time.sleep(0.3)
tcpCliSock.send(('function_1_off').encode())
elif 'function_2_on' in data:
functionMode = 2
fpv.FindColor(1)
tcpCliSock.send(('function_2_on').encode())
elif 'function_3_on' in data:
functionMode = 3
fpv.WatchDog(1)
tcpCliSock.send(('function_3_on').encode())
elif 'function_4_on' in data:
functionMode = 4
servo_move.resume()
tcpCliSock.send(('function_4_on').encode())
elif 'function_5_on' in data:
functionMode = 5
servo_move.resume()
tcpCliSock.send(('function_5_on').encode())
elif 'function_6_on' in data:
if MPU_connection:
functionMode = 6
servo_move.resume()
tcpCliSock.send(('function_6_on').encode())
#elif 'function_1_off' in data:
# tcpCliSock.send(('function_1_off').encode())
elif 'function_2_off' in data:
functionMode = 0
fpv.FindColor(0)
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
tcpCliSock.send(('function_2_off').encode())
elif 'function_3_off' in data:
functionMode = 0
fpv.WatchDog(0)
tcpCliSock.send(('function_3_off').encode())
elif 'function_4_off' in data:
functionMode = 0
servo_move.pause()
move.motorStop()
tcpCliSock.send(('function_4_off').encode())
elif 'function_5_off' in data:
functionMode = 0
servo_move.pause()
move.motorStop()
tcpCliSock.send(('function_5_off').encode())
elif 'function_6_off' in data:
functionMode = 0
servo_move.pause()
move.motorStop()
init_get = 0
tcpCliSock.send(('function_6_off').encode())
elif 'lookleft' == data:
servo_command = 'lookleft'
servo_move.resume()
elif 'lookright' == data:
servo_command = 'lookright'
servo_move.resume()
elif 'up' == data:
servo_command = 'up'
servo_move.resume()
elif 'down' == data:
servo_command = 'down'
servo_move.resume()
elif 'lookup' == data:
servo_command = 'lookup'
servo_move.resume()
elif 'lookdown' == data:
servo_command = 'lookdown'
servo_move.resume()
elif 'grab' == data:
servo_command = 'grab'
servo_move.resume()
elif 'loose' == data:
servo_command = 'loose'
servo_move.resume()
elif 'stop' == data:
if not functionMode:
servo_move.pause()
servo_command = 'no'
pass
elif 'home' == data:
servo.ahead()
elif 'wsB' in data:
try:
set_B = data.split()
speed_set = int(set_B[1])
except:
pass
elif 'CVFL' in data:
if not FPV.FindLineMode:
FPV.FindLineMode = 1
tcpCliSock.send(('CVFL_on').encode())
else:
move.motorStop()
FPV.FindLineMode = 0
tcpCliSock.send(('CVFL_off').encode())
elif 'Render' in data:
if FPV.frameRender:
FPV.frameRender = 0
else:
FPV.frameRender = 1
elif 'WBswitch' in data:
if FPV.lineColorSet == 255:
FPV.lineColorSet = 0
else:
FPV.lineColorSet = 255
elif 'lip1' in data:
try:
set_lip1 = data.split()
lip1_set = int(set_lip1[1])
FPV.linePos_1 = lip1_set
except:
pass
elif 'lip2' in data:
try:
set_lip2 = data.split()
lip2_set = int(set_lip2[1])
FPV.linePos_2 = lip2_set
except:
pass
elif 'err' in data:
try:
set_err = data.split()
err_set = int(set_err[1])
FPV.findLineError = err_set
except:
pass
elif 'FCSET' in data:
FCSET = data.split()
fpv.colorFindSet(int(FCSET[1]), int(FCSET[2]), int(FCSET[3]))
elif 'setEC' in data: #Z
ECset = data.split()
try:
fpv.setExpCom(int(ECset[1]))
except:
pass
elif 'defEC' in data: #Z
fpv.defaultExpCom()
else:
pass
print(data)
