Object-Follower-Robot/object_follower.py at master · CharithGR/Object-Follower-Robot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
from picamera.array import PiRGBArray
from picamera import PiCamera
from gpiozero import Servo
from gpiozero import Motor
import time
import cv2
import numpy as np

#function to filter the video feed for only the required color using HSV values
def segment_colour(frame):
    hsv_roi =  cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(hsv_roi, np.array([160, 160,10]), np.array([190,255,255]))
    kern_dilate = np.ones((8,8),np.uint8)
    kern_erode  = np.ones((3,3),np.uint8)
    mask= cv2.erode(mask,kern_erode)
    mask=cv2.dilate(mask,kern_dilate)
    cv2.imshow('mask',mask)
    return mask

# function to find the contour of the object of the chosen and enclose it in a rectangle
def find_blob(blob):
    largest_contour=0
    cont_index=0
    _, contours, hierarchy = cv2.findContours(blob, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE)
    for idx, contour in enumerate(contours):
        area=cv2.contourArea(contour)
        if (area >largest_contour) :
            largest_contour=area
            cont_index=idx
    r=(0,0,2,2)
    if len(contours) > 0:
        r = cv2.boundingRect(contours[cont_index])
    return r,largest_contour

def target_hist(frame):
    hsv_img=cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    hist=cv2.calcHist([hsv_img],[0],None,[50],[0,255])
    return hist

# picamera settings (change according to convinience)
camera = PiCamera()
camera.rotation = 270
camera.brightness = 51
camera.resolution = (480, 368)
camera.framerate = 16
rawCapture = PiRGBArray(camera, size=(480, 368))

motorR = Motor(23,18)
motorL = Motor(25,24)
last = 0

time.sleep(0.01)

# take continuous video feed from camera, track the object and follow it
# using the co-ordinates of the centre of rectangle and it's area (to measure how far/close it is)
# proportional control is used to control the speed of the motors
for image in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
      frame = image.array
      global centre_x
      global centre_y
      centre_x=0.
      centre_y=0.
      hsv1 = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
      mask_red=segment_colour(frame)
      loct,area=find_blob(mask_red)
      x,y,w,h=loct

      if (w*h) < 10:
            found = 0
            if last==0:
                print('pivot left')
                motorR.forward(0.75)
                motorL.backward(0.75)
            else:
                print('pivot right')
                motorL.forward(0.75)
                motorR.backward(0.75)
      else:
            found = 1
            simg2 = cv2.rectangle(frame, (x,y), (x+w,y+h), 255,2)
            cv2.imshow('simg2',simg2)
            centre_x=x+((w)/2)
            centre_y=y+((h)/2)
            cv2.circle(frame,(int(centre_x),int(centre_y)),3,(0,110,255),-1)
            print (centre_x,centre_y)
            print(w*h)
            value = centre_x/160
            if (w*h) < 24000:
                if centre_x < 160:
                    motorR.forward(1)
                    motorL.forward(value)
                    print('left:',value)
                    last = 0
                elif centre_x < 320:
                    motorR.forward(1)
                    motorL.forward(1)
                    print('front')
                else:
                    motorR.forward((2.999 - value))
                    motorL.forward(1)
                    print('right:',2.999 - value)
                    last = 1
            elif (w*h) < 28000:
                if centre_x < 160:
                    print('left')
                    motorR.forward(0.5)
                    motorL.stop()
                    last = 0
                elif centre_x < 320:
                    print('stop')
                    motorR.stop()
                    motorL.stop()
                else:
                    print('right')
                    motorL.forward(0.5)
                    motorR.stop()
                    last = 1
            else:
                print('reverse')
                motorR.backward(0.5)
                motorL.backward(0.5)

      rawCapture.truncate(0)
      cv2.imshow('stuff',frame)
      if(cv2.waitKey(1) & 0xff == ord('q')):
            break
cv2.destroyAllWindows()