ラズパイとopencv，wiringpiを組み合わせた赤色物体の追尾ロボットの高速化

実現したいこと

picameraを搭載したラズパイとopencvを組み合わせ，画像処理で赤色の物体を認識し，その物体を追跡するようにwiringpiを使用しモーターを作動させる．

すでにできていること

画像認識とモーターの作動，赤色の物体の位置によってモーターの回転方向の切り替えさらが実現している
###問題点
出力先をロボットのモーターではなくモーター単体で実験的に動作ができているか確認することはできているがいざロボットに繋いでみるとモーターの回転数が早く，制御が追いつかない．
繋いだロボットが本番機ではなく実験機なのでモーターの回転数が下がる予定で制御は間に合う可能性はあるが心配なので制御を早くしたい．

該当のソースコード

python3
1import wiringpi as pi
2import cv2
3import numpy as np
4import picamera
5
6picamera.resolution=(640,480)
7
8pi.wiringPiSetupGpio()
9pi.pinMode(14,1)
10pi.pinMode(15,1)
11pi.pinMode(23,1)
12pi.pinMode(24,1)
13
14def OUT_PUT(set):
15  if 220<=set<=420:
16   pi.digitalWrite(14,0)
17   pi.digitalWrite(15,1)
18   pi.digitalWrite(23,0)
19   pi.digitalWrite(24,1)
20  elif 0<=set<220:
21   pi.digitalWrite(14,1)
22   pi.digitalWrite(15,0)
23   pi.digitalWrite(23,0)
24   pi.digitalWrite(24,1)
25  elif 420<set<=640:
26   pi.digitalWrite(14,0)
27   pi.digitalWrite(15,1)
28   pi.digitalWrite(23,1)
29   pi.digitalWrite(24,0)
30  else:
31   pi.digitalWrite(14,1)
32   pi.digitalWrite(15,0)
33   pi.digitalWrite(23,0)
34   pi.digitalWrite(24,1)
35
36def find_rect_of_target_color(image):
37  hsv= cv2.cvtColor(image, cv2.COLOR_BGR2HSV_FULL)
38  h = hsv[:,:,0]
39  s = hsv[:,:,1]
40  mask = np.zeros(h.shape, dtype=np.uint8)
41  mask[((h < 20) | (h > 200)) & (s > 128)] = 255
42  _, contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
43  rects = []
44  for contour in contours:
45    approx = cv2.convexHull(contour)
46    rect = cv2.boundingRect(approx)
47    rects.append(np.array(rect))
48    x=(rect[0]+(rect[2]/2))
49    y=OUT_PUT(x)
50  return rects
51
52if __name__ == "__main__":
53  capture = cv2.VideoCapture(0)
54  while cv2.waitKey(30) < 0:
55    _, frame = capture.read()
56    rects = find_rect_of_target_color(frame)
57    if len(rects) > 0:
58      rect = max(rects, key=(lambda x: x[2] * x[3]))
59      cv2.rectangle(frame, tuple(rect[0:2]), tuple(rect[0:2] + rect[2:4]), (0, 0, 255), thickness=2)
60    cv2.imshow('red', frame)
61  capture.release()
62  cv2.destroyAllWindows()
63  pi.digitalWrite(14,0)
64  pi.digitalWrite(15,0)
65  pi.digitalWrite(23,0)
66  pi.digitalWrite(24,0)