text: import os
  import cv2
  from base_camera import BaseCamera
  import RPIservo
  import numpy as np
  import move
  import switch
  import datetime
  import Kalman_filter
  import PID
  import time
  import threading
  import imutils
  import robotLight
  
  led = robotLight.RobotLight()
  pid = PID.PID()
  pid.SetKp(0.5)
  pid.SetKd(0)
  pid.SetKi(0)
  
  CVRun = 1
  linePos_1 = 440
  linePos_2 = 380
  lineColorSet = 255
  frameRender = 1
  findLineError = 20
  
  colorUpper = np.array([44, 255, 255])
  colorLower = np.array([24, 100, 100])
  
  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 = 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)
              # switch.switch(1,1)
              # switch.switch(2,1)
              # switch.switch(3,1)
  
          if (timestamp - self.lastMovtionCaptured).seconds >= 0.5:
              led.setColor(0,78,255)
              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.1*(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:
              if CVThread.Y_lock == 1:
                  led.setColor(255,78,0)
                  # switch.switch(1,1)
                  # switch.switch(2,1)
                  # switch.switch(3,1)
              else:
                  led.setColor(0,78,255)
                  # 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
  
  
  class Camera(BaseCamera):
      video_source = 0
      modeSelect = 'none'
      # modeSelect = 'findlineCV'
      # modeSelect = 'findColor'
      # modeSelect = 'watchDog'
  
  
      def __init__(self):
          if os.environ.get('OPENCV_CAMERA_SOURCE'):
              Camera.set_video_source(int(os.environ['OPENCV_CAMERA_SOURCE']))
          super(Camera, self).__init__()
  
  
      def colorFindSet(self, invarH, invarS, invarV):
          global colorUpper, colorLower
          HUE_1 = invarH+15
          HUE_2 = invarH-15
          if HUE_1>180:HUE_1=180
          if HUE_2<0:HUE_2=0
  
          SAT_1 = invarS+150
          SAT_2 = invarS-150
          if SAT_1>255:SAT_1=255
          if SAT_2<0:SAT_2=0
  
          VAL_1 = invarV+150
          VAL_2 = invarV-150
          if VAL_1>255:VAL_1=255
          if VAL_2<0:VAL_2=0
  
          colorUpper = np.array([HUE_1, SAT_1, VAL_1])
          colorLower = np.array([HUE_2, SAT_2, VAL_2])
          print('HSV_1:%d %d %d'%(HUE_1, SAT_1, VAL_1))
          print('HSV_2:%d %d %d'%(HUE_2, SAT_2, VAL_2))
          print(colorUpper)
          print(colorLower)
  
      def modeSet(self, invar):
          Camera.modeSelect = invar
  
      def CVRunSet(self, invar):
          global CVRun
          CVRun = invar
  
      def linePosSet_1(self, invar):
          global linePos_1
          linePos_1 = invar
  
      def linePosSet_2(self, invar):
          global linePos_2
          linePos_2 = invar
  
      def colorSet(self, invar):
          global lineColorSet
          lineColorSet = invar
  
      def randerSet(self, invar):
          global frameRender
          frameRender = invar
  
      def errorSet(self, invar):
          global findLineError
          findLineError = invar
  
      @staticmethod
      def set_video_source(source):
          Camera.video_source = source
  
      @staticmethod
      def frames():
          camera = cv2.VideoCapture(Camera.video_source)
          if not camera.isOpened():
              raise RuntimeError('Could not start camera.')
  
          cvt = CVThread()
          cvt.start()
  
          while True:
              # read current frame
              _, img = camera.read()
              if img.all is None:
                  continue
  
              if Camera.modeSelect == 'none':
                  switch.switch(1,0)
                  cvt.pause()
              else:
                  if cvt.CVThreading:
                      pass
                  else:
                      cvt.mode(Camera.modeSelect, img)
                      cvt.resume()
                  try:
                      img = cvt.elementDraw(img)
                  except:
                      pass
              
  
  
              # encode as a jpeg image and return it
              if cv2.imencode('.jpg', img)[0]:
                  yield cv2.imencode('.jpg', img)[1].tobytes()
  