Pan and Tilt Servos Not responding

[pageauc]

Received my Dawn Robot yesterday. Got it running on my RPI B+ including streaming except pan/tilt does not work. Running on DAWN USB Battery Pack. Needed to usb plug the RPI separately into battery pack and not through UBEC since RPI would stop once load put on drive motors.
My Problem is that neither pan/tilt servo's are responding at all. Did robot_control_test.py and drive motors turn but no pan/tilt. Program exits with connection=True message after drive motors move a bit. Verified that pan/tilt connections look correct per instructions and photo. D3 and D4 with yellow wires towards red light on board. Not sure how to trouble shoot any further. I have an OpenElectrons pan/tilt and was thinking of trying it in Dawn Robot to see if it might be problem with dagu ardurino board but decided it was better to try to trouble shoot
With neither pan/tilt servo working I am thinking it cant be a faulty servo but rather some communication problem. I have downloaded the latest dawn rpi B+ image as well as using my own image that I built.
Let me know how to trouble shoot this problem so I can determine cause.

Thanks
Claude

[pageauc]

Well got pan/tilt working. I thought it might be power related since my Openelectrons.com pan/tilt moves the servo's a little when it is booted. So I RTFM again. In looking at the photo of the power and motor connections I noticed there was a jumper between the first two pins with the red UBEC wire in the far right pin (facing with the drive motor h-bridge connection at the far end.). This was not made clear in the instructions and a jumper was not provided. Dug up a jumper out of my computer building box of stuff, inserted and success. The servo does indeed make a small activation noise on boot. I now have a usable robot that I can start to work with. Thought the pan/tilt was a little slow to move. Already ran the web configuration web page and adjusted the drive motors to go straight. I will try to adjust the pan/tilt to see if I can speed up it's response time. Looks like it is moving at one unit at a time. Maybe more increments would allow it to pan/tilt faster.
I have opencv installed and working using picamera python module and would like to integrate some behaviors so the robot can look for a face and track via pan/tilt and wheels.
Here is my test code fyi. This code will need to be changed to work with the Dawn Robot pan/tilt camera.

Code: Select all

#!/usr/bin/env python
# opencv-find-face : Opencv face tracking with pan/tilt search and lock
# written by Claude Pageau -
# This is a little laggy but does work OK.
# Uses pipan.py module from openelectrons.com RPI camera pan/tilt to control
# camera tracking or use your own pan/tilt module and modify code accordingly.
# if you are not using openelectrons.com pan/tilt hardware.
# Also picamera python module must be installed as well as opencv
# To install opencv and python for opencv
# sudo apt-get install libopencv-dev python-opencv
# To install picamera python module
# sudo apt-get install python-picamera
# You will also need to install python picamera.array tha includes numpy
# sudo pip install "picamera[array]"
# copy /usr/share/opencv/haarcascades/haarcascade_frontalface_default.xml
# to same folder that this python script is in.
#    Note
# v4l2 driver is not used since stream is created using picamera module
# using picamera.array
# If you have any questions email pageauc@gmail.com
 
print "Initializing ...."
import io
import time
import picamera
import picamera.array
import cv2
import numpy as np

# openelectron.com python module and files from the OpenElectron RPI camera pan/tilt
# Copy pipan.py to same folder as this script.
import pipan

p = pipan.PiPan()

# To speed things up, lower the resolution of the camera
CAMERA_WIDTH = 320
CAMERA_HEIGHT = 240
# Show opencv window
show_window = False

# Camera center of image
cam_cx = CAMERA_WIDTH / 2
cam_cy = CAMERA_HEIGHT / 2
inch = 9.0

# bounds checking for pan/tilt search.
limit_y_bottom = 90
limit_y_top = 150
limit_y_level = 140
limit_x_left = 60
limit_x_right = 240

# Approx Center of Pan/Tilt motion
pan_x_c = 120
pan_y_c = 130

# Set Initial starting position of pan/tilt
pan_cx = pan_x_c
pan_cy = pan_y_c

# Amount pan/tilt moves when searching
pan_move_x = 30
pan_move_y = 20

