Raspberry Pi Zero: Controller Timing Stability

Servo motor control is usually implemented with an embedded MCU as typically very fast, consistant control loops are required for real life motion controls. Programming, capturing data, and PID tuning can be tedious in an embedded system environment. So in this notebook we will be examining raspberry pi zero loop stability while controlling a brushed, dc motor.We will be testing the loop timing under different conditions:

1) Executing normally
2) Executing as an individual python thread
3) Executing during video streaming
4) Executing with multiple controller threads
5) Executing with overclocked CPU

Procedure

Because this platform is ultmately being developed for a raspbery pi robot with a live video stream using gstreamer, I will test the loop time both while the raspberry pi is streaming and not streaming video. A total of 8 tests were performed in a variety of combinations of the above conditions. Those conditions can be seen in the table below:

	Video Not Streaming	Video Streaming	Overclocked CPU: Video Not Streaming
Main Loop	Test 1	Test 3	Test 7
Threaded Loop	Test 2	Test 4	Test 8
5 Threaded Controllers	Not Tested	Test 5	Test 9
2 Threaded Controllers	Not Tested	Test 6	Test 10

We will build a list of execution times for each testing state and analyze the distribution of those execution times. This will enable us to analyze the frequency and stability of the control loop.

Software

The raspberry pi is using berryconda to manage python virtual environments, and is running a native jupyter notebook server to allow quick code editing and easy data capture.

Hardware

Between the raspberry pi there is a micrcontroller and an h-bridge. The microcontroller is setup to measure encoder counts and send them when requested to the Raspberry Pi. Likewise, the microcontroller listens to the serial line and sets the motor direction and PWM based off raspberry pi commands.

#! /home/pi/berryconda3/envs/pidtuner/bin/python
%matplotlib inline

import matplotlib 
import seaborn as sns
import matplotlib.pyplot as plt
import serial
import time
import RPi.GPIO as GPIO
import atexit
import random
import threading
import numpy as np
from scipy.stats import norm
from scipy import stats

ser = serial.Serial(
port = '/dev/ttyS0',
baudrate = 115200,
bytesize = serial.EIGHTBITS,
parity = serial.PARITY_NONE,
stopbits = serial.STOPBITS_ONE,
timeout = 1,
xonxoff = False,
rtscts = False,
dsrdtr = False,
writeTimeout = 2
)

def setPwm(pwm):
    message = str.encode('R' + str(pwm) + '!')
    ser.write(message)

def getRevs():
    ser.write(str.encode('?!'))
    val = ser.readline()
    return (int(val))

class PID (threading.Thread):

    setpoint = 0
    last = 0
    error = 0
    lastValue = 0

    Tkp = 0
    Tki = 0
    Tkd = 0

    def __init__(self, kp, ki, kd, direction, outputFunc, inputFunc, threadID):
        threading.Thread.__init__(self)
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.direction = direction
        self.threadID = threadID
        self.outputFunc = outputFunc
        self.inputFunc = inputFunc
        self.output = 0
        
        #Used for thread timing
        self.count = 0
        self.runtimes = []

        #This is used to terminate the thread
        self.shutdown_flag = threading.Event()

        self.min = 0
        self.max = 0

    def join(self, timeout=None):
        self.shutdown_flag.set()
        threading.Thread.join(self, timeout)

    def setLimits(self, outputMin, outputMax):
        self.min = outputMin
        self.max = outputMax
        
    def __run__(self):
        controllerInput = self.inputFunc()
        controllerOutput = self.compute(controllerInput)
        self.outputFunc(controllerOutput)

    def run(self):
        while not self.shutdown_flag.is_set():
            #self.__run__()
            dt = self.timeFunc(self.__run__)
            self.runtimes.append(dt)
            self.count += 1

    def timeFunc(self, func):
        start = time.monotonic()
        func()
        return time.monotonic() - start
    
    def compute(self, value):

        #update timer
        now = time.monotonic()
        dt = now - self.last
        self.last = now


        #update PID terms
        self.error = self.setpoint - value
        self.Tkp =  self.kp*self.error
        self.Tki += self.ki*self.error*dt
        self.Tkd =  self.kd*(value - self.lastValue)/dt

        self.lastValue = value

        output = self.Tkp + self.Tki - self.Tkd

        if output < self.min:
            output = self.min
        if output > self.max:
            output = self.max

        self.output = output*self.direction

        return(self.output)

