text: # -*- coding: utf-8 -*-
  """
  Created on Sat Feb 26 20:21:06 2022
  
  @author: udaya
  """
  
  from djitellopy import Tello
  import cv2
  import numpy as np
  
  
  ######################################################################
  width = 640  # WIDTH OF THE IMAGE
  height = 480  # HEIGHT OF THE IMAGE
  deadZone =100
  ######################################################################
  
  startCounter =0
  
  # CONNECT TO TELLO
  mytello = Tello()
  mytello.connect()
  mytello.for_back_velocity = 0
  mytello.left_right_velocity = 0
  mytello.up_down_velocity = 0
  mytello.yaw_velocity = 0
  mytello.speed = 0
  
  
  
  print(mytello.get_battery())
  
  mytello.streamoff()
  mytello.streamon()
  ######################## 
  
  frameWidth = width
  frameHeight = height
  # cap = cv2.VideoCapture(1)
  # cap.set(3, frameWidth)
  # cap.set(4, frameHeight)
  # cap.set(10,200)
  
  
  global imgContour
  global dir
  def empty(a):
      pass
  
  cv2.namedWindow("HSV")
  cv2.resizeWindow("HSV",640,240)
  cv2.createTrackbar("HUE Min","HSV",0,179,empty)
  cv2.createTrackbar("HUE Max","HSV",75,179,empty)
  cv2.createTrackbar("SAT Min","HSV",105,255,empty)
  cv2.createTrackbar("SAT Max","HSV",245,255,empty)
  cv2.createTrackbar("VALUE Min","HSV",80,255,empty)
  cv2.createTrackbar("VALUE Max","HSV",140,255,empty)
  
  cv2.namedWindow("Parameters")
  cv2.resizeWindow("Parameters",640,240)
  cv2.createTrackbar("Threshold1","Parameters",166,255,empty)
  cv2.createTrackbar("Threshold2","Parameters",171,255,empty)
  cv2.createTrackbar("Area","Parameters",4410,30000,empty)
  
  
  def stackImages(scale,imgArray):
      rows = len(imgArray)
      cols = len(imgArray[0])
      rowsAvailable = isinstance(imgArray[0], list)
      width = imgArray[0][0].shape[1]
      height = imgArray[0][0].shape[0]
      if rowsAvailable:
          for x in range ( 0, rows):
              for y in range(0, cols):
                  if imgArray[x][y].shape[:2] == imgArray[0][0].shape [:2]:
                      imgArray[x][y] = cv2.resize(imgArray[x][y], (0, 0), None, scale, scale)
                  else:
                      imgArray[x][y] = cv2.resize(imgArray[x][y], (imgArray[0][0].shape[1], imgArray[0][0].shape[0]), None, scale, scale)
                  if len(imgArray[x][y].shape) == 2: imgArray[x][y]= cv2.cvtColor( imgArray[x][y], cv2.COLOR_GRAY2BGR)
          imageBlank = np.zeros((height, width, 3), np.uint8)
          hor = [imageBlank]*rows
          hor_con = [imageBlank]*rows
          for x in range(0, rows):
              hor[x] = np.hstack(imgArray[x])
          ver = np.vstack(hor)
      else:
          for x in range(0, rows):
              if imgArray[x].shape[:2] == imgArray[0].shape[:2]:
                  imgArray[x] = cv2.resize(imgArray[x], (0, 0), None, scale, scale)
              else:
                  imgArray[x] = cv2.resize(imgArray[x], (imgArray[0].shape[1], imgArray[0].shape[0]), None,scale, scale)
              if len(imgArray[x].shape) == 2: imgArray[x] = cv2.cvtColor(imgArray[x], cv2.COLOR_GRAY2BGR)
          hor= np.hstack(imgArray)
          ver = hor
      return ver
  
  def getContours(img,imgContour):
      global dir
      contours, hierarchy = cv2.findContours(img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
      for cnt in contours:
          area = cv2.contourArea(cnt)
          areaMin = cv2.getTrackbarPos("Area", "Parameters")
          if area > areaMin:
              cv2.drawContours(imgContour, cnt, -1, (255, 0, 255), 7)
              peri = cv2.arcLength(cnt, True)
              approx = cv2.approxPolyDP(cnt, 0.02 * peri, True)
              #print(len(approx))
              x , y , w, h = cv2.boundingRect(approx)
              cx = int(x + (w / 2))  # CENTER X OF THE OBJECT
              cy = int(y + (h / 2))  # CENTER X OF THE OBJECT
  
              if (cx <int(frameWidth/2)-deadZone):
                  cv2.putText(imgContour, " GO LEFT " , (20, 50), cv2.FONT_HERSHEY_COMPLEX,1,(0, 0, 255), 3)
                  cv2.rectangle(imgContour,(0,int(frameHeight/2-deadZone)),(int(frameWidth/2)-deadZone,int(frameHeight/2)+deadZone),(0,0,255),cv2.FILLED)
                  dir = 1
              elif (cx > int(frameWidth / 2) + deadZone):
                  cv2.putText(imgContour, " GO RIGHT ", (20, 50), cv2.FONT_HERSHEY_COMPLEX,1,(0, 0, 255), 3)
                  cv2.rectangle(imgContour,(int(frameWidth/2+deadZone),int(frameHeight/2-deadZone)),(frameWidth,int(frameHeight/2)+deadZone),(0,0,255),cv2.FILLED)
                  dir = 2
              elif (cy < int(frameHeight / 2) - deadZone):
                  cv2.putText(imgContour, " GO UP ", (20, 50), cv2.FONT_HERSHEY_COMPLEX,1,(0, 0, 255), 3)
                  cv2.rectangle(imgContour,(int(frameWidth/2-deadZone),0),(int(frameWidth/2+deadZone),int(frameHeight/2)-deadZone),(0,0,255),cv2.FILLED)
                  dir = 3
              elif (cy > int(frameHeight / 2) + deadZone):
                  cv2.putText(imgContour, " GO DOWN ", (20, 50), cv2.FONT_HERSHEY_COMPLEX, 1,(0, 0, 255), 3)
                  cv2.rectangle(imgContour,(int(frameWidth/2-deadZone),int(frameHeight/2)+deadZone),(int(frameWidth/2+deadZone),frameHeight),(0,0,255),cv2.FILLED)
                  dir = 4
              else: dir=0
  
              cv2.line(imgContour, (int(frameWidth/2),int(frameHeight/2)), (cx,cy),(0, 0, 255), 3)
              cv2.rectangle(imgContour, (x, y), (x + w, y + h), (0, 255, 0), 5)
              cv2.putText(imgContour, "Points: " + str(len(approx)), (x + w + 20, y + 20), cv2.FONT_HERSHEY_COMPLEX, .7,(0, 255, 0), 2)
              cv2.putText(imgContour, "Area: " + str(int(area)), (x + w + 20, y + 45), cv2.FONT_HERSHEY_COMPLEX, 0.7,(0, 255, 0), 2)
              cv2.putText(imgContour, " " + str(int(x)) + " " + str(int(y)), (x - 20, y - 45), cv2.FONT_HERSHEY_COMPLEX,0.7,(0, 255, 0), 2)
              return [[cx,cy],area]
          else: 
              return [[0,0],0]
          
          
  
  def display(img):
      cv2.line(img,(int(frameWidth/2)-deadZone,0),(int(frameWidth/2)-deadZone,frameHeight),(255,255,0),3)
      cv2.line(img,(int(frameWidth/2)+deadZone,0),(int(frameWidth/2)+deadZone,frameHeight),(255,255,0),3)
      cv2.circle(img,(int(frameWidth/2),int(frameHeight/2)),5,(0,0,255),5)
      cv2.line(img, (0,int(frameHeight / 2) - deadZone), (frameWidth,int(frameHeight / 2) - deadZone), (255, 255, 0), 3)
      cv2.line(img, (0, int(frameHeight / 2) + deadZone), (frameWidth, int(frameHeight / 2) + deadZone), (255, 255, 0), 3)
  
  pid = [0.3, 0.5, 0]
  fbRange = [6200, 6800]
  pError = 0
  def trackObj(tello, center, area, w, pid, pError):
      fb = 0
      x,y = center[0], center[1]
      error = x - w/2
      yaw = pid[0] * error + pid[1] * (error - pError)
      yaw = int(np.clip(yaw, -100,100))
      if area > fbRange[0] and area < fbRange[1]:
          fb = 0
      elif area > fbRange[1]:
          fb = -20
      elif area < fbRange[0] and area != 0:
          fb = 20
      if x == 0:
          yaw = 0
          error = 0
      tello.send_rc_control(0, fb, 0, yaw)
      return error
  
  # cap   = cv2.VideoCapture(0)
  width = 640  # WIDTH OF THE IMAGE
  height = 480  # HEIGHT OF THE IMAGE
  deadZone =100
  pts = [(0,0)]*10
  ctr = 0
  while True:
      # success, myFrame = cap.read()
      myFrame = mytello.get_frame_read().frame
      img = cv2.resize(myFrame, (width, height))
      imgContour = img.copy()
      imgHsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
  
      h_min = cv2.getTrackbarPos("HUE Min","HSV")
      h_max = cv2.getTrackbarPos("HUE Max", "HSV")
      s_min = cv2.getTrackbarPos("SAT Min", "HSV")
      s_max = cv2.getTrackbarPos("SAT Max", "HSV")
      v_min = cv2.getTrackbarPos("VALUE Min", "HSV")
      v_max = cv2.getTrackbarPos("VALUE Max", "HSV")
  
      greenLower = (h_min,s_min,v_min)
      greenUpper = (h_max,s_max,v_max)
      mask = cv2.inRange(imgHsv, greenLower, greenUpper)
      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
  
      # only proceed if at least one contour was found
      if len(cnts) > 0:
          # find the largest contour in the mask, then use
          # it to compute the minimum enclosing circle and
          # centroid
          c = max(cnts, key=cv2.contourArea)
          ((x, y), radius) = cv2.minEnclosingCircle(c)
          M = cv2.moments(c)
          area = cv2.contourArea(c)
          center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
  
          # only proceed if the radius meets a minimum size
          if radius > 10:
                  # draw the circle and centroid on the frame,
                  # then update the list of tracked points
                  cv2.circle(img, (int(x), int(y)), int(radius),
                          (0, 255, 255), 2)
                  cv2.circle(img, center, 5, (0, 0, 255), -1)
          pts[ctr%10] = center
          ctr = ctr+1
  
      # loop over the set of tracked points
          for i in range(1, len(pts)):
                  # if either of the tracked points are None, ignore
                  # them
                  if pts[i - 1] is None or pts[i] is None:
                          continue
                  cv2.line(img, pts[i - 1], pts[i], (0, 0, 255), 1)
  
          # show the frame to our screen
          pError = trackObj(mytello, center, area, width, pid, pError)
      if startCounter == 0:
          mytello.takeoff()
          startCounter = 1
  
      cv2.imshow("Frame", img)
      key = cv2.waitKey(1) & 0xFF
      if key == ord("q"):
          mytello.land()
          break
  
  #cap.release()
  cv2.destroyAllWindows()
  