diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..08d52e0 --- /dev/null +++ b/.gitignore @@ -0,0 +1,56 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] + +# C extensions +*.so + +# Distribution / packaging +.Python +env/ +bin/ +build/ +develop-eggs/ +dist/ +eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +*.egg-info/ +.installed.cfg +*.egg + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.cache +nosetests.xml +coverage.xml + +# Translations +*.mo + +# Mr Developer +.mr.developer.cfg +.project +.pydevproject + +# Rope +.ropeproject + +# Django stuff: +*.log +*.pot + +# Sphinx documentation +docs/_build/ + +# vim +*.swp diff --git a/QuickBot.py b/QuickBot.py deleted file mode 100755 index 9506efc..0000000 --- a/QuickBot.py +++ /dev/null @@ -1,741 +0,0 @@ -#!/usr/bin/python -""" -@brief QuickBot class for Beaglebone Black - -@author Rowland O'Flaherty (rowlandoflaherty.com) -@date 02/07/2014 -@version: 1.0 -@copyright: Copyright (C) 2014, Georgia Tech Research Corporation -see the LICENSE file included with this software (see LINENSE file) -""" - -from __future__ import division -import sys -import time -import re -import socket -import threading -import numpy as np - -import Adafruit_BBIO.GPIO as GPIO -import Adafruit_BBIO.PWM as PWM -import Adafruit_BBIO.ADC as ADC - -# Constants -LEFT = 0 -RIGHT = 1 -MIN = 0 -MAX = 1 - -DEBUG = False - -ADCTIME = 0.001 - -## Tic toc constants -TICTOC_START = 0 -TICTOC_COUNT = 0 -TICTOC_MEAN = 0 -TICTOC_MAX = -float('inf') -TICTOC_MIN = float('inf') - -## Encoder buffer constants and variables -ENC_BUF_SIZE = 2**9 -ENC_IND = [0, 0] -ENC_TIME = [[0]*ENC_BUF_SIZE, [0]*ENC_BUF_SIZE] -ENC_VAL = [[0]*ENC_BUF_SIZE, [0]*ENC_BUF_SIZE] - -ADC_LOCK = threading.Lock() - -## Run variables -RUN_FLAG = True -RUN_FLAG_LOCK = threading.Lock() - - -class QuickBot(): - """The QuickBot Class""" - - # === Class Properties === - # Parameters - sampleTime = 20.0 / 1000.0 - - # Pins - ledPin = 'USR1' - - # Motor Pins -- (LEFT, RIGHT) - dir1Pin = ('P8_14', 'P8_12') - dir2Pin = ('P8_16', 'P8_10') - pwmPin = ('P9_16', 'P9_14') - - # ADC Pins - irPin = ('P9_38', 'P9_40', 'P9_36', 'P9_35', 'P9_33') - encoderPin = ('P9_39', 'P9_37') - - # Encoder counting parameter and variables - ticksPerTurn = 16 # Number of ticks on encoder disc - encWinSize = 2**5 # Should be power of 2 - minPWMThreshold = [45, 45] # Threshold on the minimum value to turn wheel - encTPrev = [0.0, 0.0] - encThreshold = [0.0, 0.0] - encTickState = [0, 0] - encTickStateVec = np.zeros((2, encWinSize)) - - # Constraints - pwmLimits = [-100, 100] # [min, max] - - # State PWM -- (LEFT, RIGHT) - pwm = [0, 0] - - # State IR - irVal = [0.0, 0.0, 0.0, 0.0, 0.0] - ithIR = 0 - - # State Encoder - encTime = [0.0, 0.0] # Last time encoders were read - encPos = [0.0, 0.0] # Last encoder tick position - encVel = [0.0, 0.0] # Last encoder tick velocity - - # Encoder counting parameters - encCnt = 0 # Count of number times encoders have been read - encSumN = [0, 0] # Sum of total encoder samples - encBufInd0 = [0, 0] # Index of beginning of new samples in buffer - encBufInd1 = [0, 0] # Index of end of new samples in buffer - encTimeWin = np.zeros((2, encWinSize)) # Moving window of encoder sample times - encValWin = np.zeros((2, encWinSize)) # Moving window of encoder raw sample values - encPWMWin = np.zeros((2, encWinSize)) # Moving window corresponding PWM input values - encTau = [0.0, 0.0] # Average sampling time of encoders - - ## Stats of encoder values while input = 0 and vel = 0 - encZeroCntMin = 2**4 # Min number of recorded values to start calculating stats - encZeroMean = [0.0, 0.0] - encZeroVar = [0.0, 0.0] - encZeroCnt = [0, 0] - encHighCnt = [0, 0] - encLowCnt = [0, 0] - encLowCntMin = 2 - - # Variables - ledFlag = True - cmdBuffer = '' - - # UDP - baseIP = '192.168.7.1' - robotIP = '192.168.7.2' - port = 5005 - robotSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - robotSocket.setblocking(False) - - # === Class Methods === - # Constructor - def __init__(self, baseIP, robotIP): - - # Initialize GPIO pins - GPIO.setup(self.dir1Pin[LEFT], GPIO.OUT) - GPIO.setup(self.dir2Pin[LEFT], GPIO.OUT) - GPIO.setup(self.dir1Pin[RIGHT], GPIO.OUT) - GPIO.setup(self.dir2Pin[RIGHT], GPIO.OUT) - - GPIO.setup(self.ledPin, GPIO.OUT) - - # Initialize PWM pins: PWM.start(channel, duty, freq=2000, polarity=0) - PWM.start(self.pwmPin[LEFT], 0) - PWM.start(self.pwmPin[RIGHT], 0) - - # Set motor speed to 0 - self.setPWM([0, 0]) - - # Initialize ADC - ADC.setup() - self.encoderRead = encoderRead(self.encoderPin) - - # Set IP addresses - self.baseIP = baseIP - self.robotIP = robotIP - self.robotSocket.bind((self.robotIP, self.port)) - - - if DEBUG: - ## Stats of encoder values while moving -- high, low, and all tick state - self.encHighLowCntMin = 2**5 # Min number of recorded values to start calculating stats - self.encHighMean = [0.0, 0.0] - self.encHighVar = [0.0, 0.0] - self.encHighTotalCnt = [0, 0] - - self.encLowMean = [0.0, 0.0] - self.encLowVar = [0.0, 0.0] - self.encLowTotalCnt = [0, 0] - - self.encNonZeroCntMin = 2**5 - self.encNonZeroMean = [0.0, 0.0] - self.encNonZeroVar = [0.0, 0.0] - self.encNonZeroCnt = [0, 0] - - # Record variables - self.encRecSize = 2**13 - self.encRecInd = [0, 0] - self.encTimeRec = np.zeros((2, self.encRecSize)) - self.encValRec = np.zeros((2, self.encRecSize)) - self.encPWMRec = np.zeros((2, self.encRecSize)) - self.encNNewRec = np.zeros((2, self.encRecSize)) - self.encPosRec = np.zeros((2, self.encRecSize)) - self.encVelRec = np.zeros((2, self.encRecSize)) - self.encTickStateRec = np.zeros((2, self.encRecSize)) - self.encThresholdRec = np.zeros((2, self.encRecSize)) - - # Getters and Setters - def setPWM(self, pwm): - # [leftSpeed, rightSpeed]: 0 is off, caps at min and max values - - self.pwm[LEFT] = min( - max(pwm[LEFT], self.pwmLimits[MIN]), self.pwmLimits[MAX]) - self.pwm[RIGHT] = min( - max(pwm[RIGHT], self.pwmLimits[MIN]), self.pwmLimits[MAX]) - - # Left motor - if self.pwm[LEFT] > 0: - GPIO.output(self.dir1Pin[LEFT], GPIO.LOW) - GPIO.output(self.dir2Pin[LEFT], GPIO.HIGH) - PWM.set_duty_cycle(self.pwmPin[LEFT], abs(self.pwm[LEFT])) - elif self.pwm[LEFT] < 0: - GPIO.output(self.dir1Pin[LEFT], GPIO.HIGH) - GPIO.output(self.dir2Pin[LEFT], GPIO.LOW) - PWM.set_duty_cycle(self.pwmPin[LEFT], abs(self.pwm[LEFT])) - else: - GPIO.output(self.dir1Pin[LEFT], GPIO.LOW) - GPIO.output(self.dir2Pin[LEFT], GPIO.LOW) - PWM.set_duty_cycle(self.pwmPin[LEFT], 0) - - # Right motor - if self.pwm[RIGHT] > 0: - GPIO.output(self.dir1Pin[RIGHT], GPIO.LOW) - GPIO.output(self.dir2Pin[RIGHT], GPIO.HIGH) - PWM.set_duty_cycle(self.pwmPin[RIGHT], abs(self.pwm[RIGHT])) - elif self.pwm[RIGHT] < 0: - GPIO.output(self.dir1Pin[RIGHT], GPIO.HIGH) - GPIO.output(self.dir2Pin[RIGHT], GPIO.LOW) - PWM.set_duty_cycle(self.pwmPin[RIGHT], abs(self.pwm[RIGHT])) - else: - GPIO.output(self.dir1Pin[RIGHT], GPIO.LOW) - GPIO.output(self.dir2Pin[RIGHT], GPIO.LOW) - PWM.set_duty_cycle(self.pwmPin[RIGHT], 0) - - # Methods - def run(self): - global RUN_FLAG - self.encoderRead.start() - try: - while RUN_FLAG is True: - self.update() - - # Flash BBB LED - if self.ledFlag is True: - self.ledFlag = False - GPIO.output(self.ledPin, GPIO.HIGH) - else: - self.ledFlag = True - GPIO.output(self.ledPin, GPIO.LOW) - time.sleep(self.sampleTime) - except: - RUN_FLAG_LOCK.acquire() - RUN_FLAG = False - RUN_FLAG_LOCK.release() - raise - - self.cleanup() - return - - def cleanup(self): - sys.stdout.write("Shutting down...") - self.setPWM([0, 0]) - self.robotSocket.close() - GPIO.cleanup() - PWM.cleanup() - if DEBUG: - # tictocPrint() - self.writeBuffersToFile() - sys.stdout.write("Done\n") - - def update(self): - self.readIRValues() - self.readEncoderValues() - self.parseCmdBuffer() - - def parseCmdBuffer(self): - global RUN_FLAG - try: - line = self.robotSocket.recv(1024) - except socket.error as msg: - return - - self.cmdBuffer += line - - bufferPattern = r'\$[^\$\*]*?\*' # String contained within $ and * symbols with no $ or * symbols in it - bufferRegex = re.compile(bufferPattern) - bufferResult = bufferRegex.search(self.cmdBuffer) - - if bufferResult: - msg = bufferResult.group() - print msg - self.cmdBuffer = '' - - msgPattern = r'\$(?P[A-Z]{3,})(?P=?)(?P\??)(?(2)(?P.*)).*\*' - msgRegex = re.compile(msgPattern) - msgResult = msgRegex.search(msg) - - if msgResult.group('CMD') == 'CHECK': - self.robotSocket.sendto('Hello from QuickBot\n',(self.baseIP, self.port)) - - elif msgResult.group('CMD') == 'PWM': - if msgResult.group('QUERY'): - self.robotSocket.sendto(str(self.pwm) + '\n',(self.baseIP, self.port)) - - elif msgResult.group('SET') and msgResult.group('ARGS'): - args = msgResult.group('ARGS') - pwmArgPattern = r'(?P[-]?\d+),(?P[-]?\d+)' - pwmRegex = re.compile(pwmArgPattern) - pwmResult = pwmRegex.match(args) - if pwmResult: - pwm = [int(pwmRegex.match(args).group('LEFT')), - int(pwmRegex.match(args).group('RIGHT'))] - self.setPWM(pwm) - - elif msgResult.group('CMD') == 'IRVAL': - if msgResult.group('QUERY'): - reply = '[' + ', '.join(map(str, self.irVal)) + ']' - print 'Sending: ' + reply - self.robotSocket.sendto(reply + '\n', (self.baseIP, self.port)) - - elif msgResult.group('CMD') == 'ENVAL': - if msgResult.group('QUERY'): - reply = '[' + ', '.join(map(str, self.encPos)) + ']' - print 'Sending: ' + reply - self.robotSocket.sendto(reply + '\n', (self.baseIP, self.port)) - - elif msgResult.group('CMD') == 'ENVEL': - if msgResult.group('QUERY'): - reply = '[' + ', '.join(map(str, self.encVel)) + ']' - print 'Sending: ' + reply - self.robotSocket.sendto(reply + '\n', (self.baseIP, self.port)) - - elif msgResult.group('CMD') == 'RESET': - self.encPos[LEFT] = 0.0 - self.encPos[RIGHT] = 0.0 - print 'Encoder values reset to [' + ', '.join(map(str, self.encVel)) + ']' - - elif msgResult.group('CMD') == 'UPDATE': - if msgResult.group('SET') and msgResult.group('ARGS'): - args = msgResult.group('ARGS') - pwmArgPattern = r'(?P[-]?\d+),(?P[-]?\d+)' - pwmRegex = re.compile(pwmArgPattern) - pwmResult = pwmRegex.match(args) - if pwmResult: - pwm = [int(pwmRegex.match(args).group('LEFT')), - int(pwmRegex.match(args).group('RIGHT'))] - self.setPWM(pwm) - - reply = '[' + ', '.join(map(str, self.encPos)) + ', ' \ - + ', '.join(map(str, self.encVel)) + ']' - print 'Sending: ' + reply - self.robotSocket.sendto(reply + '\n', (self.baseIP, self.port)) - - elif msgResult.group('CMD') == 'END': - RUN_FLAG_LOCK.acquire() - RUN_FLAG = False - RUN_FLAG_LOCK.release() - - def readIRValues(self): - prevVal = self.irVal[self.ithIR] - ADC_LOCK.acquire() - self.irVal[self.ithIR] = ADC.read_raw(self.irPin[self.ithIR]) - time.sleep(ADCTIME) - ADC_LOCK.release() - - if self.irVal[self.ithIR] >= 1100: - self.irVal[self.ithIR] = prevVal - - self.ithIR = ((self.ithIR+1) % 5) - - - def readEncoderValues(self): - if DEBUG and (self.encCnt % 10) == 0: - print "------------------------------------" - print "EncPos: " + str(self.encPos) - print "EncVel: " + str(self.encVel) - print "***" - print "Threshold: " + str(self.encThreshold) - print "***" - print "Zero Cnt: " + str(self.encZeroCnt) - print "Zero Mean: " + str(self.encZeroMean) - print "Zero Var: " + str(self.encZeroVar) - print "***" - print "NonZero Cnt: " + str(self.encNonZeroCnt) - print "NonZero Mean: " + str(self.encNonZeroMean) - print "NonZero Var: " + str(self.encNonZeroVar) - print "***" - print "High Cnt: " + str(self.encHighTotalCnt) - print "High Mean: " + str(self.encHighMean) - print "High Var: " + str(self.encHighVar) - print "***" - print "Low Cnt: " + str(self.encLowTotalCnt) - print "Low Mean: " + str(self.encLowMean) - print "Low Var: " + str(self.encLowVar) - - self.encCnt = self.encCnt + 1 - - # Fill window - for side in range(0, 2): - self.encTime[side] = self.encTimeWin[side][-1] - - self.encBufInd0[side] = self.encBufInd1[side] - self.encBufInd1[side] = ENC_IND[side] - ind0 = self.encBufInd0[side] # starting index - ind1 = self.encBufInd1[side] # ending index (this element is not included until the next update) - - if ind0 < ind1: - N = ind1 - ind0 # number of new elements - self.encSumN[side] = self.encSumN[side] + N - self.encTimeWin[side] = np.roll(self.encTimeWin[side], -N) - self.encTimeWin[side, -N:] = ENC_TIME[side][ind0:ind1] - self.encValWin[side] = np.roll(self.encValWin[side], -N) - self.encValWin[side, -N:] = ENC_VAL[side][ind0:ind1] - self.encPWMWin[side] = np.roll(self.encPWMWin[side], -N) - self.encPWMWin[side, -N:] = [self.pwm[side]]*N - - elif ind0 > ind1: - N = ENC_BUF_SIZE - ind0 + ind1 # number of new elements - self.encSumN[side] = self.encSumN[side] + N - self.encTimeWin[side] = np.roll(self.encTimeWin[side], -N) - self.encValWin[side] = np.roll(self.encValWin[side], -N) - self.encPWMWin[side] = np.roll(self.encPWMWin[side], -N) - self.encPWMWin[side, -N:] = [self.pwm[side]]*N - if ind1 == 0: - self.encTimeWin[side, -N:] = ENC_TIME[side][ind0:] - self.encValWin[side, -N:] = ENC_VAL[side][ind0:] - else: - self.encTimeWin[side, -N:-ind1] = ENC_TIME[side][ind0:] - self.encValWin[side, -N:-ind1] = ENC_VAL[side][ind0:] - self.encTimeWin[side, -ind1:] = ENC_TIME[side][0:ind1] - self.encValWin[side, -ind1:] = ENC_VAL[side][0:ind1] - - if ind0 != ind1: - tauNew = self.encTimeWin[side,-1] - self.encTimeWin[side,-N] - self.encTau[side] = tauNew / self.encCnt + self.encTau[side] * (self.encCnt-1)/self.encCnt # Running average - if self.encSumN[side] > self.encWinSize: - self.countEncoderTicks(side) - - # Fill records - if DEBUG: - ind = self.encRecInd[side] - if ind+N < self.encRecSize: - self.encTimeRec[side, ind:ind+N] = self.encTimeWin[side, -N:] - self.encValRec[side, ind:ind+N] = self.encValWin[side, -N:] - self.encPWMRec[side, ind:ind+N] = self.encPWMWin[side, -N:] - self.encNNewRec[side, ind:ind+N] = [N]*N - self.encPosRec[side, ind:ind+N] = [self.encPos[side]]*N - self.encVelRec[side, ind:ind+N] = [self.encVel[side]]*N - self.encTickStateRec[side, ind:ind+N] = self.encTickStateVec[side, -N:] - self.encThresholdRec[side, ind:ind+N] = [self.encThreshold[side]]*N - self.encRecInd[side] = ind+N - - def countEncoderTicks(self, side): - # Set variables - t = self.encTimeWin[side] # Time vector of data (non-consistent sampling time) - tPrev = self.encTPrev[side] # Previous read time - pwm = self.encPWMWin[side] # Vector of PWM data - pwmPrev = pwm[-1] # Last PWM value that was applied - tickStatePrev = self.encTickState[side] # Last state of tick (high (1), low (-1), or unsure (0)) - tickCnt = self.encPos[side] # Current tick count - tickVel = self.encVel[side] # Current tick velocity - encValWin = self.encValWin[side] # Encoder raw value buffer window - threshold = self.encThreshold[side] # Encoder value threshold - minPWMThreshold = self.minPWMThreshold[side] # Minimum PWM to move wheel - - N = np.sum(t > tPrev) # Number of new updates - - tickStateVec = np.roll(self.encTickStateVec[side], -N) - - # Determine wheel direction - if tickVel != 0: - wheelDir = np.sign(tickVel) - else: - wheelDir = np.sign(pwmPrev) - - # Count ticks and record tick state - indTuple = np.where(t == tPrev) # Index of previous sample in window - if len(indTuple[0] > 0): - ind = indTuple[0][0] - newInds = ind + np.arange(1, N+1) # Indices of new samples - for i in newInds: - if encValWin[i] > threshold: # High tick state - tickState = 1 - self.encHighCnt[side] = self.encHighCnt[side] + 1 - self.encLowCnt[side] = 0 - if tickStatePrev == -1: # Increment tick count on rising edge - tickCnt = tickCnt + wheelDir - - else: # Low tick state - tickState = -1 - self.encLowCnt[side] = self.encLowCnt[side] + 1 - self.encHighCnt[side] = 0 - tickStatePrev = tickState - tickStateVec[i] = tickState - - # Measure tick speed - diffTickStateVec = np.diff(tickStateVec) # Tick state transition differences - fallingTimes = t[np.hstack((False,diffTickStateVec == -2))] # Times when tick state goes from high to low - risingTimes = t[np.hstack((False,diffTickStateVec == 2))] # Times when tick state goes from low to high - fallingPeriods = np.diff(fallingTimes) # Period times between falling edges - risingPeriods = np.diff(risingTimes) # Period times between rising edges - tickPeriods = np.hstack((fallingPeriods, risingPeriods)) # All period times - if len(tickPeriods) == 0: - if all(pwm[newInds] < minPWMThreshold): # If all inputs are less than min set velocity to 0 - tickVel = 0 - else: - tickVel = wheelDir * 1/np.mean(tickPeriods) # Average signed tick frequency - - # Estimate new mean values - newEncRaw = encValWin[newInds] - if pwmPrev == 0 and tickVel == 0: - x = newEncRaw - l = self.encZeroCnt[side] - mu = self.encZeroMean[side] - sigma2 = self.encZeroVar[side] - (muPlus, sigma2Plus, n) = recursiveMeanVar(x, l, mu, sigma2) - self.encZeroMean[side] = muPlus - self.encZeroVar[side] = sigma2Plus - self.encZeroCnt[side] = n - elif tickVel != 0: - if DEBUG: - x = newEncRaw - l = self.encNonZeroCnt[side] - mu = self.encNonZeroMean[side] - sigma2 = self.encNonZeroVar[side] - (muPlus, sigma2Plus, n) = recursiveMeanVar(x, l, mu, sigma2) - self.encNonZeroMean[side] = muPlus - self.encNonZeroVar[side] = sigma2Plus - self.encNonZeroCnt[side] = n - - NHigh = np.sum(tickStateVec[newInds] == 1) - if NHigh != 0: - indHighTuple = np.where(tickStateVec[newInds] == 1) - x = newEncRaw[indHighTuple[0]] - l = self.encHighTotalCnt[side] - mu = self.encHighMean[side] - sigma2 = self.encHighVar[side] - (muPlus, sigma2Plus, n) = recursiveMeanVar(x, l, mu, sigma2) - self.encHighMean[side] = muPlus - self.encHighVar[side] = sigma2Plus - self.encHighTotalCnt[side] = n - - NLow = np.sum(tickStateVec[newInds] == -1) - if NLow != 0: - indLowTuple = np.where(tickStateVec[newInds] == -1) - x = newEncRaw[indLowTuple[0]] - l = self.encLowTotalCnt[side] - mu = self.encLowMean[side] - sigma2 = self.encLowVar[side] - (muPlus, sigma2Plus, n) = recursiveMeanVar(x, l, mu, sigma2) - self.encLowMean[side] = muPlus - self.encLowVar[side] = sigma2Plus - self.encLowTotalCnt[side] = n - - # Set threshold value - if self.encZeroCnt[side] > self.encZeroCntMin: - self.encThreshold[side] = self.encZeroMean[side] - 3*np.sqrt(self.encZeroVar[side]) - -# elif self.encNonZeroCnt[side] > self.encNonZeroCntMin: -# self.encThreshold[side] = self.encNonZeroMean[side] - -# elif self.encHighTotalCnt[side] > self.encHighLowCntMin and self.encLowTotalCnt > self.encHighLowCntMin: -# mu1 = self.encHighMean[side] -# sigma1 = self.encHighVar[side] -# mu2 = self.encLowMean[side] -# sigma2 = self.encLowVar[side] -# alpha = (sigma1 * np.log(sigma1)) / (sigma2 * np.log(sigma1)) -# A = (1 - alpha) -# B = -2 * (mu1 - alpha*mu2) -# C = mu1**2 - alpha * mu2**2 -# x1 = (-B + np.sqrt(B**2 - 4*A*C)) / (2*A) -# x2 = (-B - np.sqrt(B**2 - 4*A*C)) / (2*A) -# if x1 < mu1 and x1 > mu2: -# self.encThreshold[side] = x1 -# else: -# self.encThreshold[side] = x2 - - - # Update variables - self.encPos[side] = tickCnt # New tick count - self.encVel[side] = tickVel # New tick velocity - self.encTickStateVec[side] = tickStateVec # New tick state vector - - self.encTPrev[side] = t[-1] # New latest update time - - - def writeBuffersToFile(self): - matrix = map(list, zip(*[self.encTimeRec[LEFT], self.encValRec[LEFT], self.encPWMRec[LEFT], self.encNNewRec[LEFT], \ - self.encTickStateRec[LEFT], self.encPosRec[LEFT], self.encVelRec[LEFT], self.encThresholdRec[LEFT], \ - self.encTimeRec[RIGHT], self.encValRec[RIGHT], self.encPWMRec[RIGHT], self.encNNewRec[RIGHT], \ - self.encTickStateRec[RIGHT], self.encPosRec[RIGHT], self.encVelRec[RIGHT], self.encThresholdRec[RIGHT]])) - s = [[str(e) for e in row] for row in matrix] - lens = [len(max(col, key=len)) for col in zip(*s)] - fmt = '\t'.join('{{:{}}}'.format(x) for x in lens) - table = [fmt.format(*row) for row in s] - f = open('output.txt', 'w') - f.write('\n'.join(table)) - f.close() - print "Wrote buffer to output.txt" - - -class encoderRead(threading.Thread): - """The encoderRead Class""" - - # === Class Properties === - # Parameters - - # === Class Methods === - # Constructor - def __init__(self,encPin=('P9_39', 'P9_37')): - - # Initialize thread - threading.Thread.__init__(self) - - # Set properties - self.encPin = encPin - - # Methods - def run(self): - global RUN_FLAG - - self.t0 = time.time() - while RUN_FLAG: - global ENC_IND - global ENC_TIME - global ENC_VAL - - for side in range(0, 2): - ENC_TIME[side][ENC_IND[side]] = time.time() - self.t0 - ADC_LOCK.acquire() - ENC_VAL[side][ENC_IND[side]] = ADC.read_raw(self.encPin[side]) - time.sleep(ADCTIME) - ADC_LOCK.release() - ENC_IND[side] = (ENC_IND[side] + 1) % ENC_BUF_SIZE - - -def recursiveMeanVar(x, l, mu, sigma2): - """ - This function calculates a new mean and variance given - the current mean "mu", current variance "sigma2", current - update count "l", and new samples "x" - """ - - m = len(x) - n = l + m - muPlus = l / n * mu + m / n * np.mean(x) - if n > 1: - sigma2Plus = 1/(n-1) * ((l-1)*sigma2 + (m-1)*np.var(x) + l*(mu - muPlus)**2 + m*(np.mean(x) - muPlus)**2) - else: - sigma2Plus = 0 - - return (muPlus, sigma2Plus, n) - -def operatingPoint(uStar, uStarThreshold): - """ - This function returns the steady state tick velocity given some PWM input. - - uStar: PWM input. - uStarThreshold: Threshold on the minimum magnitude of a PWM input value - - returns: omegaStar - steady state tick velocity - """ - # Matlab code to find beta values - # X = [40; 80; 100]; % Air Test - # Y = [0.85; 2.144; 3.5]; - # - # r = 0.0325; % Wheel radius - # c = 2*pi*r; - # X = [ 70; 70; 70; 75; 75; 75; 80; 80; 80; 85; 85; 85; 90; 90; 90]; % Ground Test - # Z = [4.25; 3.95; 4.23; 3.67; 3.53; 3.48; 3.19; 3.08; 2.93; 2.52; 2.59; 2.56; 1.99; 2.02; 2.04]; % Time to go 1 m - # Y = 1./(Z*c); - # H = [X ones(size(X))]; - # beta = H \ Y - # beta = [0.0425, -0.9504] # Air Test Results - beta = [0.0606, -3.1475] # Ground Test Results - - if np.abs(uStar) <= uStarThreshold: - omegaStar = 0.0 - elif uStar > 0: - omegaStar = beta[0]*uStar + beta[1] - else: - omegaStar = -1.0*(beta[0]*np.abs(uStar) + beta[1]) - - return omegaStar - - -def kalman(x, P, Phi, H, W, V, z): - """ - This function returns an optimal expected value of the state and covariance - error matrix given an update and system parameters. - - x: Estimate of staet at time t-1. - P: Estimate of error covariance matrix at time t-1. - Phi: Discrete time state tranistion matrix at time t-1. - H: Observation model matrix at time t. - W: Process noise covariance at time t-1. - V: Measurement noise covariance at time t. - z: Measurement at time t. - - returns: (x,P) tuple - x: Updated estimate of state at time t. - P: Updated estimate of error covariance matrix at time t. - - """ - x_p = Phi*x # Prediction of setimated state vector - P_p = Phi*P*Phi + W # Prediction of error covariance matrix - S = H*P_p*H + V # Sum of error variances - S_inv = 1/S # Inverse of sum of error variances - K = P_p*H*S_inv # Kalman gain - r = z - H*x_p # Prediction residual - w = -K*r # Process error - x = x_p - w # Update estimated state vector - v = z - H*x # Measurement error - if np.isnan(K*V): - P = P_p - else: - P = (1 - K*H)*P_p*(1 - K*H) + K*V*K # Updated error covariance matrix - - return (x, P) - - -def tic(): - global TICTOC_START - TICTOC_START = time.time() - - -def toc(tictocName = 'toc', printFlag = True): - global TICTOC_START - global TICTOC_COUNT - global TICTOC_MEAN - global TICTOC_MAX - global TICTOC_MIN - - tictocTime = time.time() - TICTOC_START - TICTOC_COUNT = TICTOC_COUNT + 1 - TICTOC_MEAN = tictocTime / TICTOC_COUNT + TICTOC_MEAN * (TICTOC_COUNT-1) / TICTOC_COUNT - TICTOC_MAX = max(TICTOC_MAX,tictocTime) - TICTOC_MIN = min(TICTOC_MIN,tictocTime) - - if printFlag: - print tictocName + " time: " + str(tictocTime) - -def tictocPrint(): - global TICTOC_COUNT - global TICTOC_MEAN - global TICTOC_MAX - global TICTOC_MIN - - print "Tic Toc Stats:" - print "Count = " + str(TICTOC_COUNT) - print "Mean = " + str(TICTOC_MEAN) - print "Max = " + str(TICTOC_MAX) - print "Min = " + str(TICTOC_MIN) - - diff --git a/QuickBotRun.py b/QuickBotRun.py deleted file mode 100755 index 4f9c50d..0000000 --- a/QuickBotRun.py +++ /dev/null @@ -1,39 +0,0 @@ -#!/usr/bin/python -""" -@brief Run QuickBot class for Beaglebone Black - -@author Rowland O'Flaherty (rowlandoflaherty.com) -@date 02/07/2014 -@version: 1.0 -@copyright: Copyright (C) 2014, Georgia Tech Research Corporation see the LICENSE file included with this software (see LINENSE file) -""" - -import sys -import time -from QuickBot import * - -print "Running QuickBot" - -baseIP = '192.168.7.1' -robotIP = '192.168.7.2' - -if len(sys.argv) > 3: - print 'Invalid number of command line arguments.' - print 'Proper syntax:' - print '>> QuickBotRun.py baseIP robotIP' - print 'Example:' - print '>> QuickBotRun.py ', baseIP, ' ', robotIP - sys.exit() - -if len(sys.argv) >= 2: - baseIP = sys.argv[1] - -if len(sys.argv) >= 3: - robotIP = sys.argv[2] - -print 'Running QuickBot Program' -print ' Base IP: ', baseIP -print ' Robot IP: ', robotIP - -QB = QuickBot(baseIP, robotIP) -QB.run() diff --git a/README.md b/README.md index 8e27caa..7cfeb14 100644 --- a/README.md +++ b/README.md @@ -1,17 +1,23 @@ -# QuickBot_BBB -This is the code that runs on the BeagleBone Black to control the QuickBot. +# Xbot_BBB +This is the code that runs on the BeagleBone Black to control a robot. ## Overview -Essentially this code establishes socket (UDP) connection with another device (BASE) and waits for commands. The commands are either of the form of directives or queries. An example directive is setting the PWM values of the motors. An example query is getting IR sensor values. +Essentially this code establishes socket (UDP) connection with another device +(BASE) and waits for commands. The commands are either of the form of +directives or queries. An example directive is setting the PWM values of the +motors. An example query is getting IR sensor values. ## Installation Clone the repo into home directory: - cd ~ - git clone https://bitbucket.org/rowoflo/quickbot_bbb.git + $ cd ~ + $ git clone https://bitbucket.org/rowoflo/quickbot_bbb.git + # or + $ git clone https://github.com/o-botics/quickbot_bbb.git ## Running -Check IP address of BASE and ROBOT (run command on both systems and look for IP address): +Check IP address of BASE and ROBOT (run command on both systems and look for IP +address): ifconfig @@ -26,26 +32,39 @@ Example output from BBB: collisions:0 txqueuelen:1000 RX bytes:66840454 (63.7 MiB) TX bytes:1878384 (1.7 MiB) -Here the IP address for the robot is 192.168.1.101. Let's assume the IP address for the BASE is 192.168.1.100. +Here the IP address for the robot is 192.168.1.101. Let's assume the IP address +for the BASE is 192.168.1.100. Change into working directory: - cd ~/quickbot_bbb + $ cd ~/quickbot_bbb -Launch QuickBotRun python script using IP addresses of BASE and ROBOT: +Quick run with given ips: - ./QuickBotRun.py 192.168.1.100 192.168.1.101 + $ python run.py -i 192.168.1.100 -r 192.168.1.101 + +All commands: + + $ python run.py --help + usage: run.py [-h] [--ip IP] [--rip RIP] [--rtype RTYPE] + + optional arguments: + -h, --help show this help message and exit + --ip IP, -i IP Computer ip (base ip) + --rip RIP, -r RIP BBB ip (robot ip) + --rtype RTYPE, -t RTYPE + Type of robot (quick|ultra) ## Command Set -* Check that the QuickBot is up and running: +* Check that the bot is up and running: * Command "$CHECK*\n" * Response - "Hello from QuickBot\n" + "Hello from [Quick|Ultra]Bot\n" * Get PWM values: @@ -73,6 +92,14 @@ Launch QuickBotRun python script using IP addresses of BASE and ROBOT: "[800, 810, 820, 830, 840]\n" +* Get Ultra values: + * Command + + "$ULTRAVAL?*\n" + + * Example response + + "[80.0, 251.0, 234.1, 12.1, 21.3]\n" * Get encoder position: * Command diff --git a/run.py b/run.py new file mode 100644 index 0000000..01a6065 --- /dev/null +++ b/run.py @@ -0,0 +1,57 @@ +#!/usr/bin/python +""" +@brief Run QuickBot class for Beaglebone Black + +@author Rowland O'Flaherty (rowlandoflaherty.com) +@date 02/07/2014 +@version: 1.0 +@copyright: Copyright (C) 2014, Georgia Tech Research Corporation see the + LICENSE file included with this software (see LINENSE file) +""" + +import sys +import argparse + + +DESCRIPTION = "" +RTYPES = ('quick', 'ultra') + + +def main(options): + print "Running XBot" + + print 'Running XBot Program' + print ' Base IP: ', options.ip + print ' Robot IP: ', options.rip + print ' Robot Type: ', options.rtype + + if options.rtype == 'quick': + import xbots.quickbot + qb = xbots.quickbot.QuickBot(options.ip, options.rip) + qb.run() + elif options.rtype == 'ultra': + import xbots.ultrabot + qb = xbots.ultrabot.UltraBot(options.ip, options.rip) + qb.run() + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description=DESCRIPTION) + parser.add_argument( + '--ip', '-i', + default='192.168.7.1', + help="Computer ip (base ip)") + parser.add_argument( + '--rip', '-r', + default='192.168.7.2', + help="BBB ip (robot ip)") + parser.add_argument( + '--rtype', '-t', + default='quick', + help="Type of robot (%s)" % '|'.join(RTYPES)) + + options = parser.parse_args() + if options.rtype not in RTYPES: + print "Chosen type not exists use (%s)" % '|'.join(RTYPES) + sys.exit(0) + main(options) diff --git a/xbots/__init__.py b/xbots/__init__.py new file mode 100644 index 0000000..792d600 --- /dev/null +++ b/xbots/__init__.py @@ -0,0 +1 @@ +# diff --git a/xbots/base.py b/xbots/base.py new file mode 100644 index 0000000..bee5a27 --- /dev/null +++ b/xbots/base.py @@ -0,0 +1,259 @@ +import sys +import time +import re +import socket +import threading + +import Adafruit_BBIO.GPIO as GPIO +import Adafruit_BBIO.PWM as PWM + +LEFT = 0 +RIGHT = 1 +MIN = 0 +MAX = 1 + +# Run variables +RUN_FLAG = True +RUN_FLAG_LOCK = threading.Lock() + +DEBUG = False + + +class BaseBot(object): + # Parameters + sampleTime = 20.0 / 1000.0 + pwm_freq = 2000 + + # Variables + ledFlag = True + cmdBuffer = '' + + # UDP + port = 5005 + robotSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + robotSocket.setblocking(False) + + def __init__(self, baseIP, robotIP): + # Initialize GPIO pins + self._setup_gpio() + + # Initialize PWM pins: PWM.start(channel, duty, freq=2000, polarity=0) + self._init_pwm(self.pwm_freq) + + # Set motor speed to 0 + self.setPWM([0, 0]) + + # Set IP addresses + self.baseIP = baseIP + self.robotIP = robotIP + self.robotSocket.bind((self.robotIP, self.port)) + + def _setup_gpio(self): + """Initialize GPIO pins""" + GPIO.setup(self.dir1Pin[LEFT], GPIO.OUT) + GPIO.setup(self.dir2Pin[LEFT], GPIO.OUT) + GPIO.setup(self.dir1Pin[RIGHT], GPIO.OUT) + GPIO.setup(self.dir2Pin[RIGHT], GPIO.OUT) + GPIO.setup(self.led, GPIO.OUT) + + def _init_pwm(self, frequency=2000): + """ Initialize PWM pins: + PWM.start(channel, duty, freq=2000, polarity=0) + """ + # XXX + # It is currently not possible to set frequency for two PWM + # a maybe solution patch pwm_test.c + # https://github.com/SaadAhmad/beaglebone-black-cpp-PWM + PWM.start(self.pwmPin[LEFT], 0)#, frequency=frequency) + PWM.start(self.pwmPin[RIGHT], 0)#, frequency=frequency) + + def setPWM(self, pwm): + # [leftSpeed, rightSpeed]: 0 is off, caps at min and max values + + self.pwm[LEFT] = min( + max(pwm[LEFT], self.pwmLimits[MIN]), self.pwmLimits[MAX]) + self.pwm[RIGHT] = min( + max(pwm[RIGHT], self.pwmLimits[MIN]), self.pwmLimits[MAX]) + + # Left motor + if self.pwm[LEFT] > 0: + GPIO.output(self.dir1Pin[LEFT], GPIO.LOW) + GPIO.output(self.dir2Pin[LEFT], GPIO.HIGH) + PWM.set_duty_cycle(self.pwmPin[LEFT], abs(self.pwm[LEFT])) + elif self.pwm[LEFT] < 0: + GPIO.output(self.dir1Pin[LEFT], GPIO.HIGH) + GPIO.output(self.dir2Pin[LEFT], GPIO.LOW) + PWM.set_duty_cycle(self.pwmPin[LEFT], abs(self.pwm[LEFT])) + else: + GPIO.output(self.dir1Pin[LEFT], GPIO.LOW) + GPIO.output(self.dir2Pin[LEFT], GPIO.LOW) + PWM.set_duty_cycle(self.pwmPin[LEFT], 0) + + # Right motor + if self.pwm[RIGHT] > 0: + GPIO.output(self.dir1Pin[RIGHT], GPIO.LOW) + GPIO.output(self.dir2Pin[RIGHT], GPIO.HIGH) + PWM.set_duty_cycle(self.pwmPin[RIGHT], abs(self.pwm[RIGHT])) + elif self.pwm[RIGHT] < 0: + GPIO.output(self.dir1Pin[RIGHT], GPIO.HIGH) + GPIO.output(self.dir2Pin[RIGHT], GPIO.LOW) + PWM.set_duty_cycle(self.pwmPin[RIGHT], abs(self.pwm[RIGHT])) + else: + GPIO.output(self.dir1Pin[RIGHT], GPIO.LOW) + GPIO.output(self.dir2Pin[RIGHT], GPIO.LOW) + PWM.set_duty_cycle(self.pwmPin[RIGHT], 0) + + def parseCmdBuffer(self): + global RUN_FLAG + try: + line = self.robotSocket.recv(1024) + except socket.error as msg: + return + + self.cmdBuffer += line + + # String contained within $ and * symbols with no $ or * symbols in it + bufferPattern = r'\$[^\$\*]*?\*' + bufferRegex = re.compile(bufferPattern) + bufferResult = bufferRegex.search(self.cmdBuffer) + msgPattern = r'\$(?P[A-Z]{3,})(?P=?)(?P\??)(?(2)(?P.*)).*\*' + + if bufferResult: + msg = bufferResult.group() + print msg + self.cmdBuffer = '' + + msgRegex = re.compile(msgPattern) + msgResult = msgRegex.search(msg) + + if msgResult.group('CMD') == 'CHECK': + self.robotSocket.sendto( + 'Hello from QuickBot\n', (self.baseIP, self.port)) + + elif msgResult.group('CMD') == 'PWM': + if msgResult.group('QUERY'): + self.robotSocket.sendto( + str(self.pwm) + '\n', (self.baseIP, self.port)) + + elif msgResult.group('SET') and msgResult.group('ARGS'): + args = msgResult.group('ARGS') + pwmArgPattern = r'(?P[-]?\d+),(?P[-]?\d+)' + pwmRegex = re.compile(pwmArgPattern) + pwmResult = pwmRegex.match(args) + if pwmResult: + pwm = [int(pwmRegex.match(args).group('LEFT')), + int(pwmRegex.match(args).group('RIGHT'))] + self.setPWM(pwm) + + elif msgResult.group('CMD') == 'IRVAL': + if msgResult.group('QUERY'): + reply = '[' + ', '.join(map(str, self.irVal)) + ']' + print 'Sending: ' + reply + self.robotSocket.sendto( + reply + '\n', (self.baseIP, self.port)) + + elif msgResult.group('CMD') == 'ULTRAVAL': + if msgResult.group('QUERY'): + reply = '[' + ', '.join(map(str, self.ultraVal)) + ']' + print 'Sending: ' + reply + self.robotSocket.sendto( + reply + '\n', (self.baseIP, self.port)) + + elif msgResult.group('CMD') == 'ENVAL': + if msgResult.group('QUERY'): + reply = '[' + ', '.join(map(str, self.encPos)) + ']' + print 'Sending: ' + reply + self.robotSocket.sendto( + reply + '\n', (self.baseIP, self.port)) + + elif msgResult.group('CMD') == 'ENVEL': + if msgResult.group('QUERY'): + reply = '[' + ', '.join(map(str, self.encVel)) + ']' + print 'Sending: ' + reply + self.robotSocket.sendto( + reply + '\n', (self.baseIP, self.port)) + + elif msgResult.group('CMD') == 'RESET': + self.encPos[LEFT] = 0 + self.encPos[RIGHT] = 0 + print 'Encoder values reset to [' + ', '.join( + map(str, self.encVel)) + ']' + + elif msgResult.group('CMD') == 'UPDATE': + if msgResult.group('SET') and msgResult.group('ARGS'): + args = msgResult.group('ARGS') + pwmArgPattern = r'(?P[-]?\d+),(?P[-]?\d+)' + pwmRegex = re.compile(pwmArgPattern) + pwmResult = pwmRegex.match(args) + if pwmResult: + pwm = [int(pwmRegex.match(args).group('LEFT')), + int(pwmRegex.match(args).group('RIGHT'))] + self.setPWM(pwm) + + reply = '[' + ', '.join(map(str, self.encPos)) + ', ' \ + + ', '.join(map(str, self.encVel)) + ']' + print 'Sending: ' + reply + self.robotSocket.sendto( + reply + '\n', (self.baseIP, self.port)) + + elif msgResult.group('CMD') == 'END': + RUN_FLAG_LOCK.acquire() + RUN_FLAG = False + RUN_FLAG_LOCK.release() + + def run(self): + global RUN_FLAG + self.encoderRead.start() + try: + while RUN_FLAG: + self.update() + + # Flash BBB LED + if self.ledFlag is True: + self.ledFlag = False + GPIO.output(self.led, GPIO.HIGH) + else: + self.ledFlag = True + GPIO.output(self.led, GPIO.LOW) + time.sleep(self.sampleTime) + except: + RUN_FLAG_LOCK.acquire() + RUN_FLAG = False + RUN_FLAG_LOCK.release() + raise + + self.cleanup() + return + + def cleanup(self): + sys.stdout.write("Shutting down...") + self.setPWM([0, 0]) + self.robotSocket.close() + GPIO.cleanup() + PWM.cleanup() + if DEBUG: + # tictocPrint() + self.writeBuffersToFile() + sys.stdout.write("Done\n") + + def writeBuffersToFile(self): + matrix = map(list, zip(*[self.encTimeRec[LEFT], self.encValRec[LEFT], + self.encPWMRec[LEFT], self.encNNewRec[LEFT], + self.encTickStateRec[LEFT], + self.encPosRec[LEFT], + self.encVelRec[LEFT], + self.encThresholdRec[LEFT], + self.encTimeRec[RIGHT], + self.encValRec[RIGHT], + self.encPWMRec[RIGHT], self.encNNewRec[RIGHT], + self.encTickStateRec[RIGHT], + self.encPosRec[RIGHT], self.encVelRec[RIGHT], + self.encThresholdRec[RIGHT]])) + s = [[str(e) for e in row] for row in matrix] + lens = [len(max(col, key=len)) for col in zip(*s)] + fmt = '\t'.join('{{:{}}}'.format(x) for x in lens) + table = [fmt.format(*row) for row in s] + f = open('output.txt', 'w') + f.write('\n'.join(table)) + f.close() + print "Wrote buffer to output.txt" diff --git a/xbots/quickbot.py b/xbots/quickbot.py new file mode 100644 index 0000000..d69f33d --- /dev/null +++ b/xbots/quickbot.py @@ -0,0 +1,424 @@ +#!/usr/bin/python +""" +@brief QuickBot class for Beaglebone Black + +@author Rowland O'Flaherty (rowlandoflaherty.com) +@date 02/07/2014 +@version: 1.0 +@copyright: Copyright (C) 2014, Georgia Tech Research Corporation +see the LICENSE file included with this software (see LINENSE file) +""" + +from __future__ import division +import threading +import time +import base +import utils + +import numpy as np +import quickbot_config as config +import Adafruit_BBIO.ADC as ADC + +# Constants + +ADCTIME = 0.001 + +# Encoder buffer constants and variables +ENC_BUF_SIZE = 2 ** 9 +ENC_IND = [0, 0] +ENC_TIME = [[0] * ENC_BUF_SIZE, [0] * ENC_BUF_SIZE] +ENC_VAL = [[0] * ENC_BUF_SIZE, [0] * ENC_BUF_SIZE] + +ADC_LOCK = threading.Lock() + + +class QuickBot(base.BaseBot): + """The QuickBot Class""" + + # Parameters + sampleTime = 20.0 / 1000.0 + + # Motor Pins -- (LEFT, RIGHT) + dir1Pin = (config.INl1, config.INr1) + dir2Pin = (config.INl2, config.INr2) + pwmPin = (config.PWMl, config.PWMr) + + # LED pin + led = config.LED + + # Encoder counting parameter and variables + ticksPerTurn = 16 # Number of ticks on encoder disc + encWinSize = 2 ** 5 # Should be power of 2 + minPWMThreshold = [45, 45] # Threshold on the minimum value to turn wheel + encTPrev = [0.0, 0.0] + encThreshold = [0.0, 0.0] + encTickState = [0, 0] + encTickStateVec = np.zeros((2, encWinSize)) + + # Constraints + pwmLimits = [-100, 100] # [min, max] + + # State PWM -- (LEFT, RIGHT) + pwm = [0, 0] + + # State IR + irVal = [0.0, 0.0, 0.0, 0.0, 0.0] + ithIR = 0 + + # State Encoder + encTime = [0.0, 0.0] # Last time encoders were read + encPos = [0.0, 0.0] # Last encoder tick position + encVel = [0.0, 0.0] # Last encoder tick velocity + + # Encoder counting parameters + encCnt = 0 # Count of number times encoders have been read + encSumN = [0, 0] # Sum of total encoder samples + encBufInd0 = [0, 0] # Index of beginning of new samples in buffer + encBufInd1 = [0, 0] # Index of end of new samples in buffer + # Moving window of encoder sample times + encTimeWin = np.zeros((2, encWinSize)) + # Moving window of encoder raw sample values + encValWin = np.zeros((2, encWinSize)) + # Moving window corresponding PWM input values + encPWMWin = np.zeros((2, encWinSize)) + encTau = [0.0, 0.0] # Average sampling time of encoders + + # Stats of encoder values while input = 0 and vel = 0 + # Min number of recorded values to start calculating stats + encZeroCntMin = 2 ** 4 + encZeroMean = [0.0, 0.0] + encZeroVar = [0.0, 0.0] + encZeroCnt = [0, 0] + encHighCnt = [0, 0] + encLowCnt = [0, 0] + encLowCntMin = 2 + + def __init__(self, baseIP, robotIP): + super(QuickBot, self).__init__(baseIP, robotIP) + # init encoder + self.encoderRead = EncoderReader() + # Initialize ADC + ADC.setup() + + if base.DEBUG: + # Stats of encoder values while moving -- high, low, and all tick + # state + # Min number of recorded values to start calculating stats + self.encHighLowCntMin = 2**5 + self.encHighMean = [0.0, 0.0] + self.encHighVar = [0.0, 0.0] + self.encHighTotalCnt = [0, 0] + + self.encLowMean = [0.0, 0.0] + self.encLowVar = [0.0, 0.0] + self.encLowTotalCnt = [0, 0] + + self.encNonZeroCntMin = 2**5 + self.encNonZeroMean = [0.0, 0.0] + self.encNonZeroVar = [0.0, 0.0] + self.encNonZeroCnt = [0, 0] + + # Record variables + self.encRecSize = 2**13 + self.encRecInd = [0, 0] + self.encTimeRec = np.zeros((2, self.encRecSize)) + self.encValRec = np.zeros((2, self.encRecSize)) + self.encPWMRec = np.zeros((2, self.encRecSize)) + self.encNNewRec = np.zeros((2, self.encRecSize)) + self.encPosRec = np.zeros((2, self.encRecSize)) + self.encVelRec = np.zeros((2, self.encRecSize)) + self.encTickStateRec = np.zeros((2, self.encRecSize)) + self.encThresholdRec = np.zeros((2, self.encRecSize)) + + + + def update(self): + self.readIRValues() + self.readEncoderValues() + self.parseCmdBuffer() + + def readIRValues(self): + prevVal = self.irVal[self.ithIR] + ADC_LOCK.acquire() + self.irVal[self.ithIR] = ADC.read_raw(config.IRS[self.ithIR]) + time.sleep(ADCTIME) + ADC_LOCK.release() + + if self.irVal[self.ithIR] >= 1100: + self.irVal[self.ithIR] = prevVal + + self.ithIR = ((self.ithIR + 1) % 5) + + def readEncoderValues(self): + """ + We read the raw adc data and try to determine if we had a rising or + falling edge. After that we try to calculate how many ticks we got. + """ + if base.DEBUG and (self.encCnt % 10) == 0: + print "------------------------------------" + print "EncPos: " + str(self.encPos) + print "EncVel: " + str(self.encVel) + print "***" + print "Threshold: " + str(self.encThreshold) + print "***" + print "Zero Cnt: " + str(self.encZeroCnt) + print "Zero Mean: " + str(self.encZeroMean) + print "Zero Var: " + str(self.encZeroVar) + print "***" + print "NonZero Cnt: " + str(self.encNonZeroCnt) + print "NonZero Mean: " + str(self.encNonZeroMean) + print "NonZero Var: " + str(self.encNonZeroVar) + print "***" + print "High Cnt: " + str(self.encHighTotalCnt) + print "High Mean: " + str(self.encHighMean) + print "High Var: " + str(self.encHighVar) + print "***" + print "Low Cnt: " + str(self.encLowTotalCnt) + print "Low Mean: " + str(self.encLowMean) + print "Low Var: " + str(self.encLowVar) + + self.encCnt = self.encCnt + 1 + + # Fill window + for side in range(0, 2): + self.encTime[side] = self.encTimeWin[side][-1] + + self.encBufInd0[side] = self.encBufInd1[side] + self.encBufInd1[side] = ENC_IND[side] + ind0 = self.encBufInd0[side] # starting index + # ending index (this element is not included until the next update) + ind1 = self.encBufInd1[side] + + if ind0 < ind1: + N = ind1 - ind0 # number of new elements + self.encSumN[side] = self.encSumN[side] + N + self.encTimeWin[side] = np.roll(self.encTimeWin[side], -N) + self.encTimeWin[side, -N:] = ENC_TIME[side][ind0:ind1] + self.encValWin[side] = np.roll(self.encValWin[side], -N) + self.encValWin[side, -N:] = ENC_VAL[side][ind0:ind1] + self.encPWMWin[side] = np.roll(self.encPWMWin[side], -N) + self.encPWMWin[side, -N:] = [self.pwm[side]] * N + + elif ind0 > ind1: + N = ENC_BUF_SIZE - ind0 + ind1 # number of new elements + self.encSumN[side] = self.encSumN[side] + N + self.encTimeWin[side] = np.roll(self.encTimeWin[side], -N) + self.encValWin[side] = np.roll(self.encValWin[side], -N) + self.encPWMWin[side] = np.roll(self.encPWMWin[side], -N) + self.encPWMWin[side, -N:] = [self.pwm[side]] * N + if ind1 == 0: + self.encTimeWin[side, -N:] = ENC_TIME[side][ind0:] + self.encValWin[side, -N:] = ENC_VAL[side][ind0:] + else: + self.encTimeWin[side, -N:-ind1] = ENC_TIME[side][ind0:] + self.encValWin[side, -N:-ind1] = ENC_VAL[side][ind0:] + self.encTimeWin[side, -ind1:] = ENC_TIME[side][0:ind1] + self.encValWin[side, -ind1:] = ENC_VAL[side][0:ind1] + + if ind0 != ind1: + tauNew = self.encTimeWin[side, -1] - self.encTimeWin[side, -N] + # Running average + self.encTau[side] = tauNew / self.encCnt + \ + self.encTau[side] * (self.encCnt - 1) / self.encCnt + if self.encSumN[side] > self.encWinSize: + self.countEncoderTicks(side) + + # Fill records + if base.DEBUG: + ind = self.encRecInd[side] + if ind + N < self.encRecSize: + self.encTimeRec[ + side, ind:ind + N] = self.encTimeWin[side, -N:] + self.encValRec[ + side, ind:ind + N] = self.encValWin[side, -N:] + self.encPWMRec[ + side, ind:ind + N] = self.encPWMWin[side, -N:] + self.encNNewRec[side, ind:ind + N] = [N] * N + self.encPosRec[ + side, ind:ind + N] = [self.encPos[side]] * N + self.encVelRec[ + side, ind:ind + N] = [self.encVel[side]] * N + self.encTickStateRec[ + side, ind:ind + N] = self.encTickStateVec[side, + -N:] + self.encThresholdRec[ + side, ind:ind + N] = [self.encThreshold[side]] * N + self.encRecInd[side] = ind + N + + def countEncoderTicks(self, side): + # Set variables + # Time vector of data (non-consistent sampling time) + t = self.encTimeWin[side] + tPrev = self.encTPrev[side] # Previous read time + pwm = self.encPWMWin[side] # Vector of PWM data + pwmPrev = pwm[-1] # Last PWM value that was applied + # Last state of tick (high (1), low (-1), or unsure (0)) + tickStatePrev = self.encTickState[side] + tickCnt = self.encPos[side] # Current tick count + tickVel = self.encVel[side] # Current tick velocity + encValWin = self.encValWin[side] # Encoder raw value buffer window + threshold = self.encThreshold[side] # Encoder value threshold + # Minimum PWM to move wheel + minPWMThreshold = self.minPWMThreshold[side] + + N = np.sum(t > tPrev) # Number of new updates + + tickStateVec = np.roll(self.encTickStateVec[side], -N) + + # Determine wheel direction + if tickVel != 0: + wheelDir = np.sign(tickVel) + else: + wheelDir = np.sign(pwmPrev) + + # Count ticks and record tick state + indTuple = np.where(t == tPrev) # Index of previous sample in window + if len(indTuple[0] > 0): + ind = indTuple[0][0] + newInds = ind + np.arange(1, N + 1) # Indices of new samples + for i in newInds: + if encValWin[i] > threshold: # High tick state + tickState = 1 + self.encHighCnt[side] = self.encHighCnt[side] + 1 + self.encLowCnt[side] = 0 + # Increment tick count on rising edge + if tickStatePrev == -1: + tickCnt = tickCnt + wheelDir + + else: # Low tick state + tickState = -1 + self.encLowCnt[side] = self.encLowCnt[side] + 1 + self.encHighCnt[side] = 0 + tickStatePrev = tickState + tickStateVec[i] = tickState + + # Measure tick speed + # Tick state transition differences + diffTickStateVec = np.diff(tickStateVec) + # Times when tick state goes from high to low + fallingTimes = t[np.hstack((False, diffTickStateVec == -2))] + # Times when tick state goes from low to high + risingTimes = t[np.hstack((False, diffTickStateVec == 2))] + # Period times between falling edges + fallingPeriods = np.diff(fallingTimes) + # Period times between rising edges + risingPeriods = np.diff(risingTimes) + tickPeriods = np.hstack( + (fallingPeriods, risingPeriods)) # All period times + if len(tickPeriods) == 0: + # If all inputs are less than min set velocity to 0 + if all(pwm[newInds] < minPWMThreshold): + tickVel = 0 + else: + # Average signed tick frequency + tickVel = wheelDir * 1 / np.mean(tickPeriods) + + # Estimate new mean values + newEncRaw = encValWin[newInds] + if pwmPrev == 0 and tickVel == 0: + x = newEncRaw + l = self.encZeroCnt[side] + mu = self.encZeroMean[side] + sigma2 = self.encZeroVar[side] + (muPlus, sigma2Plus, n) = utils.recursiveMeanVar(x, l, mu, + sigma2) + self.encZeroMean[side] = muPlus + self.encZeroVar[side] = sigma2Plus + self.encZeroCnt[side] = n + elif tickVel != 0: + if base.DEBUG: + x = newEncRaw + l = self.encNonZeroCnt[side] + mu = self.encNonZeroMean[side] + sigma2 = self.encNonZeroVar[side] + (muPlus, sigma2Plus, n) = utils.recursiveMeanVar( + x, l, mu, sigma2) + self.encNonZeroMean[side] = muPlus + self.encNonZeroVar[side] = sigma2Plus + self.encNonZeroCnt[side] = n + + NHigh = np.sum(tickStateVec[newInds] == 1) + if NHigh != 0: + indHighTuple = np.where(tickStateVec[newInds] == 1) + x = newEncRaw[indHighTuple[0]] + l = self.encHighTotalCnt[side] + mu = self.encHighMean[side] + sigma2 = self.encHighVar[side] + (muPlus, sigma2Plus, n) = utils.recursiveMeanVar( + x, l, mu, sigma2) + self.encHighMean[side] = muPlus + self.encHighVar[side] = sigma2Plus + self.encHighTotalCnt[side] = n + + NLow = np.sum(tickStateVec[newInds] == -1) + if NLow != 0: + indLowTuple = np.where(tickStateVec[newInds] == -1) + x = newEncRaw[indLowTuple[0]] + l = self.encLowTotalCnt[side] + mu = self.encLowMean[side] + sigma2 = self.encLowVar[side] + (muPlus, sigma2Plus, n) = utils.recursiveMeanVar( + x, l, mu, sigma2) + self.encLowMean[side] = muPlus + self.encLowVar[side] = sigma2Plus + self.encLowTotalCnt[side] = n + + # Set threshold value + if self.encZeroCnt[side] > self.encZeroCntMin: + self.encThreshold[side] = self.encZeroMean[ + side] - 3 * np.sqrt(self.encZeroVar[side]) + +# elif self.encNonZeroCnt[side] > self.encNonZeroCntMin: +# self.encThreshold[side] = self.encNonZeroMean[side] + +# elif self.encHighTotalCnt[side] > self.encHighLowCntMin and \ +# self.encLowTotalCnt > self.encHighLowCntMin: +# mu1 = self.encHighMean[side] +# sigma1 = self.encHighVar[side] +# mu2 = self.encLowMean[side] +# sigma2 = self.encLowVar[side] +# alpha = (sigma1 * np.log(sigma1)) / (sigma2 * np.log(sigma1)) +# A = (1 - alpha) +# B = -2 * (mu1 - alpha*mu2) +# C = mu1**2 - alpha * mu2**2 +# x1 = (-B + np.sqrt(B**2 - 4*A*C)) / (2*A) +# x2 = (-B - np.sqrt(B**2 - 4*A*C)) / (2*A) +# if x1 < mu1 and x1 > mu2: +# self.encThreshold[side] = x1 +# else: +# self.encThreshold[side] = x2 + + # Update variables + self.encPos[side] = tickCnt # New tick count + self.encVel[side] = tickVel # New tick velocity + self.encTickStateVec[side] = tickStateVec # New tick state vector + + self.encTPrev[side] = t[-1] # New latest update time + + +class EncoderReader(threading.Thread): + """EncoderReader thread""" + + def __init__(self, encPin=(config.Ol, config.Or)): + + # Initialize thread + threading.Thread.__init__(self) + + # Set properties + self.encPin = encPin + + def run(self): + self.t0 = time.time() + + while base.RUN_FLAG: + global ENC_IND + global ENC_TIME + global ENC_VAL + + for side in range(0, 2): + ENC_TIME[side][ENC_IND[side]] = time.time() - self.t0 + ADC_LOCK.acquire() + ENC_VAL[side][ENC_IND[side]] = ADC.read_raw(self.encPin[side]) + time.sleep(ADCTIME) + ADC_LOCK.release() + ENC_IND[side] = (ENC_IND[side] + 1) % ENC_BUF_SIZE diff --git a/xbots/quickbot_config.py b/xbots/quickbot_config.py new file mode 100644 index 0000000..61008b7 --- /dev/null +++ b/xbots/quickbot_config.py @@ -0,0 +1,43 @@ +# quickbot +# +# _'_'_ IRfm +# ++++++++++++++ +# _/ + + \_ +# IRfl _/ + + \_ IRfr +# / + LMP RMP + \ +# + __| |__ + +# + | | + +# + | | + +# _|++++ | _ _ | ++++|_ +# IRbl _|++++_| T T T T |_++++|_ IRbr +# |++++ | | | | ++++| +# ++++ Ol Or ++++ +# + + +# ++++++++++++++++++ +# +# ir +IRbl = "P9_38" +IRfl = "P9_40" +IRfm = "P9_36" +IRfr = "P9_35" +IRbr = "P9_33" +IRS = (IRbl, IRfl, IRfm, IRfr, IRbr) + +# encoder aka odometry +Ol = "P9_39" +Or = "P9_37" + +# motors +INl1 = "P8_14" +INl2 = "P8_16" +PWMl = "P9_16" + +INr1 = "P8_12" +INr2 = "P8_10" +PWMr = "P9_14" + +RMP = (INr1, INr2, PWMr) +LMP = (INl1, INl2, PWMl) + +# led +LED = "USR1" diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py new file mode 100644 index 0000000..cce97e0 --- /dev/null +++ b/xbots/ultrabot.py @@ -0,0 +1,216 @@ +""" +@brief UltraBot class for Beaglebone Black + +@author Josip Delic (delijati.net) +@date 04/23/2014 +@version: 0.1 +@copyright: Copyright (C) 2014, Georgia Tech Research Corporation +see the LICENSE file included with this software (see LINENSE file) +""" + +from __future__ import division +import threading +import time +import base +import math +import numpy as np + +import ultrabot_config as config +import Adafruit_BBIO.GPIO as GPIO + +# trigger duration +DECPULSETRIGGER = 0.0001 +# loop iterations before timeout called +INTTIMEOUT = 2100 +MAX_METER = 3.50 + +# encoder +WHEEL_RADIUS = 1.2 # cm +TIME_INTERVAL = 1.0 # seconds +TICKS = 20 # holes in disc +CONST = (2 * math.pi * WHEEL_RADIUS)/TICKS + +# globals +ENC_VEL = [0.0, 0.0] +ENC_POS = [0.0, 0.0] +ULTRAS_DIST = [0.0, 0.0, 0.0, 0.0, 0.0] + + +class UltraBot(base.BaseBot): + """The QuickBot Class""" + # Motor Pins -- (LEFT, RIGHT) + dir1Pin = (config.INl1, config.INr1) + dir2Pin = (config.INl2, config.INr2) + pwmPin = (config.PWMl, config.PWMr) + pwm_freq = 100 + + # LED pin + led = config.LED + + # Constraints + pwmLimits = [-100, 100] # [min, max] + + # State PWM -- (LEFT, RIGHT) + pwm = [0, 0] + + # State Ultras + ultraVal = [0.0, 0.0, 0.0, 0.0, 0.0] + + # State ir + irVal = [0, 0, 0, 0, 0] + + # State Encoder + encPos = [0.0, 0.0] # Last encoder tick position + encVel = [0.0, 0.0] # Last encoder tick velocity + + def __init__(self, baseIP, robotIP): + super(UltraBot, self).__init__(baseIP, robotIP) + # init encoder + self.encoderRead = EncoderReader() + + def update(self): + self.read_ultras() + self.read_encoders() + self.parseCmdBuffer() + + def read_ultras(self): + self.ultraVal = ULTRAS_DIST + + def read_encoders(self): + self.encPos = ENC_POS # New tick count + self.encVel = ENC_VEL # New tick velocity + + def run(self): + self.ultraread = UltraReader() + self.ultraread.start() + super(UltraBot, self).run() + + +class UltraReader(threading.Thread): + """UltraReader thread""" + + ultraVal = [0.0, 0.0, 0.0, 0.0, 0.0] + + def __init__(self): + # Initialize thread + threading.Thread.__init__(self) + + self._setup_ultras() + + def _setup_ultras(self): + for trigger, echo in config.ULTRAS: + GPIO.setup(echo, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) + GPIO.setup(trigger, GPIO.OUT) + GPIO.output(trigger, False) + + def _measure_ultra(self, trigger, echo): + GPIO.output(trigger, True) + time.sleep(DECPULSETRIGGER) + GPIO.output(trigger, False) + + # Wait for echo to go high (or timeout) + intcountdown = INTTIMEOUT + + while (GPIO.input(echo) == 0 and intcountdown > 0): + intcountdown = intcountdown - 1 + + # If echo is high + if intcountdown > 0: + + # Start timer and init timeout countdown + echostart = time.time() + intcountdown = INTTIMEOUT + + # Wait for echo to go low (or timeout) + while (GPIO.input(echo) == 1 and intcountdown > 0): + intcountdown = intcountdown - 1 + + # Stop timer + echoend = time.time() + + # Echo duration + echoduration = echoend - echostart + intdistance = (echoduration*1000000) / 58.0 + + return intdistance / 100.0 + + def _reject_outliers(self, data, m=2): + return data[abs(data - np.mean(data)) < m * np.std(data)] + + def run(self): + global ULTRAS_DIST + count = 3 + + while base.RUN_FLAG: + for idx, (trigger, echo) in enumerate(config.ULTRAS): + prevVal = self.ultraVal[idx] + total = [] + # XXX test if its better to do this loop over all ultras then + # calculate the mean + for i in range(count): + _distance = self._measure_ultra(trigger, echo) + if _distance >= MAX_METER or _distance is None: + _distance = prevVal + # time.sleep(0.05) + total.append(_distance) + # distance = np.mean(self._reject_outliers(np.array(total))) + distance = np.mean(total) + self.ultraVal[idx] = distance + ULTRAS_DIST[idx] = distance + + +class EncoderReader(threading.Thread): + """EncoderReader thread""" + + counter_l = 0 + counter_r = 0 + + def __init__(self, encPin=(config.Ol, config.Or)): + GPIO.setup(config.Ol, GPIO.IN) + GPIO.setup(config.Or, GPIO.IN) + + # Initialize thread + threading.Thread.__init__(self) + + def update_encoder_l(self, channel): + global ENC_POS + self.counter_l += 1 + ENC_POS[base.LEFT] += 1 + #print "Encoder (left) counter updated: %d" % self.counter_l + + def update_encoder_r(self, channel): + global ENC_POS + self.counter_r += 1 + ENC_POS[base.RIGHT] += 1 + #print "Encoder (right) counter updated: %d" % self.counter_r + + def run(self): + global ENC_VEL + GPIO.add_event_detect(config.Ol, GPIO.RISING, + callback=self.update_encoder_l) + GPIO.add_event_detect(config.Or, GPIO.RISING, + callback=self.update_encoder_r) + + current_time_l = time.time() + current_time_r = time.time() + while base.RUN_FLAG: + if (time.time() >= current_time_l + TIME_INTERVAL): + velocity_l = ( + self.counter_l * (WHEEL_RADIUS * CONST) + ) / TIME_INTERVAL + ENC_VEL[base.LEFT] = velocity_l + + self.counter_l = 0 + current_time_l = time.time() + # print "velocity_l %s cm/s ticks sum %s" % (velocity_l, + # ENC_POS[base.LEFT]) + if (time.time() >= current_time_r + TIME_INTERVAL): + velocity_r = ( + self.counter_r * (WHEEL_RADIUS * CONST) + ) / TIME_INTERVAL + ENC_VEL[base.RIGHT] = velocity_r + + self.counter_r = 0 + current_time_r = time.time() + # print "velocity_r %s cm/s ticks sum %s" % (velocity_r, + # ENC_POS[base.RIGHT]) diff --git a/xbots/ultrabot_config.py b/xbots/ultrabot_config.py new file mode 100644 index 0000000..d07497d --- /dev/null +++ b/xbots/ultrabot_config.py @@ -0,0 +1,54 @@ +# ultrabot +# +# _'_'_ UXfm +# ++++++++++++++ +# _/ + + \_ +# UXfl _/ + + \_ UXfr +# / + LMP RMP + \ +# + __| |__ + +# + | | + +# + | | + +# _|++++ | _ _ | ++++|_ +# UXbl _|++++_| T T T T |_++++|_ UXbr +# |++++ | | | | ++++| +# ++++ Ol Or ++++ +# + + +# ++++++++++++++++++ +# +# ultrasonic +UTbl = "P8_12" +UEbl = "P8_11" + +UTfl = "P9_21" +UEfl = "P9_22" + +UTfm = "P9_23" +UEfm = "P9_24" + +UTfr = "P9_25" +UEfr = "P9_26" + +UTbr = "P9_27" +UEbr = "P9_30" + +# changed ordering to minimize interferences +ULTRAS = ((UTfm, UEfm), (UTbr, UEbr), (UTfl, UEfl), (UTfr, UEfr), (UTbl, UEbl)) + +# encoder aka odometry +Ol = "P9_41" +Or = "P9_42" + +# motors +INl1 = "P9_11" +INl2 = "P9_12" +PWMl = "P9_14" + +INr1 = "P9_13" +INr2 = "P9_15" +PWMr = "P9_16" + +RMP = (INr1, INr2, PWMr) +LMP = (INl1, INl2, PWMl) + +# led +LED = "USR1" diff --git a/xbots/utils.py b/xbots/utils.py new file mode 100644 index 0000000..c8b54f5 --- /dev/null +++ b/xbots/utils.py @@ -0,0 +1,134 @@ +import time +import numpy as np + +# Tic toc constants +TICTOC_START = 0 +TICTOC_COUNT = 0 +TICTOC_MEAN = 0 +TICTOC_MAX = -float('inf') +TICTOC_MIN = float('inf') + + +def operatingPoint(uStar, uStarThreshold): + """ + This function returns the steady state tick velocity given some PWM input. + + uStar: PWM input. + uStarThreshold: Threshold on the minimum magnitude of a PWM input value + + returns: omegaStar - steady state tick velocity + """ + # Matlab code to find beta values + # X = [40; 80; 100]; % Air Test + # Y = [0.85; 2.144; 3.5]; + # + # r = 0.0325; % Wheel radius + # c = 2*pi*r; + # X = [ 70; 70; 70; 75; 75; 75; 80; 80; 80; 85; 85; + # 85; 90; 90; 90]; % Ground Test + # Z = [4.25; 3.95; 4.23; 3.67; 3.53; 3.48; 3.19; 3.08; 2.93; 2.52; 2.59; + # 2.56; 1.99; 2.02; 2.04]; % Time to go 1 m + # Y = 1./(Z*c); + # H = [X ones(size(X))]; + # beta = H \ Y + # beta = [0.0425, -0.9504] # Air Test Results + beta = [0.0606, -3.1475] # Ground Test Results + + if np.abs(uStar) <= uStarThreshold: + omegaStar = 0.0 + elif uStar > 0: + omegaStar = beta[0] * uStar + beta[1] + else: + omegaStar = -1.0 * (beta[0] * np.abs(uStar) + beta[1]) + + return omegaStar + + +def kalman(x, P, Phi, H, W, V, z): + """ + This function returns an optimal expected value of the state and covariance + error matrix given an update and system parameters. + + x: Estimate of staet at time t-1. + P: Estimate of error covariance matrix at time t-1. + Phi: Discrete time state tranistion matrix at time t-1. + H: Observation model matrix at time t. + W: Process noise covariance at time t-1. + V: Measurement noise covariance at time t. + z: Measurement at time t. + + returns: (x,P) tuple + x: Updated estimate of state at time t. + P: Updated estimate of error covariance matrix at time t. + + """ + x_p = Phi * x # Prediction of setimated state vector + P_p = Phi * P * Phi + W # Prediction of error covariance matrix + S = H * P_p * H + V # Sum of error variances + S_inv = 1 / S # Inverse of sum of error variances + K = P_p * H * S_inv # Kalman gain + r = z - H * x_p # Prediction residual + w = -K * r # Process error + x = x_p - w # Update estimated state vector + # v = z - H * x # Measurement error + if np.isnan(K * V): + P = P_p + else: + # Updated error covariance matrix + P = (1 - K * H) * P_p * (1 - K * H) + K * V * K + return (x, P) + + +def tic(): + global TICTOC_START + TICTOC_START = time.time() + + +def toc(tictocName='toc', printFlag=True): + global TICTOC_START + global TICTOC_COUNT + global TICTOC_MEAN + global TICTOC_MAX + global TICTOC_MIN + + tictocTime = time.time() - TICTOC_START + TICTOC_COUNT = TICTOC_COUNT + 1 + TICTOC_MEAN = tictocTime / TICTOC_COUNT + \ + TICTOC_MEAN * (TICTOC_COUNT - 1) / TICTOC_COUNT + TICTOC_MAX = max(TICTOC_MAX, tictocTime) + TICTOC_MIN = min(TICTOC_MIN, tictocTime) + + if printFlag: + print tictocName + " time: " + str(tictocTime) + + +def tictocPrint(): + global TICTOC_COUNT + global TICTOC_MEAN + global TICTOC_MAX + global TICTOC_MIN + + print "Tic Toc Stats:" + print "Count = " + str(TICTOC_COUNT) + print "Mean = " + str(TICTOC_MEAN) + print "Max = " + str(TICTOC_MAX) + print "Min = " + str(TICTOC_MIN) + + +def recursiveMeanVar(x, l, mu, sigma2): + """ + This function calculates a new mean and variance given + the current mean "mu", current variance "sigma2", current + update count "l", and new samples "x" + """ + m = len(x) + n = l + m + muPlus = l / n * mu + m / n * np.mean(x) + if n > 1: + sigma2Plus = 1 / (n - 1) * ( + (l - 1) * sigma2 + (m - 1) * np.var(x) + l * ( + mu - muPlus) ** 2 + m * (np.mean(x) - muPlus) ** 2) + else: + sigma2Plus = 0 + + return (muPlus, sigma2Plus, n)