- Fixed position problem with rapid MDI entry.
- Log debug messages to console in developer mode.
- Fixed AVR log message source.
- Fixed step correction.
+ - JOGGING, HOMMING and MDI states.
+ - Fixed position problem with rapid MDI entry.
## v0.3.8
- Fixed pwr flags display
}
// Dispatch non-empty commands
- if (*block && status == STAT_OK) {
- if (sync_q_empty()) command_reset_position();
- status = _dispatch(block);
- }
+ if (*block && status == STAT_OK) status = _dispatch(block);
block = 0; // Command consumed
#include "exec.h"
#include "state.h"
#include "scurve.h"
+#include "command.h"
#include "config.h"
#include <stdbool.h>
typedef struct {
- bool active;
bool writing;
bool done;
// Check if we are done
if (jr.done) {
+ command_reset_position();
exec_set_velocity(0);
exec_set_cb(0);
- jr.active = false;
+ state_idle();
return STAT_NOP; // Done, no move executed
}
stat_t command_jog(char *cmd) {
// Ignore jog commands when not already idle
- if (!jr.active && state_get() != STATE_READY) return STAT_NOP;
+ if (state_get() != STATE_READY && state_get() != STATE_JOGGING)
+ return STAT_NOP;
// Skip command code
cmd++;
if (*cmd) return STAT_INVALID_ARGUMENTS;
// Reset
- if (!jr.active) memset(&jr, 0, sizeof(jr));
+ if (state_get() != STATE_JOGGING) memset(&jr, 0, sizeof(jr));
jr.writing = true;
for (int axis = 0; axis < AXES; axis++)
jr.axes[axis].next = velocity[axis];
jr.writing = false;
- if (!jr.active) {
- jr.active = true;
+ if (state_get() != STATE_JOGGING) {
+ state_jogging();
exec_set_cb(jog_exec);
}
float j = l.line.max_jerk;
if (v < MIN_VELOCITY) {
+ command_reset_position();
exec_set_velocity(0);
exec_set_acceleration(0);
exec_set_jerk(0);
case STATE_READY: return PSTR("READY");
case STATE_ESTOPPED: return PSTR("ESTOPPED");
case STATE_RUNNING: return PSTR("RUNNING");
+ case STATE_JOGGING: return PSTR("JOGGING");
case STATE_STOPPING: return PSTR("STOPPING");
case STATE_HOLDING: return PSTR("HOLDING");
}
}
-void state_idle() {if (state_get() == STATE_RUNNING) _set_state(STATE_READY);}
+void state_jogging() {
+ if (state_get() == STATE_READY) _set_state(STATE_JOGGING);
+}
+
+
+void state_idle() {
+ if (state_get() == STATE_RUNNING || state_get() == STATE_JOGGING)
+ _set_state(STATE_READY);
+}
+
+
void state_estop() {_set_state(STATE_ESTOPPED);}
STATE_READY,
STATE_ESTOPPED,
STATE_RUNNING,
+ STATE_JOGGING,
STATE_STOPPING,
STATE_HOLDING,
} state_t;
void state_holding();
void state_optional_pause();
void state_running();
+void state_jogging();
void state_idle();
void state_estop();
methods: {
- get_state: function () {return this.state.xx || ''},
+ get_state: function () {
+ if (typeof this.state.cycle != 'undefined' &&
+ this.state.cycle != 'idle' && this.state.xx == 'RUNNING')
+ return this.state.cycle.toUpperCase();
+ return this.state.xx || ''
+ },
get_reason: function () {
class Comm():
- def __init__(self, ctrl):
+ def __init__(self, ctrl, next_cb, connect_cb):
self.ctrl = ctrl
+ self.next_cb = next_cb
+ self.connect_cb = connect_cb
self.queue = deque()
self.in_buf = ''
self.command = None
- ctrl.state.add_listener(self._update)
-
try:
self.sp = serial.Serial(ctrl.args.serial, ctrl.args.baud,
rtscts = 1, timeout = 0, write_timeout = 0)
self.i2c_addr = ctrl.args.avr_addr
- def start_sending_gcode(self, path):
- self.ctrl.planner.load(path)
- self.set_write(True)
-
-
- def stop_sending_gcode(self): self.ctrl.planner.reset()
+ def is_active(self):
+ return len(self.queue) or self.command is not None
def i2c_command(self, cmd, byte = None, word = None):
self.ctrl.ioloop.update_handler(self.sp, flags)
- def _update(self, update):
- if 'xx' in update and update['xx'] == 'ESTOPPED':
- self.stop_sending_gcode()
-
-
def _load_next_command(self, cmd):
log.info('< ' + json.dumps(cmd).strip('"'))
self.command = bytes(cmd.strip() + '\n', 'utf-8')
# Load next command from queue
if len(self.queue): self._load_next_command(self.queue.popleft())
- # Load next GCode command, if running or paused
- elif self.ctrl.planner.is_running():
- cmd = self.ctrl.planner.next()
+ # Load next command from callback
+ else:
+ cmd = self.next_cb()
- if cmd is None: self.set_write(False)
+ if cmd is None: self.set_write(False) # Stop writing
else: self._load_next_command(cmd)
- # Else stop writing
- else: self.set_write(False)
-
def _update_vars(self, msg):
try:
def connect(self):
try:
- # Reset AVR communication
- self.stop_sending_gcode()
+ # Call connect callback
+ self.connect_cb()
# Resume once current queue of GCode commands has flushed
self.queue_command(Cmd.RESUME)
self.web = bbctrl.Web(self)
try:
- self.planner = bbctrl.Planner(self)
self.i2c = bbctrl.I2C(args.i2c_port)
self.lcd = bbctrl.LCD(self)
self.mach = bbctrl.Mach(self)
class Mach():
def __init__(self, ctrl):
self.ctrl = ctrl
- self.comm = bbctrl.Comm(ctrl)
+ self.planner = bbctrl.Planner(ctrl)
+ self.comm = bbctrl.Comm(ctrl, self._comm_next, self._comm_connect)
self.stopping = False
+ ctrl.state.set('cycle', 'idle')
ctrl.state.add_listener(self._update)
self.comm.queue_command(Cmd.REBOOT)
- def _is_busy(self): return self.ctrl.planner.is_running()
+ def _get_cycle(self): return self.ctrl.state.get('cycle')
+
+
+ def _begin_cycle(self, cycle):
+ current = self._get_cycle()
+
+ if current == 'idle':
+ self.planner.update_position()
+ self.ctrl.state.set('cycle', cycle)
+
+ elif current == 'homing' and cycle == 'mdi': pass
+ elif current != cycle:
+ raise Exception('Cannot enter %s cycle during %s' %
+ (cycle, current))
def _update(self, update):
- if self.stopping and 'xx' in update and update['xx'] == 'HOLDING':
- self.comm.stop_sending_gcode()
+ state = self.ctrl.state.get('xx', '')
+
+ # Handle EStop
+ if 'xx' in update and state == 'ESTOPPED':
+ self._stop_sending_gcode()
+
+ # Handle stop
+ if self.stopping and 'xx' in update and state == 'HOLDING':
+ self._stop_sending_gcode()
# Resume once current queue of GCode commands has flushed
self.comm.i2c_command(Cmd.FLUSH)
self.comm.queue_command(Cmd.RESUME)
self.stopping = False
+ # Update cycle
+ if (self._get_cycle() != 'idle' and not self.planner.is_busy() and
+ not self.comm.is_active() and state == 'READY'):
+ self.ctrl.state.set('cycle', 'idle')
+
# Automatically unpause on seek hold
- if self.ctrl.state.get('xx', '') == 'HOLDING' and \
- self.ctrl.state.get('pr', '') == 'Switch found' and \
- self.ctrl.planner.is_synchronizing():
+ if (state == 'HOLDING' and
+ self.ctrl.state.get('pr', '') == 'Switch found' and
+ self.planner.is_synchronizing()):
self.ctrl.mach.unpause()
- def set(self, code, value):
- self.comm.queue_command('${}={}'.format(code, value))
+ def _comm_next(self):
+ if self.planner.is_running(): return self.planner.next()
- def mdi(self, cmd):
- if len(cmd) and cmd[0] == '$':
- equal = cmd.find('=')
- if equal == -1:
- log.info('%s=%s' % (cmd, self.ctrl.state.get(cmd[1:])))
+ def _comm_connect(self): self._stop_sending_gcode()
+
+
+ def _start_sending_gcode(self, path):
+ self.planner.load(path)
+ self.comm.set_write(True)
- else:
- name, value = cmd[1:equal], cmd[equal + 1:]
- if value.lower() == 'true': value = True
- elif value.lower() == 'false': value = False
- else:
- try:
- value = float(value)
- except: pass
+ def _stop_sending_gcode(self): self.planner.reset()
- self.ctrl.state.config(name, value)
- elif len(cmd) and cmd[0] == '\\': self.comm.queue_command(cmd[1:])
+ def _query_var(self, cmd):
+ equal = cmd.find('=')
+ if equal == -1:
+ log.info('%s=%s' % (cmd, self.ctrl.state.get(cmd[1:])))
else:
- self.ctrl.planner.mdi(cmd)
+ name, value = cmd[1:equal], cmd[equal + 1:]
+
+ if value.lower() == 'true': value = True
+ elif value.lower() == 'false': value = False
+ else:
+ try:
+ value = float(value)
+ except: pass
+
+ self.ctrl.state.config(name, value)
+
+
+ def mdi(self, cmd):
+ if not len(cmd): return
+ if cmd[0] == '$': self._query_var(cmd)
+ elif cmd[0] == '\\': self.comm.queue_command(cmd[1:])
+ else:
+ self._begin_cycle('mdi')
+ self.planner.mdi(cmd)
self.comm.set_write(True)
+ def set(self, code, value):
+ self.comm.queue_command('${}={}'.format(code, value))
+
+
def jog(self, axes):
- if self._is_busy(): raise Exception('Busy, cannot jog')
+ self._begin_cycle('jogging')
self.comm.queue_command(Cmd.jog(axes))
def home(self, axis, position = None):
- if self._is_busy(): raise Exception('Busy, cannot home')
+ self._begin_cycle('homing')
if position is not None:
self.mdi('G28.3 %c%f' % (axis, position))
def estop(self): self.comm.i2c_command(Cmd.ESTOP)
def clear(self): self.comm.i2c_command(Cmd.CLEAR)
- def start(self, path): self.comm.start_sending_gcode(path)
+
+
+ def start(self, path):
+ self._begin_cycle('running')
+ self._start_sending_gcode(path)
def step(self, path):
+ raise Exception('NYI') # TODO
self.comm.i2c_command(Cmd.STEP)
- if not self._is_busy() and path and \
- self.ctrl.state.get('xx', '') == 'READY':
- self.comm.start_sending_gcode(path)
+ if self._get_cycle() != 'running': self.start(path)
def stop(self):
self.comm.i2c_command(Cmd.FLUSH)
self.comm.queue_command(Cmd.RESUME)
- self.ctrl.planner.restart()
+ self.planner.restart()
self.comm.set_write(True)
self.comm.i2c_command(Cmd.UNPAUSE)
- def optional_pause(self): self.comm.i2c_command(Cmd.PAUSE, byte = 1)
+ def optional_pause(self):
+ if self._get_cycle() == 'running':
+ self.comm.i2c_command(Cmd.PAUSE, byte = 1)
def set_position(self, axis, position):
- if self._is_busy(): raise Exception('Busy, cannot set position')
-
if self.ctrl.state.is_axis_homed('%c' % axis):
self.mdi('G92 %c%f' % (axis, position))
else: self.comm.queue_command('$%cp=%f' % (axis, position))
def __init__(self, ctrl):
self.ctrl = ctrl
self.lastID = -1
- self.mode = 'idle'
self.setq = deque()
- ctrl.state.add_listener(self.update)
+ ctrl.state.add_listener(self._update)
self.reset()
+ def is_busy(self): return self.is_running() or len(self.setq)
def is_running(self): return self.planner.is_running()
def is_synchronizing(self): return self.planner.is_synchronizing()
- def get_config(self):
+ def update_position(self):
+ position = {}
+
+ for axis in 'xyzabc':
+ if not self.ctrl.state.is_axis_enabled(axis): continue
+ value = self.ctrl.state.get(axis + 'p', None)
+ if value is not None: position[axis] = value
+
+ self.planner.set_position(position)
+
+
+ def _get_config_vector(self, name, scale):
state = self.ctrl.state
+ v = {}
- # Axis mapping for enabled motors
- axis2motor = {}
- for i in range(4):
- if state.get('%dpm' % i, False):
- axis = 'xyzabc'[int(state.get('%dan' % i))]
- axis2motor[axis] = i
-
- def get_vector(name, scale = 1):
- v = {}
- for axis in 'xyzabc':
- if axis in axis2motor:
- motor = axis2motor[axis]
- value = state.get(str(motor) + name, None)
- if value is not None:
- v[axis] = value * scale
- return v
-
- # Starting position
- start = {}
for axis in 'xyzabc':
- if not axis in axis2motor: continue
- value = state.get(axis + 'p', None)
- if value is not None: start[axis] = value
+ motor = state.find_motor(axis)
+
+ if motor is not None and state.motor_enabled(motor):
+ value = state.get(str(motor) + name, None)
+ if value is not None: v[axis] = value * scale
+
+ return v
+
+ def _get_config(self):
config = {
- "start": start,
- "max-vel": get_vector('vm', 1000),
- "max-accel": get_vector('am', 1000000),
- "max-jerk": get_vector('jm', 1000000),
+ "max-vel": self._get_config_vector('vm', 1000),
+ "max-accel": self._get_config_vector('am', 1000000),
+ "max-jerk": self._get_config_vector('jm', 1000000),
# TODO junction deviation & accel
}
return config
- def update(self, update):
+ def _update(self, update):
if 'id' in update:
id = update['id']
self.planner.set_active(id)
# Syncronize planner variables with execution id
- self.release_set_cmds(id)
+ self._release_set_cmds(id)
- def release_set_cmds(self, id):
+ def _release_set_cmds(self, id):
self.lastID = id
# Apply all set commands <= to ID and those that follow consecutively
if id == self.lastID + 1: self.lastID = id
- def queue_set_cmd(self, id, name, value):
+ def _queue_set_cmd(self, id, name, value):
log.info('Planner set(#%d, %s, %s)', id, name, value)
self.setq.append((id, name, value))
- self.release_set_cmds(self.lastID)
+ self._release_set_cmds(self.lastID)
- def restart(self):
- state = self.ctrl.state
- id = state.get('id')
-
- position = {}
- for axis in 'xyzabc':
- if state.has(axis + 'p'):
- position[axis] = state.get(axis + 'p')
-
- log.info('Planner restart: %d %s' % (id, json.dumps(position)))
- self.planner.restart(id, position)
-
-
- def get_var(self, name):
+ def _get_var(self, name):
value = 0
if len(name) and name[0] == '_':
value = self.ctrl.state.get(name[1:], 0)
return value
- def log(self, line):
+ def _log(self, line):
line = line.strip()
m = reLogLine.match(line)
if not m: return
else: log.error('Could not parse planner log line: ' + line)
- def mdi(self, cmd):
- if self.mode == 'gcode':
- raise Exception('Cannot issue MDI command while GCode running')
-
- log.info('MDI:' + cmd)
- self.planner.load_string(cmd, self.get_config())
- self.mode = 'mdi'
-
-
- def load(self, path):
- if self.mode != 'idle':
- raise Exception('Busy, cannot start new GCode program')
-
- log.info('GCode:' + path)
- self.planner.load('upload' + path, self.get_config())
- self.mode = 'gcode'
-
-
- def reset(self):
- self.planner = gplan.Planner()
- self.planner.set_resolver(self.get_var)
- self.planner.set_logger(self.log, 1, 'LinePlanner:3')
- self.setq.clear()
-
-
- def _encode(self, block):
+ def __encode(self, block):
type = block['type']
if type == 'line':
if type == 'set':
name, value = block['name'], block['value']
- if name == 'line': self.queue_set_cmd(block['id'], name, value)
+ if name == 'line': self._queue_set_cmd(block['id'], name, value)
if name == 'tool': return Cmd.tool(value)
if name == 'speed': return Cmd.speed(value)
if name[0:1] == '_' and name[1:2] in 'xyzabc' and \
return Cmd.set_position(name[1], value)
if len(name) and name[0] == '_':
- self.queue_set_cmd(block['id'], name[1:], value)
+ self._queue_set_cmd(block['id'], name[1:], value)
return
raise Exception('Unknown planner type "%s"' % type)
- def encode(self, block):
- cmd = self._encode(block)
+ def _encode(self, block):
+ cmd = self.__encode(block)
if cmd is not None: return Cmd.set('id', block['id']) + '\n' + cmd
+ def reset(self):
+ self.planner = gplan.Planner()
+ self.planner.set_resolver(self._get_var)
+ self.planner.set_logger(self._log, 1, 'LinePlanner:3')
+ self.setq.clear()
+
+
+ def restart(self):
+ state = self.ctrl.state
+ id = state.get('id')
+
+ position = {}
+ for axis in 'xyzabc':
+ if state.has(axis + 'p'):
+ position[axis] = state.get(axis + 'p')
+
+ log.info('Planner restart: %d %s' % (id, json.dumps(position)))
+ self.planner.restart(id, position)
+
+
+ def mdi(self, cmd):
+ log.info('MDI:' + cmd)
+ self.planner.load_string(cmd, self._get_config())
+
+
+ def load(self, path):
+ log.info('GCode:' + path)
+ self.planner.load('upload' + path, self._get_config())
+
+
def has_move(self): return self.planner.has_more()
try:
while self.planner.has_more():
cmd = self.planner.next()
- cmd = self.encode(cmd)
+ cmd = self._encode(cmd)
if cmd is not None: return cmd
- if not self.is_running(): self.mode = 'idle'
-
except Exception as e:
self.reset()
log.exception(e)
return self.get('%s_homed' % axis, False)
+ def is_axis_enabled(self, axis):
+ motor = self.find_motor(axis)
+ return False if motor is None else self.motor_enabled(motor)
+
+
def axis_can_home(self, axis):
motor = self.find_motor(axis)
if motor is None: return False
return False
- def motor_enabled(self, motor): return bool(int(self.vars['%dpm' % motor]))
+ def motor_enabled(self, motor):
+ return bool(int(self.vars.get('%dpm' % motor, 0)))
+
+
def motor_homing_mode(self, motor): return int(self.vars['%dho' % motor])
projects / bbctrl-firmware / commitdiff