From 0f30364603c18e1db1db78365284ca255c8ddefb Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Thu, 17 Apr 2014 17:33:03 +0200 Subject: [PATCH 01/18] bit pep8 --- QuickBot.py | 219 +++++++++++++++++++++++++++++++--------------------- 1 file changed, 130 insertions(+), 89 deletions(-) diff --git a/QuickBot.py b/QuickBot.py index 9506efc..49fe34c 100755 --- a/QuickBot.py +++ b/QuickBot.py @@ -31,27 +31,28 @@ ADCTIME = 0.001 -## Tic toc constants +# 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 +# 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] +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 variables RUN_FLAG = True RUN_FLAG_LOCK = threading.Lock() class QuickBot(): + """The QuickBot Class""" # === Class Properties === @@ -72,7 +73,7 @@ class QuickBot(): # Encoder counting parameter and variables ticksPerTurn = 16 # Number of ticks on encoder disc - encWinSize = 2**5 # Should be power of 2 + 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] @@ -99,13 +100,17 @@ class QuickBot(): 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 + # 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 - encZeroCntMin = 2**4 # Min number of recorded values to start calculating stats + # 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] @@ -152,25 +157,26 @@ def __init__(self, baseIP, robotIP): 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 + # 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.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.encRecSize = 2 ** 13 self.encRecInd = [0, 0] self.encTimeRec = np.zeros((2, self.encRecSize)) self.encValRec = np.zeros((2, self.encRecSize)) @@ -268,7 +274,8 @@ def parseCmdBuffer(self): self.cmdBuffer += line - bufferPattern = r'\$[^\$\*]*?\*' # String contained within $ and * symbols with no $ or * symbols in it + # String contained within $ and * symbols with no $ or * symbols in it + bufferPattern = r'\$[^\$\*]*?\*' bufferRegex = re.compile(bufferPattern) bufferResult = bufferRegex.search(self.cmdBuffer) @@ -282,11 +289,13 @@ def parseCmdBuffer(self): msgResult = msgRegex.search(msg) if msgResult.group('CMD') == 'CHECK': - self.robotSocket.sendto('Hello from QuickBot\n',(self.baseIP, self.port)) + 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)) + self.robotSocket.sendto( + str(self.pwm) + '\n', (self.baseIP, self.port)) elif msgResult.group('SET') and msgResult.group('ARGS'): args = msgResult.group('ARGS') @@ -295,26 +304,29 @@ def parseCmdBuffer(self): pwmResult = pwmRegex.match(args) if pwmResult: pwm = [int(pwmRegex.match(args).group('LEFT')), - int(pwmRegex.match(args).group('RIGHT'))] + 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)) + 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)) + 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)) + self.robotSocket.sendto( + reply + '\n', (self.baseIP, self.port)) elif msgResult.group('CMD') == 'RESET': self.encPos[LEFT] = 0.0 @@ -329,13 +341,14 @@ def parseCmdBuffer(self): pwmResult = pwmRegex.match(args) if pwmResult: pwm = [int(pwmRegex.match(args).group('LEFT')), - int(pwmRegex.match(args).group('RIGHT'))] + 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)) + self.robotSocket.sendto( + reply + '\n', (self.baseIP, self.port)) elif msgResult.group('CMD') == 'END': RUN_FLAG_LOCK.acquire() @@ -350,10 +363,9 @@ def readIRValues(self): ADC_LOCK.release() if self.irVal[self.ithIR] >= 1100: - self.irVal[self.ithIR] = prevVal - - self.ithIR = ((self.ithIR+1) % 5) + self.irVal[self.ithIR] = prevVal + self.ithIR = ((self.ithIR + 1) % 5) def readEncoderValues(self): if DEBUG and (self.encCnt % 10) == 0: @@ -388,7 +400,8 @@ def readEncoderValues(self): 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) + # 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 @@ -398,7 +411,7 @@ def readEncoderValues(self): 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 + self.encPWMWin[side, -N:] = [self.pwm[side]] * N elif ind0 > ind1: N = ENC_BUF_SIZE - ind0 + ind1 # number of new elements @@ -406,7 +419,7 @@ def readEncoderValues(self): 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 + 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:] @@ -417,37 +430,48 @@ def readEncoderValues(self): 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 + 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 + 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) + # 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 - tickStatePrev = self.encTickState[side] # Last state of tick (high (1), low (-1), or unsure (0)) + # 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 - minPWMThreshold = self.minPWMThreshold[side] # Minimum PWM to move wheel + # Minimum PWM to move wheel + minPWMThreshold = self.minPWMThreshold[side] N = np.sum(t > tPrev) # Number of new updates @@ -463,13 +487,14 @@ def countEncoderTicks(self, side): 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 + 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 + # Increment tick count on rising edge + if tickStatePrev == -1: tickCnt = tickCnt + wheelDir else: # Low tick state @@ -480,17 +505,25 @@ def countEncoderTicks(self, side): 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 + # 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(pwm[newInds] < minPWMThreshold): # If all inputs are less than min set velocity to 0 + # If all inputs are less than min set velocity to 0 + if all(pwm[newInds] < minPWMThreshold): tickVel = 0 else: - tickVel = wheelDir * 1/np.mean(tickPeriods) # Average signed tick frequency + # Average signed tick frequency + tickVel = wheelDir * 1 / np.mean(tickPeriods) # Estimate new mean values newEncRaw = encValWin[newInds] @@ -509,7 +542,8 @@ def countEncoderTicks(self, side): l = self.encNonZeroCnt[side] mu = self.encNonZeroMean[side] sigma2 = self.encNonZeroVar[side] - (muPlus, sigma2Plus, n) = recursiveMeanVar(x, l, mu, sigma2) + (muPlus, sigma2Plus, n) = recursiveMeanVar( + x, l, mu, sigma2) self.encNonZeroMean[side] = muPlus self.encNonZeroVar[side] = sigma2Plus self.encNonZeroCnt[side] = n @@ -521,7 +555,8 @@ def countEncoderTicks(self, side): l = self.encHighTotalCnt[side] mu = self.encHighMean[side] sigma2 = self.encHighVar[side] - (muPlus, sigma2Plus, n) = recursiveMeanVar(x, l, mu, sigma2) + (muPlus, sigma2Plus, n) = recursiveMeanVar( + x, l, mu, sigma2) self.encHighMean[side] = muPlus self.encHighVar[side] = sigma2Plus self.encHighTotalCnt[side] = n @@ -533,14 +568,16 @@ def countEncoderTicks(self, side): l = self.encLowTotalCnt[side] mu = self.encLowMean[side] sigma2 = self.encLowVar[side] - (muPlus, sigma2Plus, n) = recursiveMeanVar(x, l, mu, sigma2) + (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]) + self.encThreshold[side] = self.encZeroMean[ + side] - 3 * np.sqrt(self.encZeroVar[side]) # elif self.encNonZeroCnt[side] > self.encNonZeroCntMin: # self.encThreshold[side] = self.encNonZeroMean[side] @@ -561,7 +598,6 @@ def countEncoderTicks(self, side): # else: # self.encThreshold[side] = x2 - # Update variables self.encPos[side] = tickCnt # New tick count self.encVel[side] = tickVel # New tick velocity @@ -569,11 +605,12 @@ def countEncoderTicks(self, side): 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], \ + 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)] @@ -586,6 +623,7 @@ def writeBuffersToFile(self): class encoderRead(threading.Thread): + """The encoderRead Class""" # === Class Properties === @@ -593,7 +631,7 @@ class encoderRead(threading.Thread): # === Class Methods === # Constructor - def __init__(self,encPin=('P9_39', 'P9_37')): + def __init__(self, encPin=('P9_39', 'P9_37')): # Initialize thread threading.Thread.__init__(self) @@ -631,12 +669,14 @@ def recursiveMeanVar(x, l, mu, sigma2): 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) + 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. @@ -658,14 +698,14 @@ def operatingPoint(uStar, uStarThreshold): # H = [X ones(size(X))]; # beta = H \ Y # beta = [0.0425, -0.9504] # Air Test Results - beta = [0.0606, -3.1475] # Ground 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] + omegaStar = beta[0] * uStar + beta[1] else: - omegaStar = -1.0*(beta[0]*np.abs(uStar) + beta[1]) + omegaStar = -1.0 * (beta[0] * np.abs(uStar) + beta[1]) return omegaStar @@ -688,19 +728,20 @@ def kalman(x, P, Phi, H, W, V, z): 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_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): + 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 + # Updated error covariance matrix + P = (1 - K * H) * P_p * (1 - K * H) + K * V * K return (x, P) @@ -710,7 +751,7 @@ def tic(): TICTOC_START = time.time() -def toc(tictocName = 'toc', printFlag = True): +def toc(tictocName='toc', printFlag=True): global TICTOC_START global TICTOC_COUNT global TICTOC_MEAN @@ -719,13 +760,15 @@ def toc(tictocName = 'toc', printFlag = True): 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) + 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 @@ -737,5 +780,3 @@ def tictocPrint(): print "Mean = " + str(TICTOC_MEAN) print "Max = " + str(TICTOC_MAX) print "Min = " + str(TICTOC_MIN) - - From b91f9334735adc4d8b3300889284a88a3015826b Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Wed, 23 Apr 2014 17:25:37 +0200 Subject: [PATCH 02/18] restructured code added ultrabot (ultrasonic, different encoders) --- .gitignore | 56 +++ QuickBot.py | 782 --------------------------------------- QuickBotRun.py | 39 -- README.md | 12 +- run.py | 57 +++ xbots/__init__.py | 1 + xbots/base.py | 232 ++++++++++++ xbots/quickbot.py | 390 +++++++++++++++++++ xbots/quickbot_config.py | 43 +++ xbots/ultrabot.py | 169 +++++++++ xbots/ultrabot_config.py | 53 +++ xbots/utils.py | 134 +++++++ 12 files changed, 1141 insertions(+), 827 deletions(-) create mode 100644 .gitignore delete mode 100755 QuickBot.py delete mode 100755 QuickBotRun.py create mode 100644 run.py create mode 100644 xbots/__init__.py create mode 100644 xbots/base.py create mode 100644 xbots/quickbot.py create mode 100644 xbots/quickbot_config.py create mode 100644 xbots/ultrabot.py create mode 100644 xbots/ultrabot_config.py create mode 100644 xbots/utils.py 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 49fe34c..0000000 --- a/QuickBot.py +++ /dev/null @@ -1,782 +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 - # 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 - - # 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 - # 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)) - - # 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 - - # String contained within $ and * symbols with no $ or * symbols in it - bufferPattern = r'\$[^\$\*]*?\*' - 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 - # 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] - 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 - # 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) = 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: - # 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) 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..dec793f 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ -# QuickBot_BBB -This is the code that runs on the BeagleBone Black to control the QuickBot. +# ultrabot_BBB +This is the code that runs on the BeagleBone Black to control the ultrabot. ## 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. @@ -32,20 +32,20 @@ Change into working directory: cd ~/quickbot_bbb -Launch QuickBotRun python script using IP addresses of BASE and ROBOT: +Launch run python script using IP addresses of BASE and ROBOT: - ./QuickBotRun.py 192.168.1.100 192.168.1.101 + ./run.py 192.168.1.100 192.168.1.101 ## Command Set -* Check that the QuickBot is up and running: +* Check that the ultrabot is up and running: * Command "$CHECK*\n" * Response - "Hello from QuickBot\n" + "Hello from ultrabot\n" * Get PWM values: diff --git a/run.py b/run.py new file mode 100644 index 0000000..a56b86c --- /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..584b8c2 --- /dev/null +++ b/xbots/base.py @@ -0,0 +1,232 @@ +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 + + # Variables + ledFlag = True + cmdBuffer = '' + + # UDP + port = 5005 + robotSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + robotSocket.setblocking(False) + + 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): + """ Initialize PWM pins: + PWM.start(channel, duty, freq=2000, polarity=0) + """ + PWM.start(self.pwmPin[LEFT], 0) + PWM.start(self.pwmPin[RIGHT], 0) + + 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') == '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 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.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..4cb08e2 --- /dev/null +++ b/xbots/quickbot.py @@ -0,0 +1,390 @@ +#!/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) + # Initialize ADC + ADC.setup() + + 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..1a597e7 --- /dev/null +++ b/xbots/ultrabot.py @@ -0,0 +1,169 @@ +""" +@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 quickbot_config as config +import Adafruit_BBIO.GPIO as GPIO + +# trigger duration +DECPULSETRIGGER = 0.0001 +# loop iterations before timeout called +INTTIMEOUT = 2100 +MAX_CENTIMETER = 350 + +# 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] + + +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) + + # LED pin + led = config.LED + + # Constraints + pwmLimits = [-100, 100] # [min, max] + + # State PWM -- (LEFT, RIGHT) + pwm = [0, 0] + + # State Ultras + irVal = [0.0, 0.0, 0.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) + + def update(self): + self.read_ultras() + self.read_encoders() + self.parseCmdBuffer() + + def _measure_ultra(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 + + # Display distance + if intcountdown > 0: + intdistance = (echoduration*1000000)/58 + print "Distance = " + str(intdistance) + "cm" + return intdistance + + def read_ultras(self): + for idx, (trigger, echo) in enumerate(config.ULTRAS): + prevVal = self.irVal[idx] + distance = self._measure_ultra(trigger, echo) + self.irVal[idx] = distance + + if self.irVal[idx] >= MAX_CENTIMETER or distance is None: + self.irVal[idx] = prevVal + + def read_encoders(self): + self.encPos = ENC_POS # New tick count + self.encVel = ENC_VEL # New tick velocity + + +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 = self.counter_l + 1 + ENC_POS[base.LEFT] = self.counter_l + #print "Encoder (left) counter updated: %d" % self.counter_l + + def update_encoder_r(self, channel): + global ENC_POS + self.counter_r = self.counter_r + 1 + ENC_POS[base.RIGHT] = self.counter_l + #print "Encoder (right) counter updated: %d" % self.counter_r + + def run(self): + 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): + global ENC_VEL + 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" % velocity_l + if (time.time() >= current_time_r + TIME_INTERVAL): + global ENC_VEL + velocity_r = ( + self.counter_r * (WHEEL_RADIUS * CONST) + ) / TIME_INTERVAL + ENC_VEL[base.RIGHT] = velocity_l + + self.counter_r = 0 + current_time_r = time.time() + print "velocity_r %s cm/s" % velocity_r diff --git a/xbots/ultrabot_config.py b/xbots/ultrabot_config.py new file mode 100644 index 0000000..4464fa9 --- /dev/null +++ b/xbots/ultrabot_config.py @@ -0,0 +1,53 @@ +# 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" + +ULTRAS = ((UTbl, UEbl), (UTfl, UEfl), (UTfm, UEfm), (UTfr, UEfr), (UTbr, UEbr)) + +# 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) From f21429814a375e2933c67a9a91ef619f63c889b5 Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Wed, 23 Apr 2014 15:46:28 +0000 Subject: [PATCH 03/18] minore argument fix --- run.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/run.py b/run.py index a56b86c..01a6065 100644 --- a/run.py +++ b/run.py @@ -46,7 +46,7 @@ def main(options): default='192.168.7.2', help="BBB ip (robot ip)") parser.add_argument( - '--rtype', 't', + '--rtype', '-t', default='quick', help="Type of robot (%s)" % '|'.join(RTYPES)) From 6eaf43d421cb17a337524d79b8faef1fc32413db Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Wed, 23 Apr 2014 21:42:11 +0200 Subject: [PATCH 04/18] added __init__ to base Bot --- xbots/base.py | 15 +++++++++++++++ xbots/quickbot.py | 34 ++++++++++++++++++++++++++++++++++ xbots/ultrabot.py | 2 ++ 3 files changed, 51 insertions(+) diff --git a/xbots/base.py b/xbots/base.py index 584b8c2..6cd706f 100644 --- a/xbots/base.py +++ b/xbots/base.py @@ -32,6 +32,21 @@ class BaseBot(object): 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() + + # 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) diff --git a/xbots/quickbot.py b/xbots/quickbot.py index 4cb08e2..d69f33d 100644 --- a/xbots/quickbot.py +++ b/xbots/quickbot.py @@ -95,9 +95,43 @@ class QuickBot(base.BaseBot): 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() diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py index 1a597e7..9e97a64 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -59,6 +59,8 @@ class UltraBot(base.BaseBot): def __init__(self, baseIP, robotIP): super(UltraBot, self).__init__(baseIP, robotIP) + # init encoder + self.encoderRead = EncoderReader() def update(self): self.read_ultras() From f1dbbcf5b253bf4c9613ffed4dd83966e558680d Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Wed, 23 Apr 2014 20:26:27 +0000 Subject: [PATCH 05/18] added ultras gpio setup --- xbots/base.py | 1 + xbots/ultrabot.py | 14 +++++++++++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/xbots/base.py b/xbots/base.py index 6cd706f..2aad5a0 100644 --- a/xbots/base.py +++ b/xbots/base.py @@ -46,6 +46,7 @@ def __init__(self, baseIP, robotIP): self.baseIP = baseIP self.robotIP = robotIP self.robotSocket.bind((self.robotIP, self.port)) + print self.robotSocket def _setup_gpio(self): """Initialize GPIO pins""" diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py index 9e97a64..b869e1a 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -14,7 +14,7 @@ import base import math -import quickbot_config as config +import ultrabot_config as config import Adafruit_BBIO.GPIO as GPIO # trigger duration @@ -61,13 +61,21 @@ def __init__(self, baseIP, robotIP): super(UltraBot, self).__init__(baseIP, robotIP) # init encoder self.encoderRead = EncoderReader() + # init ultras + self._setup_ultras() def update(self): self.read_ultras() self.read_encoders() self.parseCmdBuffer() - def _measure_ultra(trigger, echo): + 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) @@ -98,7 +106,7 @@ def _measure_ultra(trigger, echo): # Display distance if intcountdown > 0: intdistance = (echoduration*1000000)/58 - print "Distance = " + str(intdistance) + "cm" + #print "Distance = " + str(intdistance) + "cm" return intdistance def read_ultras(self): From 313c3d41cd1e3bc0092cd6d7c30682b5f8458ca9 Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Mon, 28 Apr 2014 14:09:17 +0000 Subject: [PATCH 06/18] removed print --- xbots/base.py | 2 +- xbots/ultrabot.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/xbots/base.py b/xbots/base.py index 2aad5a0..192c7c5 100644 --- a/xbots/base.py +++ b/xbots/base.py @@ -46,7 +46,7 @@ def __init__(self, baseIP, robotIP): self.baseIP = baseIP self.robotIP = robotIP self.robotSocket.bind((self.robotIP, self.port)) - print self.robotSocket + print self.robotIP def _setup_gpio(self): """Initialize GPIO pins""" diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py index b869e1a..6102993 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -166,14 +166,14 @@ def run(self): self.counter_l = 0 current_time_l = time.time() - print "velocity_l %s cm/s" % velocity_l + #print "velocity_l %s cm/s" % velocity_l if (time.time() >= current_time_r + TIME_INTERVAL): global ENC_VEL velocity_r = ( self.counter_r * (WHEEL_RADIUS * CONST) ) / TIME_INTERVAL - ENC_VEL[base.RIGHT] = velocity_l + ENC_VEL[base.RIGHT] = velocity_r self.counter_r = 0 current_time_r = time.time() - print "velocity_r %s cm/s" % velocity_r + #print "velocity_r %s cm/s" % velocity_r From 684dab4709aa5ddaf708cf00e8c6dd35cdf5185f Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Tue, 29 Apr 2014 17:42:20 +0000 Subject: [PATCH 07/18] first attempt of setting frequency --- xbots/base.py | 14 +++++++++----- xbots/ultrabot.py | 1 + 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/xbots/base.py b/xbots/base.py index 192c7c5..d4b786a 100644 --- a/xbots/base.py +++ b/xbots/base.py @@ -22,6 +22,7 @@ class BaseBot(object): # Parameters sampleTime = 20.0 / 1000.0 + pwm_freq = 2000 # Variables ledFlag = True @@ -37,7 +38,7 @@ def __init__(self, baseIP, robotIP): self._setup_gpio() # Initialize PWM pins: PWM.start(channel, duty, freq=2000, polarity=0) - self._init_pwm() + self._init_pwm(self.pwm_freq) # Set motor speed to 0 self.setPWM([0, 0]) @@ -46,7 +47,6 @@ def __init__(self, baseIP, robotIP): self.baseIP = baseIP self.robotIP = robotIP self.robotSocket.bind((self.robotIP, self.port)) - print self.robotIP def _setup_gpio(self): """Initialize GPIO pins""" @@ -56,12 +56,16 @@ def _setup_gpio(self): GPIO.setup(self.dir2Pin[RIGHT], GPIO.OUT) GPIO.setup(self.led, GPIO.OUT) - def _init_pwm(self): + def _init_pwm(self, frequency=2000): """ Initialize PWM pins: PWM.start(channel, duty, freq=2000, polarity=0) """ - PWM.start(self.pwmPin[LEFT], 0) - PWM.start(self.pwmPin[RIGHT], 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 diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py index 6102993..03b8bb8 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -40,6 +40,7 @@ class UltraBot(base.BaseBot): dir1Pin = (config.INl1, config.INr1) dir2Pin = (config.INl2, config.INr2) pwmPin = (config.PWMl, config.PWMr) + pwm_freq = 100 # LED pin led = config.LED From c9e99f02f37c7bcc8164137aa62bcd8e3017381c Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Wed, 30 Apr 2014 17:28:28 +0000 Subject: [PATCH 08/18] added ULTRAVAL to cmd --- xbots/base.py | 7 +++++++ xbots/ultrabot.py | 11 +++++++---- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/xbots/base.py b/xbots/base.py index d4b786a..86d402c 100644 --- a/xbots/base.py +++ b/xbots/base.py @@ -152,6 +152,13 @@ def parseCmdBuffer(self): 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)) + ']' diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py index 03b8bb8..324caf0 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -52,6 +52,9 @@ class UltraBot(base.BaseBot): pwm = [0, 0] # State Ultras + ultraVal = [0.0, 0.0, 0.0, 0.0, 0.0] + + # State ir irVal = [0.0, 0.0, 0.0, 0.0, 0.0] # State Encoder @@ -112,12 +115,12 @@ def _measure_ultra(self, trigger, echo): def read_ultras(self): for idx, (trigger, echo) in enumerate(config.ULTRAS): - prevVal = self.irVal[idx] + prevVal = self.ultraVal[idx] distance = self._measure_ultra(trigger, echo) - self.irVal[idx] = distance + self.ultraVal[idx] = distance - if self.irVal[idx] >= MAX_CENTIMETER or distance is None: - self.irVal[idx] = prevVal + if self.ultraVal[idx] >= MAX_CENTIMETER or distance is None: + self.ultraVal[idx] = prevVal def read_encoders(self): self.encPos = ENC_POS # New tick count From e54db9c513ef2062a69f65502ffcc69740e6b0d1 Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Sat, 3 May 2014 16:56:45 +0000 Subject: [PATCH 09/18] added docu --- README.md | 45 ++++++++++++++++++++++++++++++++++++--------- xbots/ultrabot.py | 2 +- 2 files changed, 37 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index dec793f..c286fd4 100644 --- a/README.md +++ b/README.md @@ -2,16 +2,22 @@ This is the code that runs on the BeagleBone Black to control the ultrabot. ## 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,15 +32,28 @@ 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 run python script using IP addresses of BASE and ROBOT: +Quick run with given ips: - ./run.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 @@ -45,7 +64,7 @@ Launch run python script using IP addresses of BASE and ROBOT: * Response - "Hello from ultrabot\n" + "Hello from [Quick|Ultra]Bot\n" * Get PWM values: @@ -73,6 +92,14 @@ Launch run 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/xbots/ultrabot.py b/xbots/ultrabot.py index 324caf0..c97ea88 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -55,7 +55,7 @@ class UltraBot(base.BaseBot): ultraVal = [0.0, 0.0, 0.0, 0.0, 0.0] # State ir - irVal = [0.0, 0.0, 0.0, 0.0, 0.0] + irVal = [0, 0, 0, 0, 0] # State Encoder encPos = [0.0, 0.0] # Last encoder tick position From 3bef9172a231a0b99dfcc798286b5b9131c98dea Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Tue, 6 May 2014 12:39:06 +0000 Subject: [PATCH 10/18] fixed global encoder count --- xbots/base.py | 4 ++-- xbots/ultrabot.py | 11 +++++------ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/xbots/base.py b/xbots/base.py index 86d402c..3d13e23 100644 --- a/xbots/base.py +++ b/xbots/base.py @@ -174,8 +174,8 @@ def parseCmdBuffer(self): reply + '\n', (self.baseIP, self.port)) elif msgResult.group('CMD') == 'RESET': - self.encPos[LEFT] = 0.0 - self.encPos[RIGHT] = 0.0 + self.encPos[LEFT] = 0 + self.encPos[RIGHT] = 0 print 'Encoder values reset to [' + ', '.join( map(str, self.encVel)) + ']' diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py index c97ea88..8468748 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -142,17 +142,18 @@ def __init__(self, encPin=(config.Ol, config.Or)): def update_encoder_l(self, channel): global ENC_POS - self.counter_l = self.counter_l + 1 - ENC_POS[base.LEFT] = self.counter_l + 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 = self.counter_r + 1 - ENC_POS[base.RIGHT] = self.counter_l + 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, @@ -162,7 +163,6 @@ def run(self): current_time_r = time.time() while base.RUN_FLAG: if (time.time() >= current_time_l + TIME_INTERVAL): - global ENC_VEL velocity_l = ( self.counter_l * (WHEEL_RADIUS * CONST) ) / TIME_INTERVAL @@ -172,7 +172,6 @@ def run(self): current_time_l = time.time() #print "velocity_l %s cm/s" % velocity_l if (time.time() >= current_time_r + TIME_INTERVAL): - global ENC_VEL velocity_r = ( self.counter_r * (WHEEL_RADIUS * CONST) ) / TIME_INTERVAL From 5d839db581ecf3b6f0bbb6713492c687b88f8986 Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Tue, 6 May 2014 13:09:22 +0000 Subject: [PATCH 11/18] added nicer print for velocity --- xbots/ultrabot.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py index 8468748..264347e 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -170,7 +170,8 @@ def run(self): self.counter_l = 0 current_time_l = time.time() - #print "velocity_l %s cm/s" % velocity_l + # 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) @@ -179,4 +180,5 @@ def run(self): self.counter_r = 0 current_time_r = time.time() - #print "velocity_r %s cm/s" % velocity_r + # print "velocity_r %s cm/s ticks sum %s" % (velocity_r, + # ENC_POS[base.RIGHT]) From 2caf2f3c42e5a0dcd8d6f6262b31418d630ba430 Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Thu, 8 May 2014 17:05:58 +0000 Subject: [PATCH 12/18] use meter --- xbots/ultrabot.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py index 264347e..6ec5ad3 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -21,7 +21,7 @@ DECPULSETRIGGER = 0.0001 # loop iterations before timeout called INTTIMEOUT = 2100 -MAX_CENTIMETER = 350 +MAX_METER = 3.50 # encoder WHEEL_RADIUS = 1.2 # cm @@ -109,9 +109,9 @@ def _measure_ultra(self, trigger, echo): # Display distance if intcountdown > 0: - intdistance = (echoduration*1000000)/58 - #print "Distance = " + str(intdistance) + "cm" - return intdistance + intdistance = (echoduration*1000000) / 58.0 + #print "Distance = " + str(intdistance) + "m" + return intdistance / 100.0 def read_ultras(self): for idx, (trigger, echo) in enumerate(config.ULTRAS): @@ -119,7 +119,7 @@ def read_ultras(self): distance = self._measure_ultra(trigger, echo) self.ultraVal[idx] = distance - if self.ultraVal[idx] >= MAX_CENTIMETER or distance is None: + if self.ultraVal[idx] >= MAX_METER or distance is None: self.ultraVal[idx] = prevVal def read_encoders(self): From 2e8d21aa16c63d317af36966fdb395eb732f78a9 Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Fri, 9 May 2014 11:21:33 +0000 Subject: [PATCH 13/18] typo --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index c286fd4..7cfeb14 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ -# ultrabot_BBB -This is the code that runs on the BeagleBone Black to control the ultrabot. +# 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 @@ -57,7 +57,7 @@ All commands: ## Command Set -* Check that the ultrabot is up and running: +* Check that the bot is up and running: * Command "$CHECK*\n" From 712b5ebff6a261b0046bbdad6d3913d2b7ce1b98 Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Mon, 12 May 2014 14:56:53 +0000 Subject: [PATCH 14/18] changed measure ordering --- xbots/ultrabot_config.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/xbots/ultrabot_config.py b/xbots/ultrabot_config.py index 4464fa9..d07497d 100644 --- a/xbots/ultrabot_config.py +++ b/xbots/ultrabot_config.py @@ -31,7 +31,8 @@ UTbr = "P9_27" UEbr = "P9_30" -ULTRAS = ((UTbl, UEbl), (UTfl, UEfl), (UTfm, UEfm), (UTfr, UEfr), (UTbr, UEbr)) +# changed ordering to minimize interferences +ULTRAS = ((UTfm, UEfm), (UTbr, UEbr), (UTfl, UEfl), (UTfr, UEfr), (UTbl, UEbl)) # encoder aka odometry Ol = "P9_41" From 9c84ef6fe4ff5323a382fbc090274f0db20b151c Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Thu, 15 May 2014 13:20:25 +0000 Subject: [PATCH 15/18] removed not needed if statement --- xbots/ultrabot.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py index 6ec5ad3..2f399f8 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -106,11 +106,8 @@ def _measure_ultra(self, trigger, echo): # Echo duration echoduration = echoend - echostart - - # Display distance - if intcountdown > 0: intdistance = (echoduration*1000000) / 58.0 - #print "Distance = " + str(intdistance) + "m" + return intdistance / 100.0 def read_ultras(self): From d90d3e9e89bb736a5f84f13c1e2eea9103246661 Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Thu, 15 May 2014 15:40:28 +0000 Subject: [PATCH 16/18] moved ultra mesurement into a thread --- xbots/base.py | 2 +- xbots/ultrabot.py | 50 +++++++++++++++++++++++++++++++++++------------ 2 files changed, 38 insertions(+), 14 deletions(-) diff --git a/xbots/base.py b/xbots/base.py index 3d13e23..bee5a27 100644 --- a/xbots/base.py +++ b/xbots/base.py @@ -205,7 +205,7 @@ def run(self): global RUN_FLAG self.encoderRead.start() try: - while RUN_FLAG is True: + while RUN_FLAG: self.update() # Flash BBB LED diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py index 2f399f8..2e1406a 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -32,6 +32,7 @@ # 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): @@ -65,14 +66,36 @@ def __init__(self, baseIP, robotIP): super(UltraBot, self).__init__(baseIP, robotIP) # init encoder self.encoderRead = EncoderReader() - # init ultras - self._setup_ultras() 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) @@ -110,18 +133,19 @@ def _measure_ultra(self, trigger, echo): return intdistance / 100.0 - def read_ultras(self): - for idx, (trigger, echo) in enumerate(config.ULTRAS): - prevVal = self.ultraVal[idx] - distance = self._measure_ultra(trigger, echo) - self.ultraVal[idx] = distance - - if self.ultraVal[idx] >= MAX_METER or distance is None: - self.ultraVal[idx] = prevVal + def run(self): + global ULTRAS_DIST - def read_encoders(self): - self.encPos = ENC_POS # New tick count - self.encVel = ENC_VEL # New tick velocity + while base.RUN_FLAG: + for idx, (trigger, echo) in enumerate(config.ULTRAS): + prevVal = self.ultraVal[idx] + distance = self._measure_ultra(trigger, echo) + self.ultraVal[idx] = distance + ULTRAS_DIST[idx] = distance + + if self.ultraVal[idx] >= MAX_METER or distance is None: + self.ultraVal[idx] = prevVal + ULTRAS_DIST[idx] = prevVal class EncoderReader(threading.Thread): From e4c307fb297f979ad2cc0aeb1502bc4ff4be76b2 Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Fri, 16 May 2014 08:11:14 +0000 Subject: [PATCH 17/18] measure COUNT time and take mean of measurement in ultras --- xbots/ultrabot.py | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py index 2e1406a..d74b263 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -13,6 +13,7 @@ import time import base import math +import numpy as np import ultrabot_config as config import Adafruit_BBIO.GPIO as GPIO @@ -135,18 +136,24 @@ def _measure_ultra(self, trigger, echo): def run(self): global ULTRAS_DIST + count = 3 while base.RUN_FLAG: for idx, (trigger, echo) in enumerate(config.ULTRAS): prevVal = self.ultraVal[idx] - distance = self._measure_ultra(trigger, echo) + 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(total) self.ultraVal[idx] = distance ULTRAS_DIST[idx] = distance - if self.ultraVal[idx] >= MAX_METER or distance is None: - self.ultraVal[idx] = prevVal - ULTRAS_DIST[idx] = prevVal - class EncoderReader(threading.Thread): """EncoderReader thread""" From bee217ca5a5dec48997e51659d09c820a7ae28dc Mon Sep 17 00:00:00 2001 From: Josip Delic Date: Sat, 17 May 2014 11:46:09 +0000 Subject: [PATCH 18/18] added outliner detection --- xbots/ultrabot.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/xbots/ultrabot.py b/xbots/ultrabot.py index d74b263..cce97e0 100644 --- a/xbots/ultrabot.py +++ b/xbots/ultrabot.py @@ -134,6 +134,9 @@ def _measure_ultra(self, trigger, echo): 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 @@ -150,6 +153,7 @@ def run(self): _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