# Timer seconds to wait before starting pan/tilt search for face.
# local face search
face_timer1 = 15
# Wide face search
face_timer2 = 30
# Stop pan/tilt and start Motion Detect.
face_timer3 = 45

# Motion scan settings
motion_detected = False
# sensitivity - How much the color value (0-255) needs to change to be considered a change
sensitivity = 25
# threshold - How many pixels must change to be considered motion
threshold = CAMERA_WIDTH * CAMERA_HEIGHT * 2 / 110

# Move the pan/tilt to a specific location. has built in limit checks.
def pan_goto(x,y):
   p.do_pan (int(x))
   p.do_tilt (int(y))

def motion_scan(x,y):
   print "motion_scan - Scan for Motion at cx=%d cy=%d" % (x, y)
   pan_goto(x,y)
   numImages = 0
   step = 1  # use this to toggle where the image gets saved
   captureCount = 0 # flag used to begin a sequence capture
   stream = io.BytesIO()
   with picamera.PiCamera() as camera:
      camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT)
      time.sleep(2)

      # begin motion scan
      while sensitivity > 0:
         camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT)
 
         if step == 1:
             stream.seek(0)
             camera.capture(stream, 'bgra', True)
             data1 = np.fromstring(stream.getvalue(), dtype=np.uint8)
             step = 2
         else:
             stream.seek(0)
             camera.capture(stream, 'bgra', True)
             data2 = np.fromstring(stream.getvalue(), dtype=np.uint8)
             step = 1
         numImages = numImages + 1
         if numImages > 4:  # ignore first few images because if the camera is not quite ready it will register as motion right away
             if captureCount <= 0:
                 # not capturing, test for motion (very simplistic, but works good enough for my purposes)
                 data3 = np.abs(data1 - data2)  # get difference between 2 successive images

                 # There are 4 times the number of pixels due to rgba
                 numTriggers = np.count_nonzero(data3 > sensitivity) / 4 / sensitivity

                 if numTriggers > threshold:
                    print "motion_scan - Motion Detected. Threshold=%d Triggers=%d"  % (threshold, numTriggers)
                    stream.close()
                    return True   
   
def face_scan(x,y):
  print "face_scan - Start Scan at cx=%d cy=%d" % (x, y)
  pan_parked = False
  # Face detection opencv center of face box
  face_cx = x
  face_cy = y
  # Pan/Tilt motion center point
  pan_cx = x
  pan_cy = y
   
  # Put pan/tilt in a known good position.
  pan_goto(pan_x_c, pan_y_c)   
  face_found = False
  start_time = time.time()
 
  # load a cascade file for detecting faces. This file must be in
  # same folder as this script. or loaded as part of opencv at /usr/share/opencv/haarcascades/
  face_cascade = cv2.CascadeClassifier('/usr/share/opencv/haarcascades/haarcascade_frontalface_default.xml')
  # face_cascade = cv2.CascadeClassifier('/usr/share/opencv/haarcascades/haarcascade_profileface.xml')
  # Saving the picture to an in-program stream rather than a file
  stream = io.BytesIO()
  with picamera.PiCamera() as camera:
    camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT)
    camera.vflip = True
   
    while(True):
      with picamera.array.PiRGBArray(camera) as stream:
        camera.capture(stream, format='bgr')
        # At this point the image is available as stream.array
        image = stream.array
       
      # Convert to grayscale, which is easier
      gray = cv2.cvtColor(image,cv2.COLOR_BGR2GRAY)
      # Look for faces over the given image using the loaded cascade file
      faces = face_cascade.detectMultiScale(gray, 1.3, 5)
   
      for (x,y,w,h) in faces:
        face_found = True
        #print "face_scan - Face Found at x=%d y=%d w=%d h=%d" % (x, y, w, h)
        pan_parked = False
        start_time = time.time()
        distance = ((CAMERA_WIDTH/w) * inch)/12
        face_cx = x + w/2
        Nav_LR = (cam_cx - face_cx) / 5
        pan_cx = pan_cx - Nav_LR
        face_cy = y + h/2
        Nav_UD = (cam_cy - face_cy) / 4
        pan_cy = pan_cy - Nav_UD
        # Print Navigation required to center face in image
        if ( abs(Nav_LR)>5 or abs(Nav_UD)>3 ):
           pan_goto(pan_cx, pan_cy)
           print "face_scan - Nav LR=%s UD=%s  Range=%.1f ft" % (Nav_LR, Nav_UD, distance)   
       
        if show_window:
          # Opencv has built in image manipulation functions
            cv2.rectangle(image,(x,y),(x+w,y+h),(255,0,0),2)

      # Use opencv built in window to show the image
      # Leave out if your Raspberry Pi isn't set up to display windows
      if show_window:
         cv2.imshow('Face Image', image)

      elapsed_time = time.time() - start_time         
      # start pan/tilt search for face if timer runs out
      if elapsed_time > face_timer3:
          if not pan_parked:
             pan_parked = True
             stream.close()
             # Return and start motion capture
             return (pan_cx, pan_cy, False)
         
      elif elapsed_time > face_timer2:
          face_found = False
