################################################################################
# #
# This file is part of the Buildbotics firmware. #
# #
# Copyright (c) 2015 - 2018, Buildbotics LLC #
# All rights reserved. #
# #
# This file ("the software") is free software: you can redistribute it #
# and/or modify it under the terms of the GNU General Public License, #
# version 2 as published by the Free Software Foundation. You should #
# have received a copy of the GNU General Public License, version 2 #
# along with the software. If not, see . #
# #
# The software is distributed in the hope that it will be useful, but #
# WITHOUT ANY WARRANTY; without even the implied warranty of #
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU #
# Lesser General Public License for more details. #
# #
# You should have received a copy of the GNU Lesser General Public #
# License along with the software. If not, see #
# . #
# #
# For information regarding this software email: #
# "Joseph Coffland" #
# #
################################################################################
import traceback
import copy
import uuid
import os
import bbctrl
from watchdog.observers import Observer
from watchdog.events import FileSystemEventHandler
import glob
class UploadChangeHandler(FileSystemEventHandler):
def __init__(self, state):
self.state = state
def on_any_event(self, event):
self.state.load_files()
class State(object):
def __init__(self, ctrl):
self.ctrl = ctrl
self.log = ctrl.log.get('State')
self.callbacks = {}
self.changes = {}
self.listeners = []
self.timeout = None
self.machine_var_set = set()
self.message_id = 0
# Defaults
self.vars = {
'line': -1,
'messages': [],
'tool': 0,
'feed': 0,
'speed': 0,
'sid': str(uuid.uuid4()),
'demo': ctrl.args.demo,
}
# Add computed variable callbacks for each motor.
#
# NOTE, variable callbacks must return metric values only because
# the planner will scale returned values when in imperial mode.
for i in range(4):
self.set_callback(str(i) + 'home_position',
lambda name, i = i: self.motor_home_position(i))
self.set_callback(str(i) + 'home_travel',
lambda name, i = i: self.motor_home_travel(i))
self.set_callback(str(i) + 'latch_backoff',
lambda name, i = i: self.motor_latch_backoff(i))
self.set_callback(str(i) + 'zero_backoff',
lambda name, i = i: self.motor_zero_backoff(i))
self.set_callback(str(i) + 'search_velocity',
lambda name, i = i: self.motor_search_velocity(i))
self.set_callback(str(i) + 'latch_velocity',
lambda name, i = i: self.motor_latch_velocity(i))
#self.set_callback('metric', lambda name: 1 if self.is_metric() else 0)
#self.set_callback('imperial', lambda name: 0 if self.is_metric() else 1)
self.reset()
self.load_files()
observer = Observer()
observer.schedule(UploadChangeHandler(self), self.ctrl.get_upload(), recursive=True)
observer.start()
def init(self):
# Init machine units
metric = self.ctrl.config.get('units', 'METRIC').upper() == 'METRIC'
self.log.info('INIT Metric %d' % metric)
if not 'metric' in self.vars: self.set('metric', metric)
if not 'imperial' in self.vars: self.set('imperial', not metric)
# Bit diameter for probing
diameter = self.ctrl.config.get('probe-diameter', 6.35)
self.log.info('INIT Diameter %f' % diameter)
self.set('bitDiameter',diameter)
def reset(self):
# Unhome all motors
for i in range(4): self.set('%dhomed' % i, False)
# Zero offsets and positions
for axis in 'xyzabc':
self.set(axis + 'p', 0)
self.set('offset_' + axis, 0)
def load_files(self):
files = []
upload = self.ctrl.get_upload()
if not os.path.exists(upload):
os.mkdir(upload)
files = filter(os.path.isfile, glob.glob(upload + '/*'))
# Sort list of files based on last modification time in ascending order
files = sorted(files, key=os.path.getmtime, reverse=True)
files = list(map(lambda f: f.replace("./upload/", ""), files))
self.set('files', files)
if len(files): self.select_file(files[0])
else: self.select_file('')
def return_files(self):
files = copy.deepcopy(self.get('files'))
return files
def clear_files(self):
self.select_file('')
self.set('files', [])
def add_file(self, filename):
if filename.startswith('EgZjaHJvbWUqCggBEAAYsQMYgAQyBggAEEUYOTIKCAE'):
filename=filename.replace('EgZjaHJvbWUqCggBEAAYsQMYgAQyBggAEEUYOTIKCAE','')
files = copy.deepcopy(self.get('files'))
if not filename in files:
files.append(filename)
files.sort()
self.set('files', files)
else:
files = copy.deepcopy(self.get('files'))
if not filename in files:
files.append(filename)
files.sort()
self.set('files', files)
self.select_file(filename)
def remove_file(self, filename):
files = copy.deepcopy(self.get('files'))
if filename in files:
files.remove(filename)
self.set('files', files)
if self.get('selected', filename) == filename:
if len(files): self.select_file('')
else: self.select_file('')
def select_file(self, filename):
self.set('selected', filename)
time = os.path.getmtime(self.ctrl.get_upload(filename))
self.set('selected_time', time)
def set_bounds(self, bounds):
for axis in 'xyzabc':
for name in ('min', 'max'):
var = '%s_%s' % (axis, name)
value = bounds[name][axis] if axis in bounds[name] else 0
self.set(var, value)
def ack_message(self, id):
self.log.info('Message %d acknowledged' % id)
msgs = self.vars['messages']
msgs = list(filter(lambda m: id < m['id'], msgs))
self.set('messages', msgs)
def add_message(self, text):
msg = dict(text = text, id = self.message_id)
self.message_id += 1
msgs = self.vars['messages']
msgs = msgs + [msg] # It's important we make a new list here
self.set('messages', msgs)
def _notify(self):
if not self.changes: return
for listener in self.listeners:
try:
listener(self.changes)
except Exception as e:
self.log.warning('Updating state listener: %s',
traceback.format_exc())
self.changes = {}
self.timeout = None
def resolve(self, name):
# Resolve axis prefixes to motor numbers
if 2 < len(name) and name[1] == '_' and name[0] in 'xyzabc':
motor = self.find_motor(name[0])
if motor is not None: return str(motor) + name[2:]
return name
def has(self, name): return self.resolve(name) in self.vars
def set_callback(self, name, cb): self.callbacks[self.resolve(name)] = cb
def set(self, name, value):
name = self.resolve(name)
keys = ['xp', 'yp', 'zp', 'offset_x', 'offset_y', 'offset_z']
if not name in self.vars or self.vars[name] != value:
self.vars[name] = value
self.changes[name] = value
# Trigger listener notify
if self.timeout is None:
self.timeout = self.ctrl.ioloop.call_later(0.25, self._notify)
if name in keys and 'cycle' in self.vars:
self.log.info('244 sanjay cycle: %s'%self.vars['cycle'])
if self.vars['cycle'] == 'mdi':
self.log.info('246 changing data %s : %d , %d' % (name,value,self.vars[name]))
self.ctrl.config.set('axes',{name: value})
def update(self, update):
keys = ['xp', 'yp', 'zp', 'offset_x', 'offset_y', 'offset_z']
for name, value in update.items():
self.set(name, value)
if 'cycle' in self.vars and name in keys:
self.log.info('256 cycle: %s'%self.vars['cycle'])
if self.vars['cycle'] == 'idle':
self.log.info('Sanjay %s : %f' % (name,value))
self.ctrl.config.set('axes',{name: value})
def get(self, name, default = None):
name = self.resolve(name)
if name in self.vars: return self.vars[name]
if name in self.callbacks: return self.callbacks[name](name)
if default is None:
self.log.warning('State variable "%s" not found' % name)
return default
def snapshot(self):
vars = copy.deepcopy(self.vars)
for name in self.callbacks:
if not name in vars:
vars[name] = self.callbacks[name](name)
axis_motors = {axis: self.find_motor(axis) for axis in 'xyzabc'}
axis_vars = {}
for name, value in vars.items():
if name[0] in '0123':
motor = int(name[0])
for axis in 'xyzabc':
if motor == axis_motors[axis]:
axis_vars[axis + '_' + name[1:]] = value
vars.update(axis_vars)
return vars
def config(self, code, value):
# Set machine variables via mach, others directly
if code in self.machine_var_set: self.ctrl.mach.set(code, value)
else: self.set(code, value)
def add_listener(self, listener):
self.listeners.append(listener)
listener(self.vars)
def remove_listener(self, listener): self.listeners.remove(listener)
def set_machine_vars(self, vars):
# Record all machine vars, indexed or otherwise
self.machine_var_set = set()
for code, spec in vars.items():
if 'index' in spec:
for index in spec['index']:
self.machine_var_set.add(index + code)
else: self.machine_var_set.add(code)
def get_position(self):
position = {}
for axis in 'xyzabc':
if self.is_axis_enabled(axis) and self.has(axis + 'p'):
position[axis] = self.get(axis + 'p')
return position
def get_axis_vector(self, name, scale = 1):
v = {}
for axis in 'xyzabc':
motor = self.find_motor(axis)
if motor is not None and self.motor_enabled(motor):
value = self.get(str(motor) + name, None)
if value is not None: v[axis] = value * scale
return v
def get_soft_limit_vector(self, var, default):
limit = self.get_axis_vector(var, 1)
for axis in 'xyzabc':
if not axis in limit or not self.is_axis_homed(axis):
limit[axis] = default
return limit
def find_motor(self, axis):
for motor in range(4):
if not ('%dan' % motor) in self.vars: continue
motor_axis = 'xyzabc'[self.vars['%dan' % motor]]
if motor_axis == axis.lower() and self.vars.get('%dme' % motor, 0):
return motor
def is_axis_homed(self, axis): return self.get('%s_homed' % axis, False)
def is_axis_enabled(self, axis):
motor = self.find_motor(axis)
return motor is not None and self.motor_enabled(motor)
def get_enabled_axes(self):
axes = []
for axis in 'xyzabc':
if self.is_axis_enabled(axis):
axes.append(axis)
return axes
def is_motor_faulted(self, motor):
return self.get('%ddf' % motor, 0) & 0x1f
def is_axis_faulted(self, axis):
motor = self.find_motor(axis)
return motor is not None and self.is_motor_faulted(motor)
def axis_homing_mode(self, axis):
motor = self.find_motor(axis)
if motor is None: return 'disabled'
return self.motor_homing_mode(motor)
def axis_home_fail_reason(self, axis):
motor = self.find_motor(axis)
if motor is None: return 'Not mapped to motor'
if not self.motor_enabled(motor): return 'Motor disabled'
mode = self.motor_homing_mode(motor)
if mode != 'manual':
if mode == 'switch-min' and not int(self.get(axis + '_ls', 0)):
return 'Configured for min switch but switch is disabled'
if mode == 'switch-max' and not int(self.get(axis + '_xs', 0)):
return 'Configured for max switch but switch is disabled'
softMin = int(self.get(axis + '_tn', 0))
softMax = int(self.get(axis + '_tm', 0))
if softMax <= softMin + 1:
return 'max-soft-limit must be at least 1mm greater ' \
'than min-soft-limit'
def motor_enabled(self, motor):
return bool(int(self.vars.get('%dme' % motor, 0)))
def motor_homing_mode(self, motor):
mode = str(self.vars.get('%dho' % motor, 0))
if mode == '0': return 'manual'
if mode == '1': return 'switch-min'
if mode == '2': return 'switch-max'
if mode == '3': return 'stall-min'
if mode == '4': return 'stall-max'
raise Exception('Unrecognized homing mode "%s"' % mode)
def motor_home_direction(self, motor):
mode = self.motor_homing_mode(motor)
if mode.endswith('-min'): return -1
if mode.endswith('-max'): return 1
return 0 # Disabled
def motor_home_position(self, motor):
mode = self.motor_homing_mode(motor)
# Return soft limit positions
if mode.endswith('-min'): return self.vars['%dtn' % motor]
if mode.endswith('-max'): return self.vars['%dtm' % motor]
return 0 # Disabled
def motor_home_travel(self, motor):
tmin = self.get(str(motor) + 'tm', 0)
tmax = self.get(str(motor) + 'tn', 0)
hdir = self.motor_home_direction(motor)
# (travel_max - travel_min) * 1.5 * home_dir
return (tmin - tmax) * 1.5 * hdir
def motor_latch_backoff(self, motor):
lb = self.get(str(motor) + 'lb', 0)
hdir = self.motor_home_direction(motor)
return -(lb * hdir) # -latch_backoff * home_dir
def motor_zero_backoff(self, motor):
zb = self.get(str(motor) + 'zb', 0)
hdir = self.motor_home_direction(motor)
return -(zb * hdir) # -zero_backoff * home_dir
def motor_search_velocity(self, motor):
return 1000 * self.get(str(motor) + 'sv', 0)
def motor_latch_velocity(self, motor):
return 1000 * self.get(str(motor) + 'lv', 0)
def get_axis_switch(self, axis, side):
axis = axis.lower()
if not axis in 'xyzabc':
raise Exception('Unsupported switch "%s-%s"' % (axis, side))
if not self.is_axis_enabled(axis):
raise Exception('Switch "%s-%s" axis not enabled' % (axis, side))
motor = self.find_motor(axis)
# This must match the switch ID enum in avr/src/switch.h
hmode = self.motor_homing_mode(motor)
if hmode.startswith('stall-'): return motor + 10
return 2 * motor + 2 + (0 if side.lower() == 'min' else 1)
def get_switch_id(self, switch):
# TODO Support other input switches in CAMotics gcode/machine/PortType.h
switch = switch.lower()
if switch == 'probe': return 1
if switch[1:] == '-min': return self.get_axis_switch(switch[0], 'min')
if switch[1:] == '-max': return self.get_axis_switch(switch[0], 'max')
raise Exception('Unsupported switch "%s"' % switch)