class IcingStage(Stage):
"""
classdocs
"""
@enum.unique
class WrapperID(IntEnum):
carriage = 0 # blocking
platform = 1 # blocking
nozzle = 2 # non-blocking
class CarriageWrapper(ActuatorWrapper):
logger = logging.getLogger("cookiebot.ActuatorWrapper.CarriageWrapper")
def __init__(self):
super(IcingStage.CarriageWrapper, self).__init__()
# set connection to stepper parameters here
# addr, stepper_num, and dist_per_step especially are crucial
self._wrapped_actuators["xmotor"] = StepperActuator(
identity="X-axis Stepper",
peak_rpm=8,
dist_per_step=0.014,
addr=0x60,
steps_per_rev=200,
stepper_num=1,
reversed=False,
)
self._wrapped_actuators["ymotor"] = StepperActuator(
identity="Y-axis Stepper",
peak_rpm=8,
dist_per_step=0.014,
addr=0x60,
steps_per_rev=200,
stepper_num=2,
reversed=False,
)
def zero(self):
self._wrapped_actuators["xmotor"].go_to_zero()
self._wrapped_actuators["xmotor"].go_to_zero()
def send(self, dest):
xmotor = self._wrapped_actuators["xmotor"]
ymotor = self._wrapped_actuators["ymotor"]
step_pos = (xmotor.step_pos, ymotor.step_pos)
pos = (xmotor.real_pos, ymotor.real_pos)
deltas = (dest[0] - pos[0], dest[1] - pos[1])
step_delta = (int(deltas[0] / xmotor.step_size), int(deltas[1] / ymotor.step_size))
self.logger.debug("Need to move {0} steps from {1} to {2}".format(step_delta, pos, dest))
step_points = self.bresenham((0, 0), step_delta)
step_points.append(step_delta)
xsteps = []
ysteps = []
last_p = (0, 0)
for p in step_points:
xsteps.append(cmp(p[0], last_p[0]))
ysteps.append(cmp(p[1], last_p[1]))
last_p = p
# self.logger.debug('Xsteps: {0}'.format(xsteps))
# self.logger.debug('Ysteps: {0}'.format(ysteps))
xmotor.set_task(task=array.array("b", xsteps), blocking=True)
ymotor.set_task(task=array.array("b", ysteps), blocking=True)
def bresenham(self, start_point, end_point):
"""Bresenham's line tracing algorithm, from roguebasin source
Inputs:
start_point: (x,y) pair of integer indices representing the grid
cell where the ray begins
end_point: (x,y) pair of integer indices representing the grid cell
where the ray stops
Outputs:
intersected_points: list of (x,y) pairs intersected by the line
between the start and end points. Does not include the start
or end points
"""
# Setup initial conditions
x1, y1 = start_point
x2, y2 = end_point
dx = x2 - x1
dy = y2 - y1
# Determine how steep the line is
is_steep = abs(dy) > abs(dx)
# Rotate line
if is_steep:
x1, y1 = y1, x1
x2, y2 = y2, x2
# Swap start and end points if necessary and store swap state
swapped = False
if x1 > x2:
x1, x2 = x2, x1
y1, y2 = y2, y1
swapped = True
# Recalculate differentials
dx = x2 - x1
dy = y2 - y1
# Calculate error
error = int(dx / 2.0)
ystep = 1 if y1 < y2 else -1
# Iterate over bounding box generating points between start and end
y = y1
points = []
for x in xrange(x1, x2):
coord = (y, x) if is_steep else (x, y)
points.append(coord)
error -= abs(dy)
if error < 0:
y += ystep
error += dx
# Reverse the list if the coordinates were swapped
if swapped:
points.reverse()
return points
class NozzleWrapper(ActuatorWrapper):
logger = logging.getLogger("cookiebot.ActuatorWrapper.NozzleWrapper")
def __init__(self):
super(IcingStage.NozzleWrapper, self).__init__()
# set connection to stepper parameters here
# addr, stepper_num, and dist_per_step especially are crucial
self._wrapped_actuators["nozzle"] = StepperActuator(
identity="Nozzle Stepper",
peak_rpm=3.6,
dist_per_step=0.00025,
addr=0x61,
max_dist=2.0,
steps_per_rev=200,
stepper_num=1,
reversed=True,
)
def zero(self):
pass
def send(self, command):
act = self._wrapped_actuators["nozzle"]
if command == "off":
self.logger.debug("Sending a short, blocking, shutoff task to turn off the nozzle")
act.set_rpm(15)
task = [-1 for _ in xrange(400)]
task.extend([0 for _ in xrange(325)])
task.extend([1 for _ in xrange(20)])
act.set_task(task=array.array("b", task), blocking=False)
elif command == "run":
ticks_to_go = act.max_steps - act.step_pos
self.logger.debug(
"Sending {0} forward steps to keep the nozzle running until 1) it runs out or 2) the task is changed".format(
ticks_to_go
)
)
act.set_rpm(3.6)
act.set_task(task=array.array("b", [1 for _ in xrange(ticks_to_go)]), blocking=False)
elif command == "on":
act.set_rpm(15)
self.logger.debug("Sending a short, blocking, start-up command to turn on the nozzle")
task = [1 for _ in xrange(250)]
task.extend([0 for _ in xrange(100)])
act.set_task(task=array.array("b", task), blocking=True)
class PlatformWrapper(ActuatorWrapper):
logger = logging.getLogger("cookiebot.ActuatorWrapper.PlatformWrapper")
def __init__(self):
super(IcingStage.PlatformWrapper, self).__init__()
# set connection to stepper parameters here
# addr, stepper_num, and dist_per_step especially are crucial
# also the value of go_to_zero
self._wrapped_actuators["platform"] = StepperActuator(
identity="Platform Stepper",
peak_rpm=30,
dist_per_step=0.00025,
max_dist=0.25,
addr=0x61,
steps_per_rev=200,
stepper_num=2,
)
def zero(self):
pass
def send(self, bool_command):
act = self._wrapped_actuators["platform"]
if bool_command:
ticks_to_go = act.max_steps - act.step_pos
act.set_task(task=array.array("b", [1 for _ in xrange(ticks_to_go)]), blocking=True)
self.logger.debug("Sending {0} raising steps".format(ticks_to_go))
else:
ticks_to_go = act.step_pos
act.set_task(task=array.array("b", [-1 for _ in xrange(ticks_to_go)]), blocking=True)
self.logger.debug("Sending {0} lowering steps".format(ticks_to_go))
logger = logging.getLogger("cookiebot.Stage.IcingStage")
def __init__(self, zero=False, actuators=[0, 1, 2]):
"""
constructor
"""
super(IcingStage, self).__init__()
self.steps = []
self.step_ready = True
self._wrappers = {
IcingStage.WrapperID.carriage: IcingStage.CarriageWrapper(),
IcingStage.WrapperID.nozzle: IcingStage.NozzleWrapper(),
IcingStage.WrapperID.platform: IcingStage.PlatformWrapper(),
}
self.active_wrappers = [id for id in self._wrappers.keys() if id.value in actuators]
self.logger.debug("Active wrappers are {0}".format(self.active_wrappers))
# Set up assorted parameters
if not zero:
self.x_cookie_shift = (0.0, 4.5)
self.y_cookie_shift = (0.0, 4.5)
if zero:
self.logger.info("Commanded to zero before execution")
self.x_cookie_shift = (9.0, 4.0)
self.y_cookie_shift = (9.0, 4.0)
for wrap in self._wrappers:
wrap.zero()
self._recipe_timer = RepeatedTimer(0.1, self._check_recipe, start=False)
def start_recipe(self):
self.logger.info("Starting recipe")
self._recipe_timer.restart()
for actuator in self._wrappers.values():
actuator.unpause()
def stop_recipe(self):
self.logger.info("Halting recipe progress immediately")
self._recipe_timer.stop()
for actuator in self._wrappers.values():
actuator.pause()
def recipe_done(self):
return not self.steps and self._check_actuators()
def shutdown(self):
"""This recipe completely stops the execution of the stage
It CANNOT BE CALLED by the self._recipe_timer, in any way
"""
self.steps = []
self._recipe_timer.stop()
self.live = False
for act in self._wrappers.values():
act.kill()
def _check_recipe(self):
"""Frequently-called method that checks if another step of the recipe
should be executed, and executes it if so"""
if self.live and self.step_ready and self.steps and self._check_actuators():
# we need to start the next command
self.step_ready = False # boring mutex on _check_recipe
next_step, self.steps = self.steps[0], self.steps[1:]
self.logger.info("Executing step {0}".format(next_step))
for actuator, command in next_step.items():
if actuator in self.active_wrappers:
self._wrappers[actuator].pause()
self._wrappers[actuator].send(command)
else:
self.logger.debug("Not taking steps for actuator {0}".format(actuator))
for actuator, command in next_step.items():
if actuator in self.active_wrappers:
self._wrappers[actuator].unpause()
self.step_ready = True
def _check_actuators(self):
for w in self._wrappers.values():
try:
ready = w.check_ready()
except ExecutionError as e:
self.logger.error("Wrapper says actuator is dead with error {0}".format(e))
self.logger.error("Terminating stage")
self.live = False
return False
if not ready:
return False
return True
def load_recipe(self, recipe):
self.logger.info("Begining recipe load")
parsed = []
parsed.append({IcingStage.WrapperID.carriage: (0, 0), IcingStage.WrapperID.platform: True})
for cookie_pos, cookie_spec in sorted(recipe.cookies.items(), key=lambda p: p[0]):
icing_coms = self._load_icing_file(cookie_spec["icing"].value)
offset_coms = self._offset_commands(icing_coms, cookie_pos)
parsed.extend(offset_coms)
# every recipe ends by stopping the nozzle, zeroing the carriage, and
# lowering the platform
parsed.append({IcingStage.WrapperID.carriage: (0, 0), IcingStage.WrapperID.platform: False})
self.logger.info("Loaded a recipe with {0} steps".format(len(parsed)))
self.steps = parsed[:]
def _load_icing_file(self, filename):
"""Load an icing file and return a list of commands
Arguments:
filename - an icing spec file
Returns:
A list of dictionaries {actuator:command}, each defining one
'action'
e.g. [{IcingStage.WrapperID.carriage:(0,1),
IcingStage.WrapperID.nozzle:False}]
Would move to (0,1) with the nozzle off, no action for the platform
Commands in a dictionary can be assumed to be started relatively
simultaneously (think us difference)
Assigned to Cynthia
"""
coms = []
with open(os.path.join(DATA_DIR, filename), "r") as icingspec:
for line in icingspec:
coms.append({IcingStage.WrapperID(idx): com for idx, com in literal_eval(line).items()})
return coms
def _offset_commands(self, commands, pos):
"""Take a list of command dictionaries and shift the positions based on
which cookie slot the cookie belongs to
Arguments:
commands - a list of command dictionaries, as returned by
load_icing_file
pos - a cookie position, as returned by a recipe
Returns:
An updated list of command dictionaries
"""
new_commands = list(commands)
for c in new_commands:
if IcingStage.WrapperID.carriage in c:
old_dest = c[IcingStage.WrapperID.carriage]
new_dest = self._shift_point(old_dest, pos)
c[IcingStage.WrapperID.carriage] = new_dest
return new_commands
def _shift_point(self, coord, cookiepos):
"""Shift a single coordinate based on the cookiepos it belongs to"""
x = self.x_cookie_shift[0] + coord[0] + cookiepos[0] * self.x_cookie_shift[1]
y = self.y_cookie_shift[0] + coord[1] + cookiepos[1] * self.y_cookie_shift[1]
return (x, y)
class Actuator(object):
'''
Base class for all types of actuators
This class exposes a set of public methods that an be called to use the
actuator. These methods will be identical for all actuators (although they
will take different arguments) It also defines some private methods that
SHOULD or MUST be overridden in order to have a real, functioning actuator
'''
logger = logging.getLogger('cookiebot.Actuator')
@enum.unique
class State(enum.IntEnum):
ready = 0
executing = 1
executing_blocked = 2
dead = 3
def __init__(self, identity='', run_interval=0.1):
'''
Constructor
Prepares an actuator to receive commands, assigns its ID, and starts
execution.
'''
self.logger.debug(
'Create actuator {0} with interval {1}'.format(identity, run_interval))
self.state = Actuator.State.ready
self.identity = identity if identity else str(uuid1())
self._task = None
self._timer = RepeatedTimer(run_interval, self._run_execution)
def __str__(self):
return self.identity
def set_task(self, task=None, blocking=False):
'''Public API for assigning a task to an actuator
Raises CommandError if the command is, for some reason, invalid
'''
if self.state is Actuator.State.dead:
self.logger.error(
'Cannot set tasks on {0} because it is dead'.format(self))
raise CommandError('Actuator is dead and cannot be commanded')
if self.state is Actuator.State.executing_blocked:
self.logger.error(
'Cannot change task on {0} while executing a blocking task'.format(self))
raise CommandError(
'Cannot change task while executing a blocking task')
if not self._validate_task(task):
self.logger.error(
'Invalid task provided to actuator {0}'.format(self))
raise CommandError(
'Task {0} is not valid for actuator {1}'.format(task, self))
self._task = task
self._task_is_blocking = blocking
self._run_execution()
def _run_execution(self):
'''Private method called repeatedly and frequently to update the state
Raises ExecutionError if something goes wrong
'''
if self._task and self.state == Actuator.State.ready or self.state == Actuator.State.executing:
self.state = Actuator.State.executing_blocked if self._task_is_blocking else Actuator.State.executing
if self.state == Actuator.State.executing or self.state == Actuator.State.executing_blocked:
if not self._check_bounds():
self.state = Actuator.State.dead
self.logger.error(
'Bounds violated, setting state of {0} to dead'.format(self))
if self._task_is_complete():
self.state = Actuator.State.ready
self.logger.debug('Done with task for {0}'.format(self))
else:
try:
self._execute_task()
except ExecutionError as e:
self.logger.error(
'Execution failed with error {0}'.format(e))
self.logger.error(
'Setting actuator to dead on account of error')
self.state = Actuator.State.dead
def kill(self):
'''Public API method - kill this actuator
Prevents setting or executing tasks in the future. Attempts to halt
actuator
DOES NOT stop the execution of the RepeatedTimer
Because that causes a "joining self" error on the thread
'''
self.logger.debug(
'Killing actuator {0} and stopping thread'.format(self))
self.state = Actuator.State.dead
self.pause()
def pause(self):
self.logger.debug('Pausing thread for actuator {0}'.format(self))
self._timer.stop()
def unpause(self):
self.logger.debug('Unpausing thread for actuator {0}'.format(self))
self._timer.restart()
def _check_bounds(self):
'''Private method to make sure that the actuator in a valid location
Should be uniquely implemented by all subclasses
Should return True if the actuator is within its bounds/conditions, or
False otherwise
This version assumes everything is fine and reports such
'''
return True
def _validate_task(self, task):
'''Private method for determining if a task if valid for an actuator
Must be implemented uniquely by all subclasses
Returns True if the task will be okay, False otherwise
Default version assumes anything passed at all is a valid task
This means I could say task='ASHFSDJHSLKBJNS' and the actuator would go
"Fine by me!"
Please override this method in subclasses.
'''
return True
def _task_is_complete(self):
'''Private method for checking if the assigned task (self.task) is done
Must be implemented uniquely by all subclasses
Should return True if the task is done enough to stop, else False
'''
return True
def _execute_task(self):
'''Private method to do external interfacing and actually send commands
to the actuator
Must be uniquely implemented by every subclass
Void method - does not return anything
Should raise ExecutionError if something horrid happens
'''
pass
class CookieGUI(Ui_MainWindow, QMainWindow):
'''
classdocs
'''
def __init__(self):
'''
Constructor
'''
# Initialization of GUI from Qt Designer
super(CookieGUI, self).__init__()
self.setupUi(self)
# add button
# clear button (must reset all displays and cookiepos)
# run button
# cancel button
self.add_cookie_button.clicked.connect(self._add_cookie_callback)
self.clear_recipe_button.clicked.connect(self._reset_recipe_callback)
self.start_button.clicked.connect(self._run_click_callback)
self.stop_button.clicked.connect(self._cancel_execution_callback)
self.terminate_button.clicked.connect(self._shutdown_stage_callback)
self.logger = logging.getLogger('cookiebot')
self.printerbox = OutLog(self.console, interval_ms=250)
self.printstream = SignalStream(interval_ms=100)
sys.stdout = self.printstream
sys.stderr = self.printstream
self.printstream.write_signal.connect(self.print_to_gui)
screen_handler = logging.StreamHandler(stream=self.printstream)
screen_format = logging.Formatter(fmt='%(asctime)s - %(message)s')
screen_handler.setLevel(logging.INFO)
screen_handler.setFormatter(screen_format)
self.logger.addHandler(screen_handler)
self.logger.setLevel(logging.DEBUG)
self.logger.info('Start of program execution '
'{0}'.format(datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')))
self.recipe = Recipe()
self.stage = IcingStage(zero=False, actuators=[0, 1, 2])
self.positions = [(0, 0), (1, 0), (0, 1), (1, 1)]
self.q_image_displays = [
self.image_00, self.image_10, self.image_01, self.image_11]
self.icings = [
Recipe.IcingType.d_outline,
Recipe.IcingType.u_outline,
Recipe.IcingType.k_outline,
Recipe.IcingType.e_outline,
Recipe.IcingType.square,
Recipe.IcingType.spiral_square,
Recipe.IcingType.blue_devil
]
self.pattern_images = [
Recipe.IcingImage.d_outline,
Recipe.IcingImage.u_outline,
Recipe.IcingImage.k_outline,
Recipe.IcingImage.e_outline,
Recipe.IcingImage.square,
Recipe.IcingImage.spiral_square,
Recipe.IcingImage.blue_devil
]
self.pbar_timer = RepeatedTimer(0.25, self._update_progress_bar, start=False)
self.show()
@QtCore.pyqtSlot(str)
def print_to_gui(self, text):
'''Routes a message to this instance's textbox
This is a slot and is implemented in conjunction with the SignalStream
in threadsafety to provide thread safe access to the GUI textbox
'''
self.printerbox.write(text)
def _run_click_callback(self):
if self.stage.live:
if self.stage.recipe_done():
self.logger.info('Starting a new recipe!')
try:
self.stage.load_recipe(self.recipe)
self.num_steps = float(len(self.stage.steps))
except (RecipeError, IOError) as e:
logging.error(
'Something is wrong with that recipe file! Shutting down.')
self.stage.shutdown()
raise e
#self.pbar_timer.restart()
else:
self.logger.info('Rebooting the recipe that was running')
self.stage.start_recipe()
else:
self.logger.info(
'Stage is dead, cannot do anything. Please exit.')
def _add_cookie_callback(self):
if self.stage.live:
cookie_idx = self.cookie_select.currentIndex()
pos_idx = self.pos_select.currentIndex()
self.logger.info(
'Adding cookie {0} to the recipe at {1}'.format(self.icings[cookie_idx], self.positions[pos_idx]))
self.recipe.add_cookie(
{'icing': self.icings[cookie_idx]}, self.positions[pos_idx])
image = self.q_image_displays[pos_idx]
scene = QGraphicsScene()
scene.addPixmap(
QPixmap(os.path.join(DATA_DIR, self.pattern_images[cookie_idx].value)))
image.setScene(scene)
image.fitInView(scene.itemsBoundingRect())
image.show()
else:
self.logger.info(
'Stage is dead, cannot do anything. Please exit.')
def _reset_recipe_callback(self):
if self.stage.live:
self.recipe = Recipe()
if self.stage.steps:
self.stage.steps = []
for image in self.q_image_displays:
image.setScene(QGraphicsScene())
self.logger.info('Recipe cleared')
else:
self.logger.info(
'Stage is dead, cannot do anything. Please exit.')
def _cancel_execution_callback(self):
if self.stage.live and not self.stage.recipe_done():
self.logger.info('Pausing recipe execution')
self.stage.stop_recipe()
self.logger.info('Recipe execution paused')
def _shutdown_stage_callback(self):
self.logger.warning('Terminating the icing stage')
self.stage.shutdown()
self.logger.warning('Stage terminated. Please exit.')
def _update_progress_bar(self):
fraction_to_go = len(self.stage.steps) / self.num_steps if self.num_steps else 1
self.progress_bar.setValue(100*(1-fraction_to_go))
def closeEvent(self, event):
self.logger.info("User has clicked the red x on the main window")
self.stage.shutdown()
self.pbar_timer.stop()
event.accept()