pid1 = PID(17,0,0.16, -1.0, setPwm, getRevs, 'pid1')
pid1.setLimits(-1000.0, 1000.0)
pid1.setpoint = getRevs()

@atexit.register
def exit():
    setPwm(0.0)
    pid1.shutdown_flag.set()
    pid1.join()
    ser.close()
    GPIO.cleanup()
    print('PID test ended.')

Test 1: Main Loop Execution, No Video Stream

def mainTest():
    count = 0
    main_loop_times = []
    pid1.setpoint = getRevs() - 625

    while count < 1000:
        now = time.monotonic()
        pid1.__run__()
        main_loop_times.append(time.monotonic() - now)
        count += 1

    setPwm(0.0)
    return main_loop_times

data1 = mainTest()
mu, std = norm.fit(data1)
sns.distplot(data1)

print('Mean',mu)
print('Standard Deviation', std)
print('Number Samples', len(data1))

fig = plt.figure()
ax = fig.add_subplot(111)
stats.probplot(data1, dist=stats.loggamma, sparams=(2.5,), plot=ax)
plt.show()

Mean 0.0073300706770123725
Standard Deviation 0.0009723582933104428
Number Samples 1000

Test 2: Threaded Execution, No Video Stream

def threadTest():
    pid = PID(17,0,0.16, -1.0, setPwm, getRevs, 'pid1')
    pid.setLimits(-1000.0, 1000.0)
    pid.setpoint = getRevs() - 625
    
    count = 0
    thread_loop_times = []
    pid.start()

    while(pid.count < 1000):
        time.sleep(0.01)
    pid.shutdown_flag.set()
    pid.join()
    
    return pid.runtimes

data2 = threadTest()
mu, std = norm.fit(data2)
sns.distplot(data2)

print('Mean',mu)
print('Standard Deviation', std)
print('Number Samples', len(data2))

fig = plt.figure()
ax = fig.add_subplot(111)
stats.probplot(data2, dist=stats.loggamma, sparams=(2.5,), plot=ax)
plt.show()

Mean 0.007794305371624852
Standard Deviation 0.0009235506427647417
Number Samples 1001

Test 3: Main Loop Execution, Video Streaming

data3 = mainTest()
mu, std = norm.fit(data3)
plt.hist(data3, bins=100, density=True,  color='g')
plt.show()
print('Mean',mu)
print('Standard Deviation', std)
print(len(data3))

Mean 0.009181247276006615
Standard Deviation 0.0032182004652700565
1000

Test 4: Threaded Execution, Video Streaming

data4 = threadTest()
mu, std = norm.fit(data4)
plt.hist(data4, bins=100, density=True,  color='g')
plt.show()
print('Mean',mu)
print('Standard Deviation', std)

Mean 0.009382372759245373
Standard Deviation 0.003445627311796786

Test 5: 5 Threaded Execution: Video Streaming

def fakeFoo(var):
    return 0

def fakeFoo2():
    return 0

def multiThreadTest(numThreads, waitCounts):
    pids = []
    data = []
    count = 0
    
    pid = None
    pid = PID(2,0,0.0, -1.0, setPwm, getRevs, 'pid'+str(count))
    pid.setLimits(-1000.0, 1000.0)
    pid.setpoint = getRevs() - 625
    pids.append(pid)
    count += 1

    while(count < numThreads):
        pid = None
        pid = PID(17,0,0.16, -1.0, fakeFoo, fakeFoo2, 'pid'+str(count))
        pid.setLimits(-1000.0, 1000.0)
        pid.setpoint = getRevs() - 625
        pids.append(pid)
        count += 1
        
    for pid in pids:
        pid.start()

    print('Threads Spawned')
    
    while(pids[0].count < waitCounts):
        time.sleep(0.01)
    
    print('Done Waiting')
    
    for pid in pids:
        pid.shutdown_flag.set()
        pid.join()
        
    print('Threads shutdown')
    for pid in pids:
        data += pid.runtimes
    
    setPwm(0.0)
    return pids[0].runtimes

