GPIO.setup(line_pin_right, GPIO.IN)
GPIO.setup(line_pin_middle, GPIO.IN)
GPIO.setup(line_pin_left, GPIO.IN)
#motor.setup()
def run():
status_right = GPIO.input(line_pin_right)
status_middle = GPIO.input(line_pin_middle)
status_left = GPIO.input(line_pin_left)
#print('R%d M%d L%d'%(status_right,status_middle,status_left))
if status_middle == 1:
move.move(100, 'forward', 'no', 1)
elif status_left == 1:
move.move(100, 'forward', 'right', 0.6)
elif status_right == 1:
move.move(100, 'forward', 'left', 0.6)
else:
move.move(100, 'backward', 'no', 1)
if __name__ == '__main__':
try:
setup()
move.setup()
while 1:
run()
pass
except KeyboardInterrupt:
move.destroy()
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 = 1
T_servo = 0
P_anglePos = 0
T_anglePos = 0
cameraDiagonalW = 64
cameraDiagonalH = 48
videoW = 640
videoH = 480
Y_lock = 0
X_lock = 0
tor = 27
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
led.setColor(255,78,0)
led.both_off()
led.red()
# switch.switch(1,1)
# switch.switch(2,1)
# switch.switch(3,1)
if (timestamp - self.lastMovtionCaptured).seconds >= 0.5:
led.setColor(0,78,255)
led.both_off()
led.blue()
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(2,-outv)
if CVRun:
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
else:
move.motorStop()
pass
elif posInput < (setCenter - findLineError):
# move.motorStop()
#turnLeft
error = (320-posInput)/3
outv = int(round((pid.GenOut(error)),0))
CVThread.scGear.moveAngle(2,outv)
if CVRun:
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
else:
move.motorStop()
pass
else:
if CVRun:
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
else:
move.motorStop()
#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.35*(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 == 0:
errorGenOut = CVThread.kalman_filter_Y.kalman(errorInput)
CVThread.T_anglePos += 0.35*(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:
led.setColor(255,78,0)
led.both_off()
led.red()
print('locked')
else:
led.setColor(0,78,255)
led.both_off()
led.blue()
print('unlocked')
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
async def recv_msg(websocket):
global speed_set, modeSelect
move.setup()
direction_command = 'no'
turn_command = 'no'
while True:
response = {
'status' : 'ok',
'title' : '',
'data' : None
}
data = ''
data = await websocket.recv()
try:
data = json.loads(data)
except Exception as e:
print('not A JSON')
if not data:
continue
if isinstance(data,str):
robotCtrl(data, response)
switchCtrl(data, response)
functionSelect(data, response)
configPWM(data, response)
if 'get_info' == data:
response['title'] = 'get_info'
response['data'] = [info.get_cpu_tempfunc(), info.get_cpu_use(), info.get_ram_info()]
if 'wsB' in data:
try:
set_B=data.split()
speed_set = int(set_B[1])
except:
pass
elif 'AR' == data:
modeSelect = 'AR'
screen.screen_show(4, 'ARM MODE ON')
try:
fpv.changeMode('ARM MODE ON')
except:
pass
elif 'PT' == data:
modeSelect = 'PT'
screen.screen_show(4, 'PT MODE ON')
try:
fpv.changeMode('PT MODE ON')
except:
pass
#CVFL
elif 'CVFL' == data:
flask_app.modeselect('findlineCV')
elif 'CVFLColorSet' in data:
color = int(data.split()[1])
flask_app.camera.colorSet(color)
elif 'CVFLL1' in data:
pos = int(data.split()[1])
flask_app.camera.linePosSet_1(pos)
elif 'CVFLL2' in data:
pos = int(data.split()[1])
flask_app.camera.linePosSet_2(pos)
elif 'CVFLSP' in data:
err = int(data.split()[1])
flask_app.camera.errorSet(err)
elif 'defEC' in data:#Z
fpv.defaultExpCom()
elif(isinstance(data,dict)):
if data['title'] == "findColorSet":
color = data['data']
flask_app.colorFindSet(color[0],color[1],color[2])
if not functionMode:
if OLED_connection:
screen.screen_show(5,'Functions OFF')
else:
pass
print(data)
response = json.dumps(response)
await websocket.send(response)
def setup():
move.setup()