def adjustPower(self, reqPower, powerIn, fRange): powerOutLevel = 0.0 if powerIn.level > 0: a = powerIn.angle if (a >= 0 and a <= 45) or (a >= 315 and a <= 360): if fRange <= self.minForwardRange: powerOutLevel = 0.0 else: if fRange >= self.freeForwardRange: powerOutLevel = reqPower.level else: maxLevel = (1.0 - self.minForwardRange / fRange) if powerIn.level <= maxLevel: powerOutLevel = powerIn.level else: powerOutLevel = maxLevel if powerOutLevel < self.minPowerLevel: powerOutLevel = self.minPowerLevel else: powerOutLevel = powerIn.level self.lastForwardRange = fRange return power(powerOutLevel, powerIn.angle)
def execTask(self, task): logging.debug("executing mpops task: " + str(task)) if isinstance(task, observeTurn): if task.angle == -1: # this shutdown method is obsolete self.commandQ.put('Shutdown') else: turnObserver = self.addObserver(task.angle, self.broadcastQ) if isinstance(task, observeHeading): headingObserver = self.addHeadingObserver(task.heading, self.broadcastQ) if type(task) is calibrateMagnetometer: try: self.mpu.calibrateMag(task.samples, task.source) except Exception as e: print(f"Error calibrating magnetometer\n{str(e)}") finally: self.broadcastQ.put(power(0., 0))
motorController = robdrivers.sdc2130.SDC2130() mover = movepa.Movepa(motorController) motorController.messagePub.addSubscriber(genericSubscriber) motorController.voltagePub.addSubscriber(genericSubscriber) motorController.voltagePub.addSubscriber(genericSubscriber) motorController.ampsPub.addSubscriber(genericSubscriber) #motorController.motorControlPub.addSubscriber(genericSubscriber) cq = multiprocessing.JoinableQueue() bq = multiprocessing.JoinableQueue() g = threading.Thread(target=MPservice, args=(cq, bq)) g.start() print("Give mpu time for gyro calibration") time.sleep(2) print("sending turn observer request") cq.put(observeTurn(90)) while True: if not bq.empty(): m = bq.get() print(str(m)) break else: mover.movepa(power(0.2, 90)) time.sleep(0.100) mover.movepa(power(0., 0.)) cq.put('Shutdown') motorController.closeController()
def prepare(self, cmd): preparedCommand = cmd if cmd == 'S' or cmd == 's': preparedCommand = power(0, 0) return preparedCommand if cmd == 'Shutdown': return preparedCommand logging.debug("type of cmd '%s' is: %s" % (str(cmd), type(cmd))) if type(cmd) is str and cmd[0] != '/': cmdWords = cmd.split(' ') if len(cmdWords) == 1 and cmdWords[0].lower() == 'stop': preparedCommand = power(0, 0) return preparedCommand if len(cmdWords) == 2 and cmdWords[0].lower() == 'stop': cw1 = cmdWords[1] if cw1 in self.extProcesses.keys(): p = self.extProcesses[cw1] p.terminate() termResult = p.wait(2) self.extProcesses.pop(cw1) print("\tStopped {}, result {}".format(cw1, termResult)) logging.debug("Stopped {}, result {}".format( cw1, termResult)) else: print("\tUnknown stop {}".format(cw1)) logging.debug("Unknown stop {}".format(cw1)) return None if type(cmd) is str and len(cmd) > 3: preparedCommand = None try: rawfields = cmd.split('/') fields = [f for f in rawfields if f != ''] command = fields[0] if command in command_map: params = [] mapentry = command_map[command] cmdentry = mapentry[len(fields)] cmdtype = list(cmdentry.keys())[0] i = 1 # handle nav as a special case for now since it nests power # suggest generalizing to avoid adding further special cases if cmdtype is nav: params.append( power(float(fields[i]), float(fields[i + 1]))) i += 2 for t in cmdentry[cmdtype]: if t is power: continue params.append(t(fields[i])) i += 1 preparedCommand = cmdtype(*params) else: print(f"Unknown command: {command}") # print(f"Test Prepared command: {str(preparedCommand)}") except ValueError as e: print(f"Value error preparing command: {str(cmd)}\n{str(e)}") except KeyError as e: print( f"Key error preparing command: {str(cmd)}\n{str(e)} - invalid parameter count" ) except TypeError as e: print(f"Type error preparing command: {str(cmd)}\n{str(e)}") except Exception as e: print( f"Unexpected error preparing command: {str(cmd)}\n{str(e)}" ) ''' # Leaving this old block in for now while generalized version above is in testing if cmd[0:3] == '/r/': values = cmd[3:].split('/') if len(values) == 2: preparedCommand = power(float(values[0]), float(values[1])) elif len(values) == 5: #('nav', 'power range sensor interval') level = float(values[0]) angle = float(values[1]) range = float(values[2]) sensor = values[3] interval = float(values[4]) preparedCommand = nav(power(level, angle), range, sensor, interval) else: print("Error parsing power command: {}".format(cmd)) logging.error("Error parsing power command: {}".format(cmd)) elif cmd[0:3] == '/a/': angle = cmd[3:] preparedCommand = observeTurn(float(angle)) elif cmd[0:3] == '/t/': angle = cmd[3:] preparedCommand = executeTurn(float(angle)) elif cmd[0:3] == '/h/': heading = cmd[3:] preparedCommand = executeHeading(float(heading)) elif cmd[0:3] == '/s/': msg = cmd[3:] preparedCommand = speech(str(msg)) elif cmd[0:3] == '/d/': song = cmd[3:] preparedCommand = dance(song) elif cmd[0:3] == '/m/': fields = cmd[3:].split('/') samples = int(fields[0]) if samples > 1000: print("Limiting calibration samples to 1000 max") samples = 1000 if len(fields) == 2: source = str(fields[1]) else: source = None preparedCommand = calibrateMagnetometer(samples, source) ''' else: preparedCommand = feedback(cmd) #print "feedback: %s" % (str(cmd),) return preparedCommand
if self.missed or self.withinTolerance: self.dLF.close() return if __name__ == '__main__': import sys sys.path.append('../robdrivers') import p8x32lbr mpu = p8x32lbr.P8X32() mpu.flush() good, readings = mpu.read() q_out = queue.Queue() navdata = nav(power(0, 0), 20, 'Back', 0) o = RangeObserver(qOut=q_out, navdata=navdata, testMode=False) mpu.rangePub.addSubscriber(o.update) timeout = 30. t1 = time.time() while True: reading = mpu.read() if not q_out.empty(): m = q_out.get() print(str(m)) break if time.time() - t1 > timeout: print("timed out")
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()
def intervalStopCallback(self, task): msg = "Stopping motors after interval %.3f" % task.interval print(msg) logging.info(msg) self.commandQ.put(power(0, 0))
def execTask(self, task): logging.debug("executing ops task: " + str(task)) if printTests: print("executing task: " + str(task)) if type(task) is power: self.lastPower = task self.requestedPower = task result = "no move result" if self.autoAdjust: self.adjustedTask = self.rangeRules.adjustPower( self.requestedPower, self.lastPower, self.forwardRange) if printTests: print(("adjusted task: %s" % str(self.adjustedTask))) result = self.mover.movepa(self.adjustedTask) else: result = self.mover.movepa(task) logging.debug(str(result)) #print str(result) if type(task) is nav: # nav: power, range, interval self.commandQ.put(task.power) # todo handle the case of having both range and duration wrt to priorities and cross management if task.range != 0: self.rangecq.put(observeRange(task)) if task.interval != 0: i = threading.Timer(task.interval, self.intervalStopCallback, kwargs={'task': task}) i.start() if type(task) is mpuData: self.mpuData = task self.curHeading = task.heading self.reportMpu(task) if type(task) is observeTurn: self.mpucq.put(task) if type(task) is executeTurn: mpuTask = observeTurn(task.angle) self.mpucq.put(mpuTask) if task.angle >= 0: # result = self.mover.movepa(power(0.25,90.0)) self.commandQ.put(power(0.20, 90.0)) else: # result = self.mover.movepa(power(0.25,270.0)) self.commandQ.put(power(0.20, 270.0)) if type(task) is executeHeading: if task.heading >= 0. and task.heading <= 360.: mpuTask = observeHeading(task.heading) self.mpucq.put(mpuTask) pa = calcDirection(self.curHeading, task.heading)[0] self.commandQ.put(power(0.20, pa)) logging.debug("Heading Exectuion Ordered: %s, Power Angle %d" %\ (str(task), pa)) else: logging.debug('Invalid heading: ' + str(task)) if type(task) is calibrateMagnetometer: logging.info("Executing magnegtometer calibration") if task.source is None or task.source == '-': self.commandQ.put(power(0.2, 90)) self.mpucq.put(task) # dancing not supported in this version # for the moment, make dancing an operation. It should be an application. # if type(task) is dance: # logging.debug("calling dance") # result = robapps.danceapp.DanceApp(dance, self.mover) # logging.debug(str(result)) if type(task) is tuple and len( task) == 3: # todo need to make an observation type if task[0] == 'Observed': self.commandQ.put(power(0, 0)) logging.debug("Stopped motors on completed observation") if task[0] == 'Missed': self.commandQ.put(power(0, 0)) logging.debug("Stopped motors on missed observation")
name='Manager', args=( cq, bq, #mpucq,cq, # rangecq,cq) )) mgrThread.start() time.sleep(0.5) rangeP.start() time.sleep(0.5) mpuP.start() time.sleep(1) p = power(.25, 90) cq.put(p) # cq.put(executeTurn(90)) time.sleep(5) cq.put('Shutdown') #p = power(.50,0) #cq.put(p) #time.sleep(8) # time to run unless range-oriented stop #cq.put(power(0,0)) #time.sleep(3) #rangecq.put("Shutdown") #mpucq.put("Shutdown")
powerOutLevel = 0.0 else: if fRange >= self.freeForwardRange: powerOutLevel = reqPower.level else: maxLevel = (1.0 - self.minForwardRange / fRange) if powerIn.level <= maxLevel: powerOutLevel = powerIn.level else: powerOutLevel = maxLevel if powerOutLevel < self.minPowerLevel: powerOutLevel = self.minPowerLevel else: powerOutLevel = powerIn.level self.lastForwardRange = fRange return power(powerOutLevel, powerIn.angle) if __name__ == '__main__': rule = RangeRules() r = 50.0 print("for Power .75, ranges and adjusted power:") p = power(0.75, 0) for r in range(150, 1, -2): newPower = rule.adjustPower(p, r) print(r, newPower.level)