How to Program a SunFounder PiDog

Introduction

Last time, the SunFounder PiDog was introduced, this time we’ll introduce how to program the PiDog to do our bidding. Previously, we looked at the SunFounder PiCar and how to program it. Both robots share the same RobotHat to interface the Raspberry Pi with the various servos, motors and devices attached to the robot. For the PiCar this is fairly simple as you just need to turn on the motors to go and set the steering servo to the angle you want to turn. The PiDog is much more complicated. There are eight servo motors that control the legs. On each leg, one servo sets the angle of the shoulder joint and the other sets the angle of the elbow joint. To get the PiDog to walk takes coordinating the setting of each of these servos in a coordinated fashion. SunFounder provides a PiDog Python class that hides most of the complexity, though it does give you direct access if you want to control the legs directly.

Introduction to the PiDog Class

To use the PiDog class, you need to instantiate a PiDog object and then start issuing commands. A common initialization sequence is:

       dog = Pidog()
       dog.do_action('stand', speed=80)
       dog.wait_all_done()

The first line creates a PiDog object to use, then the next line calls the do_action method which has a long list of standard actions. In the case of ‘stand’, it will set the eight leg servo motors to put the PiDog in the standing position. This is much easier than having to set all eight servo positions ourselves. Then you need to wait for this operation to complete before issuing another command, which is accomplished by calling the wait_all_done method.

The PiDog class uses multithreading so you can issue head and leg related commands at the same time and they are performed on separate threads. The head is reasonably complex as the neck consists of three servo motors. Similarly for the tail which is controlled by one servo motor. So you could start leg, head and tail commands before calling wait_all_done. Then you are free to issue another set of commands. If you issue a new command while a previous one is executing then the library will throw an exception.

To get the PiDog to move we use commands like the following actions:

       dog.do_action('trot', step_count=5, speed=98)
       dog.do_action('turn_right', step_count=5, speed=98)
       dog.do_action('backward', step_count=5, speed=98) 

There are also apis to use the speaker, read the ultrasonic distance sensor and set the front LED strip such as:

   dog.speak("single_bark_1")
   dog.ultrasonic.read_distance()
   dog.rgb_strip.set_mode('breath', 'white', bps=0.5)

To use the camera, it is exactly the same as for the PiCar using the vilib library, as this is the same camera as the PiCar, connected in the same manner. The sample program below uses the vilib program to compare successive images to see if the PiDog is stuck, though this doesn’t work very well as the legs cause so much movement that the images are rarely the same.

Sample Program

I took the PiCar Roomba program and modified it to work with the PiDog. It doesn’t work as well as a Roomba, as the PiDog is quite a bit slower than the PiCar and the standard actions to turn the PiDog have quite a large turning radius. It might be nice if there were some standard actions to perhaps turn 180 degrees in a more efficient manner. The listing is at the end of the article.

PiDog running the sample program.

Documentation

At the lowest level, all the source code for the Python classes like PiDog.py are included which are quite interesting to browse. Then there is a set of sample programs. The standard set is documented on the website, then there is a further set in a test folder that is installed, but not mentioned on the website. The website then has an easy coding section that explains the various parts of the PiDog class. Although there isn’t a reference document, I found the provided docs quite good and the source code easy enough to read for the complete picture.

Summary

Although coordinating eight servo motors to control walking is quite complicated, the PiDog class allows you to still control basic operations without having to understand that complexity. If you did want to program more complicated motion, like perhaps getting the PiDog to gallop then you are free to do so. I found programming the PiDog fairly straightforward and enjoyable. I find the way the PiDog moves quite interesting and this might give you a first look at more complicated robotic programming like you might encounter with a robot arm with a fully articulated hand.

from pidog import Pidog
import time
import random
from vilib import Vilib
import os
import cv2
import numpy as np

SPIRAL = 1
BACKANDFORTH = 2
STUCK = 3

state = SPIRAL
StartTurn = 80
foundObstacle = 40
StuckDist = 10
lastPhoto = ""
currentPhoto = ""
MSE_THRESHOLD = 20

def compareImages():
    if lastPhoto == "":
        return 0
    img1 = cv2.imread(lastPhoto)
    img2 = cv2.imread(currentPhoto)
    if img1 is None or img2 is None:
        return(MSE_THRESHOLD + 1) 
    img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
    img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
    h, w = img1.shape
    try:
        diff = cv2.subtract(img1, img2)
    except:
        return(0)
    err = np.sum(diff**2)
    mse = err/(float(h*w))
    print("comp mse = ", mse)
    return mse    

def take_photo():
    global lastPhoto, currentPhoto
    _time = time.strftime('%Y-%m-%d-%H-%M-%S',time.localtime(time.time()))
    name = 'photo_%s'%_time
    username = os.getlogin()
    path = f"/home/{username}/Pictures/"
    Vilib.take_photo(name, path)
    print('photo save as %s%s.jpg'%(path,name))
    if lastPhoto != "":
        os.remove(lastPhoto)
    lastPhoto = currentPhoto
    currentPhoto = path + name + ".jpg"

def executeSpiral(dog):
    global state
    dog.do_action('turn_right', step_count=5, speed=98)      
    dog.wait_all_done()
    distance = round(dog.ultrasonic.read_distance(), 2)
    print("spiral distance: ",distance)
    if distance <= foundObstacle and distance != -1:
        state = BACKANDFORTH

def executeUnskick(dog):
    global state    
    print("unskick backing up")
    dog.speak("single_bark_1")
    dog.do_action('backward', step_count=5, speed=98)    
    dog.wait_all_done()
    time.sleep(1.2)
    state = SPIRAL                    

def executeBackandForth(dog):
    global state    
    distance = round(dog.ultrasonic.read_distance(), 2)
    print("back and forth distance: ",distance)
    if distance >= StartTurn or distance == -1:       
        dog.do_action('trot', step_count=5, speed=98)
        dog.wait_all_done()
    elif distance < StuckDist:
        state = STUCK
    else:
        dog.do_action('turn_right', step_count=5, speed=98)
        dog.wait_all_done()
    time.sleep(0.5)                

def main():
    global state
    try:
        dog = Pidog()
        dog.do_action('stand', speed=80)
        dog.wait_all_done()
        time.sleep(.5)        
        dog.rgb_strip.set_mode('breath', 'white', bps=0.5)       
        Vilib.camera_start(vflip=False,hflip=False)
        while True:
            take_photo()
            if state == SPIRAL:               
                executeSpiral(dog)
            elif state == BACKANDFORTH:
                executeBackandForth(dog)
            elif state == STUCK:
                    executeUnskick(dog)
            if compareImages() < MSE_THRESHOLD:
                state = STUCK                    
    finally:
        dog.close()

if __name__ == "__main__":
    main()