I'm Thad.

SPOT

I wanted to dive into computer vision. I had a little bit of background with IMAQ Vision on the NI CompactRIO through FRC, but this is a very specialized system that doesn’t exist in other places. OpenCV is a very popular library for processing images which has ports for nearly any system. What better use of vision processing than making something to follow you around?

The robot I built was rather simple. I used some scrap 1/16” walled trapezoid tube to create a basic frame. It is 28 inches wide and 18 inches long, with four rails running in the y dimension, and two rails in the x dimension, so as to make wells for the wheels. Large (8”) corner gussets are used in the back for rigidity, as well as some smaller ones in the front.

On top of this frame, the gearboxes are mounted. They are built off of AndyMark Toughboxes. I used the plates from these with some custom standoffs (made of churro tube) so as to make them more serviceable and easier to integrate to other systems. They have two 14 tooth to 50 tooth gear reductions, resulting in a final reduction of 1:12.75. With a CIM motor on each, the maximum free output speed is 390 revolutions per minute. The output shaft goes to a toothed pulley, which meshes with a toothed belt to drive the 6” wheels. The wheels are mounted on static 3/8” bolts with bearings, just beneath the frame. This creates a very smooth, relatively quiet and lossless drivetrain.

In the rear is a ½” bolt that is used as a hitch for wagons. On the front, to counter issues with braking while loaded with a wagon, there are two wheelie bars.

The power wiring for the robot is fairly simple. The battery’s power is connected to a toggle switch, then to a 30 amp auto-resetting breaker. This allows the robot’s power to be easily shut down in case of malfunction, or for Arduino reboots (where the pins are toggled and can send strange commands to the Jaguars). Power and ground are broken out at a terminal block near the front, so as to be easily interfaced with other systems in parallel. From this block, two Jaguar motor controllers are powered, which receive commands from the Arduino (more on this later), and then control power and direction of the motors. Two fans are mounted on the motors (these were found unneeded) for cooling, with a switch to toggle them on and off. A car power plug is mounted on the front, with a 20 amp auto-resetting breaker, for powering an inverter if need be.

An Arduino is located near the front, which is connected over servo style PWM (pulse-width modulation) to send commands to the Jaguars. This is connected over USB to the PC, and the PC sends commands to the Arduino about what the Jaguars should do.

The communication between the Arduino and the PC is rather simple. Since I’m using python on the PC, I’m using PySerial as a quick and robust library to do this. To send out a motor command, the PC gives two ASCII characters- between 4 and 124, with 64 being neutral, 4 being full reverse, and 124 being full forward- followed by ASCII character 0. The Arduino will timeout at 100 milliseconds. So, if there hasn’t been a recent command, or the serial communication to the PC is lost, the Arduino will turn off the motors, for safety purposes should the PC lag, or serial communication lost.

The bulk of programming work went into developing the algorithm to detect and locate targets- and only targets. The high-CPU intensive work is taken care of by a popular, open-source, vision processing library, OpenCV. I chose to work with OpenCV for python because of ease of use with other libraries, and the performance losses are almost none since it is written in C++, just with python wrapping.

The first step into locating targets is getting the image. This is accomplished with a simple camera.read() call.

hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)  

lower_bound = np.array([300/2,60,20])  
upper_bound = np.array([360/2,255,260])  
lower_bound2 = np.array([0/2,60,20])  
upper_bound2 = np.array([30/2,255,260])  

mask1 = cv2.inRange(hsv, lower_bound, upper_bound)  
mask2 = cv2.inRange(hsv, lower_bound2, upper_bound2)  
mask = cv2.bitwise_or(mask1,mask2)  

res = cv2.bitwise_and(image,image, mask= mask) 

After we do this, we find all the contours- the outlines of groups of pixels. Then, we filter out the shapes by their perimeter (rough and quickly), compute the convex hulls of those that remain (which is the smallest bounding convex shape), and then sort these by their areas.

contours, hierarchy = cv2.findContours(mask,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)  

if contours:  
  contours = sorted([cv2.convexHull(contour) for contour in contours if cv2.arcLength(contour, True)>40], key=cv2.contourArea, reverse=True)  

