def __init__(self, config):
VacuumInterface.__init__(self)
self.logr = LagerLogger("")
self.logr.console(LagerLogger.DEBUG)
self._dev_path = config['robot']['robot_dev']
self.robot = AdvancedRoomba(config)
self.robot.start(self._dev_path, 115200)
self.robot.passive()
self._telemetry = TelemetryData()
# Keep Alive Code
#setup GPIO to reference pins based the board
GPIO.setmode(GPIO.BOARD)
#device_detect pin is the pin we set low to turn on the robot, we must
#set it to out mode for rasing high and low
GPIO.setup(DEVICE_DETECT, GPIO.OUT)
#keep alive thread
self._keep_alive_timer = Timer(1, self._keep_alive)
self._keep_alive_timer.start()
# Dust-bin detection
gpio_sensor(DUSTBIN, self.gpio_dustbin_cb)
# Callback functions
self._callbacks = dict() # name=(cmp_fun, cb)
self._sensor_update_timer = Timer(.1, self.poll_sensors_cb)
self._sensor_update_timer.start()
def __init__(self, config):
VacuumInterface.__init__(self)
self.logr = LagerLogger("")
self.logr.console(LagerLogger.DEBUG)
self._dev_path = config['robot']['robot_dev']
self.robot = NeatoDriver(self._dev_path)
self.robot.passive()
self._callbacks = dict()
self.reg_sensor_cb(
"buttons", self._buttons_cb,
lambda old, new: not set([k for k, v in old.items() if v]) == set(
[k for k, v in new.items() if v]))
self._telemetry = TelemetryData()
self._sensor_update_timer = Timer(.001, self.poll_sensors_cb)
self._sensor_update_timer.start()
def __init__(self, config):
VacuumInterface.__init__(self)
self.config = config
self._callbacks = dict()
self._telemetry = TelemetryData()
#setup GPIO to reference pins based the board
GPIO.setmode(GPIO.BOARD)
gpio_sensor(DROPWHEEL, self.gpio_wheel_cb)
gpio_sensor(BASE, self.gpio_dock_cb)
gpio_sensor(POWER, self.gpio_powerbtn_cb)
gpio_sensor(DUSTBIN, self.gpio_dustbin_cb)
gpio_sensor(SPOT, self.gpio_spot_cb)
gpio_sensor(CLEAN, self.gpio_clean_cb)
gpio_sensor(MAX, self.gpio_max_cb)
class NeatoInterface(VacuumInterface):
def __init__(self, config):
VacuumInterface.__init__(self)
self.logr = LagerLogger("")
self.logr.console(LagerLogger.DEBUG)
self._dev_path = config['robot']['robot_dev']
self.robot = NeatoDriver(self._dev_path)
self.robot.passive()
self._callbacks = dict()
self.reg_sensor_cb(
"buttons", self._buttons_cb,
lambda old, new: not set([k for k, v in old.items() if v]) == set(
[k for k, v in new.items() if v]))
self._telemetry = TelemetryData()
self._sensor_update_timer = Timer(.001, self.poll_sensors_cb)
self._sensor_update_timer.start()
def reg_sensor_cb(self, sensor_name, cb, cmp_fun=None):
if sensor_name not in self._callbacks.keys():
self._callbacks[sensor_name] = list()
if cmp_fun is None:
cmp_fun = lambda old, new: not old.__eq__(new)
if (cmp_fun, cb) in self._callbacks[sensor_name]:
self.logr.warn("THIS HAS ALREADY BEEN DONE!?")
self._callbacks[sensor_name] = (cmp_fun, cb)
def poll_sensors_cb(self):
""" Poll sensors from hardware and do callbacks"""
old_sensors = dict()
for sensor_name in self._callbacks.keys():
try:
old_sensors[sensor_name] = self.robot.get_sensor(sensor_name)
except:
old_sensors[sensor_name] = None
try:
self.robot.update_sensors()
except Exception as e:
self.logr.error(e)
return
# Update telemetry
self._update_telemetry()
# Do callbacks
for sensor_name, (cmp_fun, cb) in self._callbacks.items():
new_sensor_val = self.robot.get_sensor(sensor_name)
if old_sensors[sensor_name] is None or cmp_fun(
old_sensors[sensor_name], new_sensor_val):
cb()
def get_sensor(self, key):
return self.robot.get_sensor(key)
def passive(self):
self.robot.passive()
def is_docked(self):
#return bool(self.robot.get_sensor("ExtPwrPresent")) and not\
# bool(self.robot.get_sensor("SNSR_DC_JACK_CONNECT"))
voltage = self.robot.get_sensor("VExtV")
return (voltage > 20 and voltage < 23.5)
def is_charging(self):
#return bool(self.robot.get_sensor("ExtPwrPresent"))
return (self.robot.get_sensor("VExtV") > 20)
def is_cleaning(self):
return False
def dustbin_in(self):
return bool(self.robot.get_sensor("SNSR_DUSTBIN_IS_IN"))
def _update_telemetry(self):
charging = self.is_charging()
if not charging == self._telemetry["charging"]:
self._telemetry["charging"] = charging
# See if we are on the dock
# sources = self.robot.get_sensor("charging_sources_available")
docked = self.is_docked()
if not docked == self._telemetry["docked"]:
self._telemetry["docked"] = docked
# see if the robot has been picked up
wheel_drops = self.robot.get_sensor("bumps_wheeldrops")
lifted = (wheel_drops["wheeldrop_left"]
or wheel_drops["wheeldrop_right"])
if not lifted == self._telemetry["lifted"]:
self._telemetry["lifted"] = lifted
dustbin = self.dustbin_in()
if not dustbin == self._telemetry["dustbin"]:
self._telemetry["dustbin"] = dustbin
# Check if the sweeper brush is running (that means we are cleaning)
cleaning = False
if not cleaning == self._telemetry["cleaning"]:
self._telemetry["cleaning"] = cleaning
# Check the status of button presses
buttons = self.robot.get_sensor("buttons")
buttons = [key for key, pressed in buttons.items() if pressed]
if not set(self._telemetry["buttons"]) == set(buttons):
self._telemetry["buttons"] = buttons
# Check the robot voltage
# voltage = self.robot.get_sensor("BatteryVoltageInmV")
voltage = self.robot.get_sensor("VBattV")
if voltage is None:
voltage = 0
#voltage /= 1000
if abs(voltage - self._telemetry["battery_voltage"]) >= .2: #40.0 :
self._telemetry["battery_voltage"] = voltage
def set_telemetry_cb(self, fun):
self._telemetry.set_telemetry_update_cb(fun)
def terminate(self):
self._sensor_update_timer.cancel()
time.sleep(.5)
self.robot.close()
self.logr.info("Clean shutdown achieved - cheers!")
def _buttons_cb(self):
# Save the last button or buttons to be pressed.
# Note: This gets called when buttons are released too, so make
# sure something is actually pressed we didn't just release
buttons = self.robot.get_sensor("buttons")
new_buttons = [k for k, v in buttons.items() if v]
if new_buttons:
self._lastbuttons = new_buttons
class DiscoveryInterface(VacuumInterface):
def __init__(self, config):
VacuumInterface.__init__(self)
self.config = config
self._callbacks = dict()
self._telemetry = TelemetryData()
#setup GPIO to reference pins based the board
GPIO.setmode(GPIO.BOARD)
gpio_sensor(DROPWHEEL, self.gpio_wheel_cb)
gpio_sensor(BASE, self.gpio_dock_cb)
gpio_sensor(POWER, self.gpio_powerbtn_cb)
gpio_sensor(DUSTBIN, self.gpio_dustbin_cb)
gpio_sensor(SPOT, self.gpio_spot_cb)
gpio_sensor(CLEAN, self.gpio_clean_cb)
gpio_sensor(MAX, self.gpio_max_cb)
def set_telemetry_cb(self, fun):
self._telemetry.set_telemetry_update_cb(fun)
def gpio_wheel_cb(self, value):
value = not value
if not value == self._telemetry["lifted"]:
self._telemetry["lifted"] = value
logr.debug("Wheel= %s" % str(value)) #(not bool(GPIO.input(BASE)))
sys.stdout.flush()
def gpio_dock_cb(self, value):
if not value == self._telemetry["docked"]:
self._telemetry["docked"] = value
logr.debug("Dock= %s" % str(value)) #(not bool(GPIO.input(BASE)))
sys.stdout.flush()
def gpio_dustbin_cb(self, value):
if not value == self._telemetry["dustbin"]:
self._telemetry["dustbin"] = value
logr.debug("dustbin= %s" % str(value))
sys.stdout.flush()
def gpio_powerbtn_cb(self, value):
self.gpio_button_cb("power", value)
def gpio_spot_cb(self, pressed):
self.gpio_button_cb("spot", pressed)
def gpio_clean_cb(self, value):
self.gpio_button_cb("clean", value)
def gpio_max_cb(self, value):
self.gpio_button_cb("max", value)
def gpio_button_cb(self, name, pressed):
buttons = self._telemetry["buttons"]
buttons = buttons if buttons else list(
) # if buttons=None, make it a list
if pressed and name not in buttons:
buttons.append(name)
if (not pressed) and name in buttons:
buttons.remove(name)
self._telemetry["buttons"] = buttons
logr.debug("%s button=%s" % (name, str(pressed)))
def reg_sensor_cb(self, sensor_name, cb, cmp_fun=None):
if sensor_name not in self._callbacks.keys():
self._callbacks[sensor_name] = list()
if cmp_fun is None:
cmp_fun = lambda old, new: not old.__eq__(new)
if (cmp_fun, cb) in self._callbacks[sensor_name]:
logr.warn("THIS HAS ALREADY BEEN DONE!?")
self._callbacks[sensor_name] = (cmp_fun, cb)
def is_docked(self):
return self._dock
def is_charging(self):
return False
def is_cleaning(self):
return False
def terminate(self):
GPIO.cleanup()
logr.info("Clean shutdown achieved - cheers!")
class BenderInterface(VacuumInterface):
def __init__(self, config):
VacuumInterface.__init__(self)
self.logr = LagerLogger("")
self.logr.console(LagerLogger.DEBUG)
self._dev_path = config['robot']['robot_dev']
self.robot = AdvancedRoomba(config)
self.robot.start(self._dev_path, 115200)
self.robot.passive()
self._telemetry = TelemetryData()
# Keep Alive Code
#setup GPIO to reference pins based the board
GPIO.setmode(GPIO.BOARD)
#device_detect pin is the pin we set low to turn on the robot, we must
#set it to out mode for rasing high and low
GPIO.setup(DEVICE_DETECT, GPIO.OUT)
#keep alive thread
self._keep_alive_timer = Timer(1, self._keep_alive)
self._keep_alive_timer.start()
# Dust-bin detection
gpio_sensor(DUSTBIN, self.gpio_dustbin_cb)
# Callback functions
self._callbacks = dict() # name=(cmp_fun, cb)
self._sensor_update_timer = Timer(.1, self.poll_sensors_cb)
self._sensor_update_timer.start()
def reg_sensor_cb(self, sensor_name, cb, cmp_fun=None):
if sensor_name not in self._callbacks.keys():
self._callbacks[sensor_name] = list()
if cmp_fun is None:
cmp_fun = lambda old, new: not old.__eq__(new)
if (cmp_fun, cb) in self._callbacks[sensor_name]:
self.logr.warn("THIS HAS ALREADY BEEN DONE!?")
self._callbacks[sensor_name] = (cmp_fun, cb)
def gpio_dustbin_cb(self, value):
""" update the dustbin status """
if not value == self._telemetry["dustbin"]:
self._telemetry["dustbin"] = value
def get_sensor(self, key):
return self.robot.get_sensor(key)
def passive(self):
self.robot.passive()
def is_docked(self):
return self._telemetry["docked"]
def is_charging(self):
return self._telemetry["charging"]
def is_cleaning(self):
return self._telemetry["cleaning"]
def dock(self):
self.robot.dock()
def clean(self):
self.robot.clean()
def pause(self):
self.robot.pause()
def _keep_alive(self):
""" Keep alive timer callback. Throws a gpio pin up and down """
GPIO.output(DEVICE_DETECT, GPIO.LOW)
time.sleep(0.1)
GPIO.output(DEVICE_DETECT, GPIO.HIGH)
def poll_sensors_cb(self):
""" Poll sensors from hardware and do callbacks"""
old_sensors = dict()
for sensor_name in self._callbacks.keys():
try:
old_sensors[sensor_name] = self.robot.get_sensor(sensor_name)
except:
old_sensors[sensor_name] = None
try:
self.robot.update_sensors()
except BadDataLengthError as e:
self.logr.error(e)
# print "Restarting SCI into control() mode"
# self.robot.control()
return
except DriverError as e:
self.logr.warn(e)
self._telemetry["sci_error"] = True
except Exception as e:
self.logri.error(e)
# print "Restarting SCI into control() mode"
# self.robot.control()
# WHEN THIS HAPPENS - the robot seems to have turned off.
# On dirtdog this would happen when the person presses
# the power button while cleaning.
return
# Update telemetry
self._update_telemetry()
# Do callbacks
for sensor_name, (cmp_fun, cb) in self._callbacks.items():
new_sensor_val = self.robot.get_sensor(sensor_name)
if old_sensors[sensor_name] is None or cmp_fun(
old_sensors[sensor_name], new_sensor_val):
cb()
def _update_telemetry(self):
# If sci error has been set (by poll_sensors_cb), undo it
if self._telemetry["sci_error"]:
self._telemetry["sci_error"] = False
# Compare charging status
charging = self.robot.get_sensor("charging_state")
charging = False if charging in ["not_charging", "error"] else True
if not charging == self._telemetry["charging"]:
self._telemetry["charging"] = charging
# See if we are on the dock
sources = self.robot.get_sensor("charging_sources_available")
if not sources["base"] == self._telemetry["docked"]:
self._telemetry["docked"] = sources["base"]
# see if the robot has been picked up
wheel_drops = self.robot.get_sensor("bumps_wheeldrops")
lifted = (wheel_drops["wheeldrop_left"]
or wheel_drops["wheeldrop_right"])
if not lifted == self._telemetry["lifted"]:
self._telemetry["lifted"] = lifted
# Check if the sweeper brush is running (that means we are cleaning)
cleaning = self.robot.get_sensor("main_brush_current") >= 1
if not cleaning == self._telemetry["cleaning"]:
self._telemetry["cleaning"] = cleaning
# Check the status of button presses
buttons = self.robot.get_sensor("buttons")
buttons = [key for key, pressed in buttons.items() if pressed]
if not set(self._telemetry["buttons"]) == set(buttons):
self._telemetry["buttons"] = buttons
# Check the robot voltage
voltage = self.robot.get_sensor("voltage") / 1000.0
if abs(voltage - self._telemetry["battery_voltage"]) >= 0.1:
self._telemetry["battery_voltage"] = voltage
def set_telemetry_cb(self, fun):
self._telemetry.set_telemetry_update_cb(fun)
def terminate(self):
self._keep_alive_timer.cancel()
self.logr.info("keep alive killed")
self._sensor_update_timer.cancel()
self.logr.info("killed sensor update timer")
time.sleep(1)
GPIO.cleanup()
self.robot.close()
self.logr.info("Clean shutdown achieved - cheers!")
def __init__(self, config):
VacuumInterface.__init__(self)
self.logr = LagerLogger("")
self._vacuum_state = VacuumState()
self._vacuum_state.set_idle()
self._spinner = Spinner()
self.logr.console(LagerLogger.DEBUG)
self._dev_path = config['robot']['robot_dev']
# Tracks button presses to prevent API calls from slipping out
self._button_tracker = ButtonTracker()
# Keep Alive Code
#setup GPIO to reference pins based the board
GPIO.setmode(GPIO.BOARD)
#device_detect pin is the pin we set low to turn on the robot, we must
#set it to out mode for rasing high and low
GPIO.setup(DEVICE_DETECT, GPIO.OUT)
self._keep_alive()
self.robot = AdvancedRoomba(config)
self.robot.start(self._dev_path, 115200)
self.robot.passive()
self._telemetry = TelemetryData()
self._button_tracker = ButtonTracker()
self._api_button_clean = APIButtonPress("clean", self._button_tracker)
self._api_button_dock = APIButtonPress("dock", self._button_tracker)
#keep alive thread
self._keep_alive_timer = Timer(60, self._keep_alive)
self._keep_alive_timer.start()
# Dust-bin detection
gpio_sensor(DUSTBIN, self.gpio_dustbin_cb)
self._dustbin_user_cb_funs = list()
# Callback functions
self._callbacks = dict() # name=(cmp_fun, cb)
self._cleaning_user_cb_funs = list()
# Prevent Baud change
# self._clean_button_safety = CleanButtonSafety(self)
# Use button presses to track vacuum state
self._button_tracker.reg_update_callback(self._hw_buttons_cb)
self.reg_cleaning_cb(self._sweeper_cb)
# Detect Docking
self.reg_sensor_cb("charging_sources_available", self._docked_cb,
lambda old, new: (not old["base"] == new["base"]))
# Detect Lifts
self.reg_sensor_cb(
"bumps_wheeldrops", self._lifted_cb, lambda old, new: any(
[((not old[x]) and new[x])
for x in ["wheeldrop_right", "wheeldrop_left"]]))
self.reg_sensor_cb(
"bumps_wheeldrops", self._dropped_cb, lambda old, new: any(
[((not new[x]) and old[x])
for x in ["wheeldrop_right", "wheeldrop_left"]]))
self._sensor_update_timer = Timer(.1, self.poll_sensors_cb)
self._sensor_update_timer.start()
class Roomba500Interface(VacuumInterface):
def __init__(self, config):
VacuumInterface.__init__(self)
self.logr = LagerLogger("")
self._vacuum_state = VacuumState()
self._vacuum_state.set_idle()
self._spinner = Spinner()
self.logr.console(LagerLogger.DEBUG)
self._dev_path = config['robot']['robot_dev']
# Tracks button presses to prevent API calls from slipping out
self._button_tracker = ButtonTracker()
# Keep Alive Code
#setup GPIO to reference pins based the board
GPIO.setmode(GPIO.BOARD)
#device_detect pin is the pin we set low to turn on the robot, we must
#set it to out mode for rasing high and low
GPIO.setup(DEVICE_DETECT, GPIO.OUT)
self._keep_alive()
self.robot = AdvancedRoomba(config)
self.robot.start(self._dev_path, 115200)
self.robot.passive()
self._telemetry = TelemetryData()
self._button_tracker = ButtonTracker()
self._api_button_clean = APIButtonPress("clean", self._button_tracker)
self._api_button_dock = APIButtonPress("dock", self._button_tracker)
#keep alive thread
self._keep_alive_timer = Timer(60, self._keep_alive)
self._keep_alive_timer.start()
# Dust-bin detection
gpio_sensor(DUSTBIN, self.gpio_dustbin_cb)
self._dustbin_user_cb_funs = list()
# Callback functions
self._callbacks = dict() # name=(cmp_fun, cb)
self._cleaning_user_cb_funs = list()
# Prevent Baud change
# self._clean_button_safety = CleanButtonSafety(self)
# Use button presses to track vacuum state
self._button_tracker.reg_update_callback(self._hw_buttons_cb)
self.reg_cleaning_cb(self._sweeper_cb)
# Detect Docking
self.reg_sensor_cb("charging_sources_available", self._docked_cb,
lambda old, new: (not old["base"] == new["base"]))
# Detect Lifts
self.reg_sensor_cb(
"bumps_wheeldrops", self._lifted_cb, lambda old, new: any(
[((not old[x]) and new[x])
for x in ["wheeldrop_right", "wheeldrop_left"]]))
self.reg_sensor_cb(
"bumps_wheeldrops", self._dropped_cb, lambda old, new: any(
[((not new[x]) and old[x])
for x in ["wheeldrop_right", "wheeldrop_left"]]))
self._sensor_update_timer = Timer(.1, self.poll_sensors_cb)
self._sensor_update_timer.start()
def reg_vacuum_state_cb(self, cb):
""" register a callback for when the vacuum state changes """
self._vacuum_state.callbacks.append(cb)
def reg_buttons_cb(self, cb):
self._button_tracker.reg_update_callback(cb)
def reg_sensor_cb(self, sensor_name, cb, cmp_fun=None):
if sensor_name == "buttons":
self.logr.error(
"cannot log buttons using this callback - use dedicated reg_button_cb!"
)
return
if sensor_name not in self._callbacks.keys():
self._callbacks[sensor_name] = list()
if cmp_fun is None:
cmp_fun = lambda old, new: not old.__eq__(new)
if (cmp_fun, cb) in self._callbacks[sensor_name]:
self.logr.warn("THIS HAS ALREADY BEEN DONE!?")
self._callbacks[sensor_name].append((cmp_fun, cb))
def reg_dustbin_cb(self, cb):
self._dustbin_user_cb_funs.append(cb)
def reg_cleaning_cb(self, cb):
"""" callbacks need to take an argument """
self._cleaning_user_cb_funs.append(cb)
def _docked_cb(self):
base = self.robot.get_sensor("charging_sources_available")["base"]
if base:
self._vacuum_state.set_docked()
def _lifted_cb(self):
""" gets called each time a wheel leaves the ground """
self.logr.debug("Lifted!")
if not self._vacuum_state.is_idle():
self._vacuum_state.set_idle()
def _dropped_cb(self):
""" gets called every time a wheel is set back down """
self.logr.debug("droped!")
if not self._vacuum_state.is_idle():
self._vacuum_state.set_idle()
def baud_fix(self):
# self.robot.sci = SerialCommandInterface(self._dev_path, 19200)
with self.robot.sci.lock:
self.logr.warn("baud_fix()!")
self._keep_alive()
time.sleep(0.20)
self.robot.sci.ser.baudrate = 19200
self.robot.sci.start()
time.sleep(0.20)
try:
self.robot.change_baud_rate(115200)
except Exception as e:
self.logr.error("change_baud_rate %s: %s" % (str(type(e)), e))
try:
self.robot.sci.start()
except Exception as e:
self.logr.error("start %s: %s" % (str(type(e)), e))
time.sleep(.15)
self.logr.warn("baud_fix() done.")
sys.stdout.flush()
def _hw_buttons_cb(self, values):
""" called whenever a person presses one of the hardware buttons on the robot """
buttons = [k for k, v in values.items() if v]
# self._clean_button_safety.set_value(True if "clean" in buttons else False)
# Dont do anything if we are just lifting up off a button
# This isn't quite right, but is probably good enough
if not buttons:
return
self.logr.debug("HW Button Pressed: %s" % str(buttons))
oi_mode = self.get_sensor("oi_mode")
if not oi_mode == "passive":
self.logr.debug("Not setting vacuum_state in control mode '%s'" %
oi_mode)
return
if self._telemetry["lifted"]:
self.logr.debug("Ignoring buttons because lifted")
return
if len(buttons) > 1:
self.logr.error(
"Heard too many button states at once %s, auto cancelling." %
str(buttons))
self.robot.pause()
if self._vacuum_state.is_idle() or self._vacuum_state.is_docked():
if "clean" in buttons:
self._vacuum_state.set_cleaning()
elif "spot" in buttons:
self._vacuum_state.set_cleaning()
# [db] we don't seperately track spot cleaning anymore
# self._vacuum_state.set_spot_cleaning()
elif "dock" in buttons:
self._vacuum_state.set_docking()
# [db] this happens if "day" or "hour" gets pressed...
else:
self.logr.warn("Unhandled last button: %s" % str(buttons))
else:
if self.is_docked():
self._vacuum_state.set_docked()
else:
self._vacuum_state.set_idle()
def _sweeper_cb(self, cleaning):
""" gets called when the sweeper either starts or stops """
self.logr.debug("sweeper_cb(%s)" % str(cleaning))
if not self._vacuum_state:
self.logr.warn(
"Sweeper changed state, but vacuum_state is unknown")
return
vacuum_state = self._vacuum_state.get()
if cleaning and (vacuum_state
not in ["cleaning", "spot_cleaning", "docking"]):
self.logr.error(
"Sweeper started, but was in state '%s'. Assuming Cleaning." %
vacuum_state)
self._vacuum_state.set_cleaning()
elif not cleaning and (vacuum_state
not in ["idle", "docking", "docked"]):
if self._telemetry["docked"]:
self._vacuum_state.set_docked()
else:
self._vacuum_state.set_idle()
def gpio_dustbin_cb(self, value):
""" update the dustbin status """
if not value == self._telemetry["dustbin"]:
self._telemetry["dustbin"] = value
if self._dustbin_user_cb_funs:
for cb in self._dustbin_user_cb_funs:
cb(value)
def get_sensor(self, key):
if key == "buttons":
return self._button_tracker.get_buttons()
return self.robot.get_sensor(key)
def passive(self):
# This gets called when the controller boots up, and just before the clean cmd
# Do NOT set the vacuum state explicitly here, because it should get overwritten
# moments later and will just cause problems. If you want vacuum_state set, use
# pause() instead.
self.logr.info("passive mode")
self.robot.passive()
def control(self):
self.keep_alive()
self.logr.info("control mode")
self.robot.control()
def is_docking(self):
return self._vacuum_state.is_docking()
def is_docked(self):
return self._vacuum_state.is_docked()
# def is_on_dock(self):
# return self._telemetry["docked"]
def is_lifted(self):
drops = self.robot.get_sensor("bumps_wheeldrops")
return (drops["wheeldrop_right"] or drops["wheeldrop_left"])
def is_charging(self):
return self._telemetry["charging"]
def is_cleaning(self):
return self._vacuum_state.is_cleaning(
) or self._vacuum_state.is_spot_cleaning()
def dock(self):
""" Sends robot to the dock. Send twice if cleaning, once if not cleaning"""
if self.is_cleaning():
self.logr.info("dock() {while cleaning}")
self._vacuum_state.set_docking()
self.robot.pause()
time.sleep(0.5)
else:
self.logr.info("dock() {while paused}")
self._api_button_dock(self.robot.dock)
self._vacuum_state.set_docking()
def clean(self, no_state_change=None):
self.logr.info("clean()")
if (self._vacuum_state.is_cleaning()
or self._vacuum_state.is_spot_cleaning()
) and no_state_change is None:
self.logr.error("Already cleaning. Ignoring command")
return
self._api_button_clean(self.robot.clean)
if no_state_change is None:
self._vacuum_state.set_cleaning()
def pause(self):
self.logr.info("pause()")
if self._telemetry["docked"]:
self._vacuum_state.set_docked()
else:
self._vacuum_state.set_idle()
self.robot.pause()
def keep_alive(self):
self._keep_alive()
def _keep_alive(self):
""" Keep alive timer callback. Throws a gpio pin up and down """
self.logr.debug("SCI Keep Alive")
GPIO.output(DEVICE_DETECT, GPIO.LOW)
time.sleep(0.1)
GPIO.output(DEVICE_DETECT, GPIO.HIGH)
def poll_sensors_cb(self):
""" Poll sensors from hardware and do callbacks"""
self._spinner.spin()
old_sensors = dict()
for sensor_name in self._callbacks.keys():
try:
old_sensors[sensor_name] = self.robot.get_sensor(sensor_name)
except:
old_sensors[sensor_name] = None
try:
self.robot.update_sensors()
except BadDataLengthError as e:
self.logr.error(e)
return
except DriverError as e:
self.logr.warn(e)
self._telemetry["sci_error"] = True
self._keep_alive()
return
except Exception as e:
self.logr.error(e)
return
# Update Buttons
self._button_tracker.update(self.robot.get_sensor("buttons"))
# Update telemetry
self._update_telemetry()
# Do callbacks
for sensor_name, cb_list in self._callbacks.items():
for (cmp_fun, cb) in cb_list:
new_sensor_val = self.robot.get_sensor(sensor_name)
if old_sensors[sensor_name] is None or cmp_fun(
old_sensors[sensor_name], new_sensor_val):
cb()
def _update_telemetry(self):
# If sci error has been set (by poll_sensors_cb), undo it
if self._telemetry["sci_error"]:
self._telemetry["sci_error"] = False
# Compare charging status
try:
charging = self.robot.get_sensor("charging_state")
chargingbool = False if charging in [
"waiting", "not_charging", "error"
] else True
if not chargingbool == self._telemetry["charging"]:
self._telemetry["charging"] = chargingbool
if charging == "error":
self.logr.error("Charging: Fault")
elif charging == "waiting":
self.logr.warn("Charging: waiting")
elif charging == "trickle_charging":
self.logr.info("Charging: trickle")
elif charging == "charging":
self.logr.info("Charging: Charging")
elif charging == "charge_recovery":
self.logr.info("Charging: Recovery")
elif charging == "not_charging":
self.logr.info("Charging: Not Charging")
else:
self.logr.error("Charging: Received unknown state '%s'" %
str(charging))
except SensorStoreException as e:
self.logr.warn(e)
except Exception as e:
self.logr.warn(e)
self.logr.warn("Not updating telemetry this time")
# See if we are on the dock
try:
sources = self.robot.get_sensor("charging_sources_available")
if not sources["base"] == self._telemetry["docked"]:
self._telemetry["docked"] = sources["base"]
except SensorStoreException as e:
self.logr.warn(e)
# see if the robot has been picked up
try:
wheel_drops = self.robot.get_sensor("bumps_wheeldrops")
lifted = (wheel_drops["wheeldrop_left"]
or wheel_drops["wheeldrop_right"])
if not lifted == self._telemetry["lifted"]:
self._telemetry["lifted"] = lifted
except SensorStoreException as e:
self.logr.warn(e)
# Check if the sweeper brush is running (that means we are cleaning)
try:
# Eva has a lower current than the other robots - dips into 80s sometimes
# so this is a good threshold, I think
cleaning = self.robot.get_sensor("main_brush_current") >= 65
# if cleaning:
# print self.robot.get_sensor("main_brush_current")
if not cleaning == self._telemetry["cleaning"]:
self._telemetry["cleaning"] = cleaning
# Do cleaning cb if it is registered.
if self._cleaning_user_cb_funs:
for cb in self._cleaning_user_cb_funs:
cb(cleaning)
except SensorStoreException as e:
self.logr.warn(e)
# Check the status of button presses
try:
buttons = self._button_tracker.get_buttons()
buttons = [key for key, pressed in buttons.items() if pressed]
if not set(self._telemetry["buttons"]) == set(buttons):
self._telemetry["buttons"] = buttons
except SensorStoreException as e:
self.logr.warn(e)
# Check the robot voltage
try:
voltage = self.robot.get_sensor("voltage") / 1000.0
if abs(voltage - self._telemetry["battery_voltage"]) >= 0.1:
self._telemetry["battery_voltage"] = voltage
except SensorStoreException as e:
self.logr.warn(e)
# Set api mode
try:
oi_mode = self.robot.get_sensor("oi_mode")
if not oi_mode == self._telemetry["api_mode"]:
self._telemetry["api_mode"] = oi_mode
except SensorStoreException as e:
self.logr.warn(e)
def set_telemetry_cb(self, fun):
self._telemetry.set_telemetry_update_cb(fun)
def terminate(self):
self._keep_alive_timer.cancel()
self.logr.info("keep alive killed")
self._sensor_update_timer.cancel()
self.logr.info("killed sensor update timer")
time.sleep(1)
GPIO.cleanup()
self.robot.close()
self.logr.info("Clean shutdown achieved - cheers!")