def reportRange(self, info):
'''
example
{u'Ranges': {u'Right': 55, u'Deltat': 12313264, u'Bottom': -1, u'Back': 18, u'Forward': 67, u'Left': 18},
u'Timestamp': 0.07504105567932129}
'''
if robtimer() - self.lastRangeTime >= self.rangeInterval:
#type checking is redundant here, info is known to be Range
if type(info) == dict:
if 'Ranges' in info:
'''
For now, let's always report
f = info['Ranges']['Forward']
l = info['Ranges']['Left']
r = info['Ranges']['Right']
b = info['Ranges']['Back']
btm = info['Ranges']['Bottom']
lastf = self.lastRanges['Ranges']['Forward']
lastl = self.lastRanges['Ranges']['Left']
lastr = self.lastRanges['Ranges']['Right']
lastb = self.lastRanges['Ranges']['Back']
lastbtm = self.lastRanges['Ranges']['Bottom']
if abs(f-lastf) > self.rangeNoise or \
abs(l-lastl) > self.rangeNoise or \
abs(r-lastr) > self.rangeNoise or \
abs(b-lastb) > self.rangeNoise or \
abs(btm-lastbtm) > self.rangeNoise:
'''
self.broadcastQ.put(info)
self.lastRangeTime = robtimer()
self.lastRanges = info
def go(self):
while True:
loopStartTime = robtimer()
if not self.comQ.empty():
command = self.comQ.get_nowait()
if command == "Shutdown":
break
if len(command) == 2:
try:
c = eval("self.%s" % command[0])
p = command[1]
c(p)
except:
raise
for i in self.iL:
payload = None
if not i['queue'].empty():
payload = i['queue'].get_nowait()
if payload:
for o in self.oL:
if i['type'] == o['type']:
if True: # replace with timing logic
o['publisher'](payload, o['queue'])
elapsedTime = robtimer() - loopStartTime
if self.minLoopTime > elapsedTime:
time.sleep(self.minLoopTime - elapsedTime)
self.end()
def genericSubscriber(self, msg):
if self.lastLogTime == 0:
logging.info('\n\nInitiating controller logging.')
self.lastLogTime = robtimer()
self.logsAllowed = 0
if robtimer() - self.lastLogTime >= 10:
logging.debug(str(msg))
self.logsAllowed += 1
if self.logsAllowed > 4: # yep, hard coded kludge for the moment. need non-generic subscribers.
self.lastLogTime = robtimer()
self.logsAllowed = 0
def reportAmps(self, a):
if robtimer() - self.lastAmpsTime >= self.ampsInterval:
ampsD = {
'amperages': {
'leftMotor': a.channel1,
'rightMotor': a.channel2,
'time': a.time
}
}
self.broadcastQ.put(ampsD)
self.lastAmpsTime = robtimer()
self.lastAmps = a
logging.debug('Amperages: %s\n' % (str(a)))
def genericSubscriber(self,msg):
if robtimer() - self.lastLogTime >= self.logInterval:
logging.debug("%s: F: %.2f, BTM: %.2f, L: %.2f, R: %.2f, B: %.2f, DT: %.2fms, T: %.2fms" \
% (time.asctime(),
msg['Ranges']['Forward'],
msg['Ranges']['Bottom'],
msg['Ranges']['Left'],
msg['Ranges']['Right'],
msg['Ranges']['Back'],
msg['Ranges']['Deltat'] / 80000000.0 * 1000.0,
msg['Timestamp'] * 1000.0))
self.lastLogTime = robtimer()
def reportMpu(self, m):
'''
example
{u'temp': 28.4, u'gyro': [0.0, 0.106, 0.0, 1549408911.4707],
u'accel': [-0.0046, -0.0247, 1.106], u'mag': [-10.7895, -10.6512, -31.6055],
u'time': 1549408911.4707, u'heading': 315.0}
'''
if robtimer() - self.lastMpuTime >= self.mpuInterval:
try:
mpuDict = {'MPU': m._asdict()}
self.broadcastQ.put(mpuDict)
self.lastMpuTime = robtimer()
self.lastMpu = m
except KeyError as e:
logging.debug("Error reporting mpu data: %s" % (str(e)))
def __init__(self, angle, qOut, curtime=None, testMode=False):
if not curtime:
self.curtime = robtimer()
else:
self.curtime = curtime
self.targetAngle = angle
self.qOut = qOut
self.observed = False
self.missed = False
self.cumulativeAngle = 0
self.lastAngleSpeed = 0
self.startTime = self.curtime
self.lastTime = self.curtime
self.totalUpdates = 0
self.totalTime = 0
self.speedSum = 0
self.observed = False
self.dLF = open(gyroLogFile, 'w')
self.dLF.write("N\tz\tt\tdeltat\ttotalt\ttotala\n")
self.testMode = testMode
self.sampleFileName = './gyrosample.log'
self.sampleFile = None
logging.debug("Initialized Observer for angle %s" %
(self.targetAngle, ))
print("Initialized Observer for angle %s at %s" % \
(self.targetAngle, time.ctime()))
def __init__(self, heading, qOut, curtime=None, testMode=False):
if not curtime:
self.curtime = robtimer()
else:
self.curtime = curtime
self.achievedTolerance = 3.0 # degrees
self.missedTolerance = 20.0
self.target = heading
self.qOut = qOut
self.lastHeading = -900
self.prevd = -900.
self.direction = 0
self.startTime = self.curtime
self.lastTime = self.curtime
self.totalUpdates = 0
self.totalTime = 0
self.observed = False
self.missed = False
self.dLF = open(headingobserverLogFile,'w')
self.dLF.write("N\ttarget\theading\tlastd\tdir\tt\tdeltat\ttotalt\n")
self.testMode = testMode
logging.debug("Initialized Heading Observer for heading %s" % (self.target,))
print("Initialized Observer for heading %s at %s" % \
(self.target, time.asctime()))
def __init__(self, address, handler, receiveQ, sendQ):
HTTPServer.__init__(self, address, handler)
self.receiveQ = receiveQ
self.sendQ = sendQ
self.currentTelemetry = {
'Ranges': {
'Left': 1,
'Right': 2,
'Forward': 3,
'Back': 4,
'Bottom': 5
}
}
self.newTelemetry = True
self.t0 = robtimer()
self.motors_powered = 0
self.telemetry_sent = 0
self.heartbeat_thread = None
self.heartbeat = False
self.dockSignal_state = {
'time_to_live': 3.0,
'left': 0.0, # timestamp of last left signal
'right': 0.0,
}
self.set_security_mode()
def read(self):
"""
Ask for accelerometer, temperature, gyro, as a burst.
While the accel, temp and gyro registers are in big endian order,
the mag data is in little endian.
Magnetometer reading has been delegated to separate function since
the data isn't ready as often as the others. (The MPU EXT registers
are still read as they would have been in slave mode, but the data
will not be valid. Instead, the mag data is collected directly
from the magnetometer. Leaving this in makes changing back to
slave mode easier, should the need arise.)
"""
if not self.mpu_enabled:
return self.mpusu_zero
if self.read_errors > self.error_limit:
print("Resetting MPU due to excessive read errors")
self.reset()
return self.mpusu_zero
try:
# self.sampleMagnetometer() # only in slave mode
regs = self.bus.read_i2c_block_data(self.mpu, READINGS_START_REG,
READINGS_LEN)
except IOError:
self.read_errors += 1
print("Unexpected MPU read error:", sys.exc_info()[0])
print("\treturning 0 result")
return self.mpusu_zero
# raise
t = robtimer()
lsb = tuple()
# accelerometer
for i in range(0, 6, 2):
lsb += (self.twos_comp(regs[i] << 8 | regs[i + 1]), )
# temperature
lsb += (self.twos_comp(regs[6] << 8 | regs[7]), )
# gyro
for i in range(8, 14, 2):
lsb += (self.twos_comp(regs[i] << 8 | regs[i + 1]), )
lsb += (t, )
mpusu = self.lsb2su(lsb)
self.mpuPub.publish(mpusu)
self.gyroPub.publish(
mpusu.gyro) # publish separately for legacy reasons..
self.lastsu = mpusu
self.numReads += 1
return mpusu
def genericSubscriber(self, msg):
if self.lastLogTime == 0:
logging.debug('\n\n%s: Initiating motion processing logging.' %\
(time.asctime(),))
logging.debug(str(msg))
self.lastLogTime = time.time()
if robtimer() - self.lastLogTime >= self.mpuLogInterval:
logging.debug(str(msg))
self.lastLogTime = time.time()
def reportBat(self, v):
if robtimer() - self.lastVoltageTime >= self.voltageInterval:
if abs(v.mainBattery -
self.lastVoltage.mainBattery) > self.voltageNoise:
bl = self.bat.getLevel(v.mainBattery)
blD = {
'Bat': {
'voltage': bl.voltage,
'level': bl.level,
'source': bl.source
}
}
self.broadcastQ.put(blD)
self.lastVoltageTime = robtimer()
self.lastVoltage = v
logging.debug("Reported Battery Level: %s" % \
(str(self.bat.getLevel(v.mainBattery)),))
logging.debug("v: %s\n" % (str(v), ))
def __init__(self, commandQ=None, broadcastQ=None):
self.commandQ = commandQ
self.broadcastQ = broadcastQ
self.tts = robtts.Robtts()
self.tts.speechPub.addSubscriber(self.genericSubscriber)
self.curtime = robtimer()
#self.minLoopTime = 0.010
ta = time.asctime()
startmsg = "\n\n%s: Starting Speech Operations" % (ta,)
#print startmsg
logging.debug(startmsg)
self.start()
def start(self):
self.curtime = robtimer()
opsStats = {
'totalLoopTime': 0,
'numLoops': 0,
'totalWaitTime': 0,
'numWaits': 0,
'successfulReadings': 0,
'badReadings': 0
}
lastWaitStart = 0
gyroReading = gyro(0, 0, 0, self.curtime)
while True:
loopStartTime = robtimer()
opsStats['numLoops'] += 1
mpuReading = self.mpu.read()
gyroReading = mpuReading.gyro
#logging.debug("%s",(str(mpuReading),))
if gyroReading.z != None:
opsStats['successfulReadings'] += 1
if robtimer(
) - self.lastMpuReportTime > self.mpuReportInterval:
self.broadcastQ.put(mpuReading)
self.lastMpuReportTime = robtimer()
else:
opsStats['badReadings'] += 1
if not self.commandQ.empty():
task = self.commandQ.get_nowait()
logging.debug("%s: mpops task is: %s" %
(time.asctime(), str(task)))
self.commandQ.task_done()
self.execTask(task)
if task == 'Shutdown':
self.processStats(opsStats)
break
elapsedTime = robtimer() - loopStartTime
waitTime = self.minLoopTime - elapsedTime
if waitTime > 0:
opsStats['totalWaitTime'] += waitTime
opsStats['numWaits'] += 1
else:
waitTime = 0
time.sleep(waitTime)
opsStats['totalLoopTime'] += robtimer() - loopStartTime
self.end()
def __init__(self, navdata, qOut, curtime=None, testMode=False):
if not curtime:
self.curtime = robtimer()
else:
self.curtime = curtime
self.maxRange = 768
self.minRange = 17
self.achievedTolerance = 1.0 # centimeters
self.withinTolerance = False
self.navdata = navdata
self.target = navdata.range
self.sensor = navdata.sensor
self.qOut = qOut
self.lastrange = -900
self.startTime = self.curtime
self.lastTime = self.curtime
self.totalUpdates = 0
self.totalTime = 0
self.observed = False
self.missed = False
self.dLF = open(rangeobserverLogFile, 'w')
self.dLF.write("N\ttarget\trange\tdeltat\ttotalt\n")
self.testMode = testMode
trange = self.target
urange = trange
if trange > self.maxRange:
trange = self.maxRange
elif trange < self.minRange:
trange = self.minRange
if trange != urange:
self.target = trange
msg = "Range Observer target clamped to %d, was %d" % (trange,
urange)
print(msg)
logging.info(msg)
logging.debug("Initialized Range Observer for range %s, sensor %s" %
(self.target, self.sensor))
print("Initialized Observer for range %s, sensor %s at %s" % \
(self.target, self.sensor, time.asctime()))
def __init__(self, commandQ=None, broadcastQ=None): # extQ=None):
self.commandQ = commandQ
self.broadcastQ = broadcastQ
# self.extQ = extQ
self.rangemcu = robdrivers.p8x32lbr.P8X32()
self.lastLogTime= 0
self.logInterval= 2.0
self.lastExtSend= 0
self.rangeReportInterval = 0.5
self.lastRangeReportTime = 0.
self.extInterval= 1
self.waitTime = 0.100 # the mb1220 range sensors have a 10Hz read rate
self.observers = []
self.rangemcu.rangePub.addSubscriber(self.genericSubscriber)
self.rangemcu.rangePub.addSubscriber(self.updateObservers)
self.curtime = robtimer()
#self.minLoopTime = 0.010
ta = time.asctime()
startmsg = "\n\n%s: Starting Range Operations" % (ta,)
#print startmsg
logging.debug(startmsg)
self.start()
def __init__(self, commandQ=None, broadcastQ=None):
self.commandQ = commandQ
self.broadcastQ = broadcastQ
self.mpu = robdrivers.mpu9150rpi.MPU9150_A()
self.lastLogTime = 0
self.observers = []
self.mpu.gyroPub.addSubscriber(self.genericSubscriber)
self.mpu.gyroPub.addSubscriber(self.updateObservers)
self.mpu.mpuPub.addSubscriber(self.genericSubscriber)
self.mpu.mpuPub.addSubscriber(self.updateObservers)
self.curtime = robtimer()
self.minLoopTime = 0.010
self.mpuLogInterval = 15.
self.mpuReportInterval = 2.
self.lastMpuReportTime = 0.
ta = time.asctime()
startmsg = "%s: Starting Motion Processing Operations" % (ta, )
# print startmsg
logging.debug(startmsg)
logging.debug("commandQ = %s\nbroadcastQ = %s" %
(str(commandQ), str(broadcastQ)))
self.start()
def start(self):
self.curtime = robtimer()
opsStats = {'totalLoopTime':0, 'numLoops':0,
'successfulReadings':0, 'badReadings':0}
lastWaitStart = 0
while True:
loopStartTime = robtimer()
opsStats['numLoops'] += 1
good, ranges = self.rangemcu.read()
if good:
# print("Range: %d" % ranges['Ranges']['Forward'])
opsStats['successfulReadings'] += 1
if robtimer() - self.lastRangeReportTime > self.rangeReportInterval:
self.broadcastQ.put(ranges)
self.lastRangeReportTime = robtimer()
else:
opsStats['badReadings'] +=1
if not self.commandQ.empty():
task = self.commandQ.get_nowait()
logging.debug("%s: rangeops task is: %s" % (time.asctime(),str(task)))
self.execTask(task)
self.commandQ.task_done()
if task == 'Shutdown':
self.processStats(opsStats)
break
elapsedTime = robtimer() - loopStartTime
time.sleep(self.waitTime)
opsStats['totalLoopTime'] += robtimer() - loopStartTime
self.end()
def rangeSender(self,msg):
if self.extQ:
if robtimer() - self.lastExtSend >= self.extInterval:
print("sending range: %s" % (str(msg),))
self.extQ.put(msg)
self.lastExtSend = robtimer()
def __init__(
self,
commandQ=None,
broadcastQ=None, # todo avoid calling with positional args
rangecq=None,
rangebq=None,
mpucq=None,
mpubq=None):
self.commandQ = commandQ
self.broadcastQ = broadcastQ
self.mpucq = mpucq
self.mpubq = mpubq
#self.mpucq = None
#self.mpubq = None
self.rangecq = rangecq
self.rangebq = rangebq
self.initializeDevices()
self.bat = robdrivers.agmbat.Agmbat()
self.mover = movepa.Movepa(self.devices['motorController'])
self.startTime = robtimer()
self.lastLogTime = 0
self.lastForwardRange = -1
self.forwardRange = -1
self.voltageMonitorThreshold = 10.6
self.alarmInterval = 60
self.rangeInterval = 0.5
self.lastRangeTime = 0
self.lastPowerTime = 0.
self.stopPower = power(0, 0)
self.lastPower = self.stopPower
self.rangeNoise = 1
self.lastVoltageAlarm = robtimer() - self.alarmInterval
self.lastVoltage = voltages(0., 0., 0., 0.)
#self.voltageInterval = 180
self.voltageInterval = 15
#self.voltageInterval = 2
self.voltageNoise = 0.1
#self.voltageNoise = 0
self.lastVoltageTime = robtimer() - self.voltageInterval
self.lastAmpsTime = 0
self.ampsInterval = 1
self.lastAmps = amperages(0, 0, "")
self.rangeRules = opsrules.RangeRules()
self.adjustedTask = power(0., 0.)
self.lastPower = power(0., 0.)
self.autoAdjust = True # False means don't adjust for range
self.lastRanges = {
'Ranges': {
'Forward': 0,
'Left': 0,
'Right': 0,
'Bottom': 0,
'Back': 0,
'Deltat': 0
},
'Timestamp': 0.0
}
self.lastMpu = mpuData(gyro(0, 0, 0, 0), accel(0, 0, 0), mag(0, 0, 0),
0, 0, 0) # todo convert to json
self.curHeading = 0
self.lastMpuTime = 0
self.mpuInterval = 2
logging.info("Instantiated Robot Operations")
if self.commandQ:
self.start()
def start(self):
printRange = True
# minLoopTime = 0.050 # todo look at variablizing minLoopTime to be able to speed up or slow down as needed
minLoopTime = 0.010
opsStats = {
'totalLoopTime': 0,
'numLoops': 0,
'totalWaitTime': 0,
'numWaits': 0,
'excessiveTimes': 0
}
while True:
loopStartTime = robtimer()
if not self.commandQ.empty():
task = self.commandQ.get_nowait()
#print "task is:",task
self.commandQ.task_done() # ensures queue doesn't hang
if task == 'Shutdown':
self.mover.movepa(power(0., 0))
print("Executing Shutdown..")
self.processStats(opsStats)
break
# since Ranges come in json dictionaries instead of named tuples,
# handle separately for now.
if isinstance(task, dict):
if 'Ranges' in task:
self.forwardRange = task['Ranges']['Forward']
self.reportRange(task)
#logging.debug("range = %d" % (self.forwardRange,))
if printRange:
print(("Initial Forward Range = %dcm" %
(self.forwardRange, )))
printRange = False
self.execTask(task)
self.checkController()
dt = robtimer() - loopStartTime
if dt > 1.0:
print("Long operations loop: %f after checking controller" %
(dt, ))
if self.autoAdjust:
self.adjustTask()
elapsedTime = robtimer() - loopStartTime
waitTime = minLoopTime - elapsedTime
opsStats['totalLoopTime'] += elapsedTime
opsStats['numLoops'] += 1
if waitTime > 0:
opsStats['totalWaitTime'] += waitTime
opsStats['numWaits'] += 1
if elapsedTime < minLoopTime and self.commandQ.empty():
time.sleep(waitTime)
if elapsedTime > 1.0:
logging.debug("Excessive operations loop time: %f" %
(elapsedTime))
opsStats['excessiveTimes'] += 1
self.end()
# self.mpu.close()
# for the i2c version of the driver, interpret close as reset mpu
self.bus.write_byte_data(self.mpu, POWER_MGMT_1, 0x80)
self.mpuPub.publish(time.asctime() + " InvenSense MPU Closed")
except:
msg = time.asctime() + " Error Closing Controller"
self.mpuPub.publish(msg)
print(msg)
if __name__ == '__main__':
mpu = MPU9150_A(
) # note: can optionally pass a port name to the constructor
# k = input("Press return to continue")
rl = []
t0 = robtimer()
for n in range(100): # normally 100 for this test
rl.append(mpu.read())
time.sleep(0.01)
t1 = robtimer()
dt = t1 - t0
print("Elapsed time for 100 cycles: %.4f, avg/cycle: %.4f, freq: %.4f" % \
(dt,dt/100.,100./dt))
print("Reads: %d, Avg Unit Conv Time: %fus" % \
(mpu.numReads, mpu.unitConTime/mpu.numReads*1000000))
#for r in rl:
# print r
print(rl[-1])
r = rl[-1]
def lsb2su(self, lsb):
"""
local sensor bus (LSB) to standard units (SI)
lsb format: (accelx,accely,accelz,temp,gyrox,gyroy,gyroz,compx,compy,compz)
conversion formulas from InvenSense UI Data Logger App Note,
MPU9150 Register Map and Product Spec, and
for magnetometer: https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial
"""
#print lsb
t0 = robtimer()
t = lsb[-1]
# accelerometer in g
aR = self.accelRange
aR = 1.0
# print("accel lsb: x=%d, y=%d, z=%d" % (lsb[0], lsb[1], lsb[2]))
a = accel(round(float(lsb[0] / 16384.) * aR, 4),
round(float(lsb[1] / 16384.) * aR, 4),
round(float(lsb[2] / 16384.) * aR, 4))
# temperature in C
temperature = round((lsb[3] / 340. + 35), 1)
# gyro in deg/sec
gR = self.gyroRange
gR = 1
calx = self.gyroCalibration.x
caly = self.gyroCalibration.y
calz = self.gyroCalibration.z
gL = [(float(lsb[4] / 131.) * gR - calx) * X_Convention,
(float(lsb[5] / 131.) * gR - caly) * Y_Convention,
(float(lsb[6] / 131.) * gR - calz) * Z_Convention]
# further squelch noise
for i in range(len(gL)):
if abs(gL[i]) < self.gyroSquelch:
gL[i] = 0.0
g = gyro(round(gL[0], 3), round(gL[1], 3), round(gL[2], 3),
round(t, 4))
# merge in the magentometer results
# magnetometer function already returns mag uT
m = self.zeroMagResult
try:
m = self.readMagnetometer()
except Exception:
self.read_errors += 1
print("Magnetometer exception", sys.exc_info()[0])
'''
# this has to be re-worked since readMagnetometer()
#todo: better error handling for a.y, a.x domains
if a.y <= 1.0:
rollRadians = asin(a.y)
else:
rollRadians = asin(1.0)
if a.x <= 1.0:
pitchRadians = asin(a.x)
else:
pitchRadians = asin(1.0)
# algorithm good for tilt <= 40 degrees, ignore tilt otherwise for now
if abs(rollRadians) > 0.78 or abs(pitchRadians) > 0.78:
m.x = self.magResolution * lsb[8] # appears x-y swapped
m.y = self.magResolution * lsb[7]
else:
cosRoll = cos(rollRadians)
sinRoll = sin(rollRadians)
cosPitch = cos(pitchRadians)
sinPitch = sin(pitchRadians)
magx = self.magResolution * lsb[7] # may need to swap x-y, investigate
magy = self.magResolution * lsb[8]
magz = self.magResolution * lsb[9] * -1 # to align with accelerometer
cx = magx * cosPitch + magz * sinPitch
cy = magx * sinRoll * sinPitch + magy * cosRoll - \
magz * sinRoll * cosPitch
if cy > 0:
heading = 90. - atan(cy/cx) * 180./pi
elif cy < 0:
heading = 270. - atan(cy/cx) * 180./pi
elif cy == 0:
if cx < 0:
heading = 180.
else:
heading = 0.
else:
heading = -1
'''
# This version of the algorithm does not tilt compensate
heading = -1
if m.y == 0:
if m.x <= 0:
heading = 0.
elif m.x > 0:
heading = 180.
else:
rawAngle = atan(m.x / m.y) * 180. / pi
#print rawAngle
if m.y <= 0:
heading = round((270. + rawAngle), 0)
else:
heading = round((90. + rawAngle), 0)
#print 'heading: %.0f' % heading
su = mpuData(g, a, m, round(heading, 2), temperature, round(t, 4))
deltat = robtimer() - t0
self.unitConTime += deltat
return su
def voltageMonitor(self, v): #todo - refactor / create monitors..
if v.mainBattery < self.voltageMonitorThreshold:
if robtimer() - self.lastVoltageAlarm >= self.alarmInterval:
vdictj = {'voltages': v._asdict()}
self.broadcastQ.put(vdictj)
self.lastVoltageAlarm = robtimer()