These convex hulled contours are displayed in blue:

After this, we iterate through each contour (from largest to smallest). We perform a set of checks to determine if the object is the target. The first step is to determine the rectangularity. We do this by computing the minimum bounding rectangle of the object, then comparing the area of the object to the area of the bounding rectangle. The rectangles that are computed are shown in red. (Note that as soon as a matching target is found, it will cease to look for any more; only the largest rectangles are drawn) If the object takes up over 80% of the area of the bounding rectangle, we consider it rectangular, and so proceed on to check if the aspect ratio is within 1:2 and 2:1. (Because we ignore rotation)

rect = cv2.minAreaRect(contours[c])  

(x,y),(tw,th),a = rect  
w = min(tw,th)  
h = max(tw,th)  

if h==0: h=0.001  
if abs(contour_area/float(w*h)) > 0.85 and
   0.5<abs(float(h)/w)<2:  

If the object is rectangular, we then need to determine if there is a circle in the middle. We iterate through all contours smaller than the current one, and compute the bounding circle. If the circle is sizable (radius is larger than an eighth of the rectangle’s height), within the rectangle, and the contour takes up at least 50% of the area of the circle, we know that the rectangle has a circle in it. For safety reasons, we finally check to see if the target isn’t too big (too close) or too small (too far; could be a false positive). If all this is good, then we know we’ve found the target to track (shown as the one with a green dot in the middle).

This gets all put together in the full source code in the def find_target() function.

import cv2
import cv2.cv
import cv
import numpy as np
import time
import pygame
from multiprocessing import *
from math import copysign, pi
import serial

class FakeArduino(object):
  def write(self, x):
    pass

class Robot(object):
  def __init__(self, serial_port='COM5'):
    try:
      self.arduino = serial.Serial(serial_port,9600, timeout=5)
      self.arduino.read(12)
    except:
      self.arduino = FakeArduino()
      print 'No connection to arduino; made fake arduino.'


  def drive_lr(self, left, right, throttle=100, max_speed=100):
    throttle*=.60
    max_speed*=.60
    left=float(left)*throttle
    right=float(right)*throttle
    if abs(left) > abs(right):
      if abs(left) > max_speed:
        right = max_speed*(right/abs(left))
        left = max_speed*(left/abs(left))
    else:
      if abs(right) > max_speed:
        left = max_speed*(left/abs(right))
        right = max_speed*(right/abs(right))

    self.arduino.write(chr(64+int(left))+chr(64+int(right))+chr(0))

  def drive_yw(self, y, w, throttle=100, max_speed=100):
    throttle*=.60
    max_speed*=.60
    if y>max_speed: y=max_speed
    elif y<-max_speed: y=-max_speed
    if w>max_speed: w=max_speed
    elif w<-max_speed: w=-max_speed



    left = (float(y)+float(w))*throttle
    right = (float(y)-float(w))*throttle

    if abs(left) > abs(right):
      if abs(left) > max_speed:
        right = max_speed*(right/abs(left))
        left = max_speed*(left/abs(left))
    else:
      if abs(right) > max_speed:
        left = max_speed*(left/abs(right))
        right = max_speed*(right/abs(right))

    self.arduino.write(chr(64+int(left))+chr(64+int(right))+chr(0))

  def stop(self):
    self.arduino.write(chr(64)+chr(64)+chr(0))


class PIDController(object):
  def __init__(self, **kwargs):
    defaults = {'kp':0,'ki':0,'kd':0,'kpe':1,'max_integral':10}
    for d in defaults:
      setattr(self,d,defaults[d])
    for k in kwargs:
      setattr(self,k,kwargs[k])

    self.integral = 0

  def compute(self, new_error):
    self.integral += new_error
    if self.integral > self.max_integral:
      self.integral = self.max_integral
    elif self.integral < -self.max_integral:
      self.integral = -self.max_integral
    return copysign(abs(new_error)**self.kpe, new_error)*self.kp + self.integral*self.ki

pygame.init()
pygame.joystick.init()
joysticks = [pygame.joystick.Joystick(x) for x in range(pygame.joystick.get_count())]
for j in joysticks:
  j.init()