#          print "face_scan - Wide Search Timer2=%d  > %s seconds" % (elapsed_time, face_timer2)
          pan_cx = pan_cx + pan_move_x
          if pan_cx > limit_x_right:
             pan_cx = limit_x_left         
             pan_cy = pan_cy + pan_move_y
             if pan_cy > limit_y_top:
                pan_cy = limit_y_bottom
          pan_goto (pan_cx, pan_cy)
   
      elif elapsed_time > face_timer1:
          face_found = False
#          print "face_scan - Local Search  Timer1=%d  > %s seconds" % (elapsed_time, face_timer1)
          pan_cx = pan_cx + pan_move_x
          if (pan_cx > limit_x_right - (pan_move_x * 2)):
             pan_cx = limit_x_left + pan_move_x         
             pan_cy = pan_cy + pan_move_y
             if (pan_cy > limit_y_top - pan_move_y):
                pan_cy = limit_y_bottom + pan_move_y
          pan_goto (pan_cx, pan_cy)


         
      if cv2.waitKey(1) & 0xFF == ord('q'):
         quit()
         #return (pan_cx, pan_cy, True)
      
# ---------------- Start Main Program

motion_detected = False
q_pressed = False
while (not q_pressed):
  if motion_detected:
    (pan_cx, pan_cy, motion_detected) = face_scan(pan_x_c, pan_y_c)
    if motion_detected:
       quit()
       #q_pressed = True   
  else:
    motion_detected = motion_scan(pan_x_c, pan_y_c)
       
# Close Window
print "Exiting Program ...."
cv2.destroyAllWindows()
camera.close()
quit()
 


[Alan]

Dear Claude,

Was just about to post a servo troubleshooting post, when I saw your reply. Glad you got it working, but really sorry that no jumper was included. We get the mini driver boards sealed from the factory and that jumber should definitely have been there.

Just to confirm, there should be a total of 5 jumpers on the board 4 on pins D7 to D10 connecting the Atmega8 to the motor driver and one which can be used to set the servo voltage rail to either 5V or battery voltage.

Test code looks nice. Pan/tilt should be fairly quick via the API, I think that I limited the speed on the web interface to stop people getting sick.

Regards

Alan

[pageauc]

Thanks Alan

Took a while to trouble shoot but I think you learn more when things don't work right away. Also tried to install a shorter USB cable between RPI and Dagu Arduino. Power but no controls worked. Installed previous cable and success. Looks like the shorter cable was a power only usb so no serial communications getting through. Life if full of little bumps in the road.
Thanks for your prompt reply
Claude ..

[pageauc]

Also FYI. The power pack left usb port is 2.1A while the right is 1A. This is marked on the power pack. Have not tried best configuration but will try UBEC plugged into 2.1A to see if it prevents RPI from spontaneously rebooting when motors run. My current config has RPI direct plugged into 2.1A usb and Dagu into 1A. This does work but I think I will do more testing. Will let you know if find out anything conclusive.
Claude ..