data = multiThreadTest(5,100)
mu, std = norm.fit(data)
sns.distplot(data)

print('Mean',mu)
print('Standard Deviation', std)
print('Number Samples', len(data))

fig = plt.figure()
ax = fig.add_subplot(111)
stats.probplot(data, dist=stats.loggamma, sparams=(2.5,), plot=ax)
plt.show()

Threads Spawned
Done Waiting
Threads shutdown
Mean 0.2168126435098398
Standard Deviation 0.09721862391775106
Number Samples 1000

The running time is almost 10x greater spawing 5 of the threads! This greatly affects controller performance. According to 'top' this program was occupying 85% of CPU overhead. Although even single threaded tasks seem to occupy this much overhead.

Test 6: 2 Threaded Execution, Video Streaming

data = multiThreadTest(2,1000)
mu, std = norm.fit(data)
sns.distplot(data)

print('Mean',mu)
print('Standard Deviation', std)
print('Number Samples', len(data))

fig = plt.figure()
ax = fig.add_subplot(111)
stats.probplot(data, dist=stats.loggamma, sparams=(2.5,), plot=ax)
plt.show()

Threads Spawned
Done Waiting
Threads shutdown
Mean 0.017995777843166767
Standard Deviation 0.008524876724822198
Number Samples 1000

Two threads (which we will need) has much better loop time although the CPU overhead is still comparable.

Pi Overclocked to 800 MHz

I wasn't thrilled with the update rate of the control loop, and the amount of processor overhead, so I tried overclocking the pi from 700 MHz to 800 MHz

Test 7: Pi Overclocked, Main Thread, No Video Stream

## Main Thread : No Video Stream
data5 = mainTest()
mu, std = norm.fit(data5)

sns.distplot(data5)

print('Mean',mu)
print('Standard Deviation', std)
print(len(data5))

fig = plt.figure()
ax = fig.add_subplot(111)
stats.probplot(data5, dist=stats.loggamma, sparams=(2.5,), plot=ax)
plt.show()

Mean 0.007363954764970913
Standard Deviation 0.0009171446874688051
1000

Test 8: Pi Overclocked, Threaded Execution, No Video Stream

## Threaded: No Video Stream
data6 = threadTest()
mu, std = norm.fit(data6)
sns.distplot(data6)
print('Mean',mu)
print('Standard Deviation', std)
print('Number Samples', len(data6))

fig = plt.figure()
ax = fig.add_subplot(111)
stats.probplot(data6, dist=stats.loggamma, sparams=(2.5,), plot=ax)
plt.show()

Mean 0.007800759974014982
Standard Deviation 0.0009823091864072152
Number Samples 1001

Test 9: Pi Overclocked, 5 Controller Threads, No Video Stream

## 5 Threaded Overclock, No Video Stream

data = multiThreadTest(5,100)
mu, std = norm.fit(data)
sns.distplot(data)

print('Mean',mu)
print('Standard Deviation', std)
print('Number Samples', len(data))

fig = plt.figure()
ax = fig.add_subplot(111)
stats.probplot(data, dist=stats.loggamma, sparams=(2.5,), plot=ax)
plt.show()

Threads Spawned
Done Waiting
Threads shutdown
Mean 0.14593184682352706
Standard Deviation 0.06581780544764805
Number Samples 102

Test 10: Pi Overclocked, 2 Controller Threads, No video Stream

## 2 Threaded Overclock: No Video Stream
data = multiThreadTest(2,1000)
mu, std = norm.fit(data)
s