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)