mode = 'track_target'  



def find_target(image):
  image_height, image_width, _ = image.shape
  hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

  lower_bound = np.array([330/2,70,20])
  upper_bound = np.array([360/2,255,260])
  lower_bound2 = np.array([0/2,70,20])
  upper_bound2 = np.array([15/2,255,260])

  mask1 = cv2.inRange(hsv, lower_bound, upper_bound)
  mask2 = cv2.inRange(hsv, lower_bound2, upper_bound2)
  mask = cv2.bitwise_or(mask1,mask2)

  # Bitwise-AND mask and original image
  res = cv2.bitwise_and(image,image, mask= mask)

  contours, hierarchy = cv2.findContours(mask,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)

  if contours:
    contours = sorted([cv2.convexHull(contour) for contour in contours if cv2.arcLength(contour, True)>40], key=cv2.contourArea, reverse=True)
    #cv2.drawContours(res, contours, -1, (255,0,0), 1)

    for c in range(len(contours)):
      contour_area = cv2.contourArea(contours[c])
      if contour_area > 1000:
        rect = cv2.minAreaRect(contours[c])
        #box = cv.BoxPoints(rect)
        #box = np.int0(box)
        #cv2.drawContours(res,[box],0,(0,0,255),2)

        (x,y),(tw,th),a = rect
        w = min(tw,th)
        h = max(tw,th)

        if h==0: h=0.001
        # if the contour takes up 85% of the area of the bounding rectangle AND the ratio of the bounding rectangle is that between a square and two squares stacked
        if abs(contour_area/float(w*h)) > 0.85 and 0.5<abs(float(h)/w)<2:
          # iterate through all the other contours
          for t in range(c-1, len(contours)):
            if t == c:
              continue
            center, radius = cv2.minEnclosingCircle(contours[t])
            #cv2.circle(res, (int(center[0]),int(center[1])), int(radius), (0,255,255), 2)
            # if the radius of the circle is big enough
            if radius > h/8:
              # if the circle is within the rectangle
              if abs(center[0]-x)<w/2 and abs(center[1]-y)<w/2:
                # if the object takes up 50% of the area of the bounding circle
                if abs(cv2.contourArea(contours[t])/((float(radius)**2)*pi)) > 0.65:
                  #cv2.drawContours(res, [contours[c]], -1, (0,255,0), 3)
                  cv2.circle(res, (int(x),int(y)), 3, (0,255,0), 4)
                  # if the rectangle is in a safe zone
                  if 50 < h < 270:
                    # return the target info
                    cv2.imshow('Target Finding Result', res)
                    return {'x':x, 'y':y, 'w':w, 'h':h, 'image_width':image_width, 'image_height':image_height, 'rel_x':(x-(image_width/2))/(image_width/2), 'rel_y':(y-(image_height/2))/(image_height/2)}
  cv2.imshow('Target Finding Result', res)
  return None


def talk_to_robot(*args):
  camera = cv2.VideoCapture(0)
  robot = Robot('COM5')
  w_controller = PIDController(kp=0.57,kpe=0.5)
  target_y_controller = PIDController(kp=2.7,kpe=1.2)
  while True:
    if mode=='joystick_yw':
      pygame.event.get()
      robot.drive_yw(-joysticks[0].get_axis(1), joysticks[0].get_axis(3), 50, 50)
      time.sleep(0.02)
    elif mode=='joystick_lr':
      pygame.event.get()
      robot.drive_lr(-joysticks[0].get_axis(1), -joysticks[0].get_axis(2), 50, 50)
      time.sleep(0.02)

    elif mode=='track_target':
      _, rgb = camera.read()  
      target_info = find_target(rgb)
      if target_info:
        robot.drive_yw(target_y_controller.compute((150- target_info['h'])/100), w_controller.compute(target_info['rel_x']), 40, 45)
      else:
        robot.stop()

      cv2.imshow('Raw Webcam Image', rgb)
      k = cv2.waitKey(3) & 0xFF
      if k == 27:
        break

    else:
      robot.stop()

if __name__ == '__main__':
  talk_to_robot()