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()