From 2f2b641c42559b85aef2b970bfed9849560bb275 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Tue, 6 Feb 2018 13:29:06 -0800 Subject: [PATCH] Fixed homing and MDI commands --- package.json | 2 +- src/avr/src/command.c | 5 +++ src/avr/src/command.h | 1 + src/avr/src/line.c | 6 +-- src/avr/src/vars.c | 4 ++ src/js/control-view.js | 2 +- src/py/bbctrl/AVR.py | 32 +++++++++------- src/py/bbctrl/Cmd.py | 79 ++++++++++++++++++++++++++++++++++++++++ src/py/bbctrl/Planner.py | 54 +++++++++++++++------------ src/py/bbctrl/State.py | 16 +++++++- 10 files changed, 156 insertions(+), 45 deletions(-) mode change 100644 => 100755 src/py/bbctrl/Cmd.py diff --git a/package.json b/package.json index 46334ea..3fe2498 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "bbctrl", - "version": "0.3.1", + "version": "0.3.2", "homepage": "https://github.com/buildbotics/bbctrl-firmware", "license": "GPL 3+", "dependencies": { diff --git a/src/avr/src/command.c b/src/avr/src/command.c index 2faeb63..2ad1a95 100644 --- a/src/avr/src/command.c +++ b/src/avr/src/command.c @@ -217,6 +217,11 @@ bool command_callback() { } +void command_set_axis_position(int axis, const float p) { + cmd.position[axis] = p; +} + + void command_set_position(const float position[AXES]) { memcpy(cmd.position, position, sizeof(cmd.position)); } diff --git a/src/avr/src/command.h b/src/avr/src/command.h index 1b20837..03f0b88 100644 --- a/src/avr/src/command.h +++ b/src/avr/src/command.h @@ -48,6 +48,7 @@ void command_print_json(); void command_flush_queue(); void command_push(char code, void *data); bool command_callback(); +void command_set_axis_position(int axis, const float p); void command_set_position(const float position[AXES]); void command_get_position(float position[AXES]); void command_reset_position(); diff --git a/src/avr/src/line.c b/src/avr/src/line.c index 27c9473..15344ae 100644 --- a/src/avr/src/line.c +++ b/src/avr/src/line.c @@ -286,10 +286,6 @@ stat_t command_line(char *cmd) { stat_t status = decode_axes(&cmd, line.target); if (status) return status; - // Zero disabled axes (TODO should moving a disable axis cause an error?) - for (int i = 0; i < AXES; i++) - if (!axis_is_enabled(i)) line.target[i] = 0; - // Get times bool has_time = false; while (*cmd) { @@ -300,7 +296,7 @@ stat_t command_line(char *cmd) { float time; if (!decode_float(&cmd, &time)) return STAT_BAD_FLOAT; - if (time < 0 || 0x10000 * SEGMENT_TIME <= time) return STAT_BAD_SEG_TIME; + if (time < 0) return STAT_BAD_SEG_TIME; line.times[seg] = time; if (time) has_time = true; } diff --git a/src/avr/src/vars.c b/src/avr/src/vars.c index 4f2fc70..d7051d1 100644 --- a/src/avr/src/vars.c +++ b/src/avr/src/vars.c @@ -398,6 +398,10 @@ stat_t command_sync_var(char *cmd) { command_push(*cmd, &buffer); + // Special case for synchronizing axis position in command queue + if (info.set.set_f32_index == set_axis_position) + command_set_axis_position(buffer.index, buffer.value._f32); + return STAT_OK; } diff --git a/src/js/control-view.js b/src/js/control-view.js index ff7e19e..11f2b4e 100644 --- a/src/js/control-view.js +++ b/src/js/control-view.js @@ -133,7 +133,7 @@ module.exports = { is_homed: function (axis) { var motor = this.get_axis_motor_id(axis); - return motor != -1 && this.state[motor + 'h']; + return motor != -1 && this.state[motor + 'homed']; }, diff --git a/src/py/bbctrl/AVR.py b/src/py/bbctrl/AVR.py index 3ba67e2..84b3a6d 100644 --- a/src/py/bbctrl/AVR.py +++ b/src/py/bbctrl/AVR.py @@ -14,20 +14,20 @@ log = logging.getLogger('AVR') # Axis homing procedure: # -# Set axis unhomed +# Mark axis unhomed # Seek closed (home_dir * (travel_max - travel_min) * 1.5) at search_velocity # Seek open (home_dir * -latch_backoff) at latch_vel # Seek closed (home_dir * latch_backoff * 1.5) at latch_vel -# Rapid to (home_dir * -zero_backoff + seek_position) -# Set axis homed and home position +# Rapid to (home_dir * -zero_backoff + position) +# Mark axis homed and set absolute position axis_homing_procedure = ''' G28.2 %(axis)s0 F[#<_%(axis)s_sv>] G38.6 %(axis)s[#<_%(axis)s_hd> * [#<_%(axis)s_tm> - #<_%(axis)s_tn>] * 1.5] G38.8 %(axis)s[#<_%(axis)s_hd> * -#<_%(axis)s_lb>] F[#<_%(axis)s_lv>] G38.6 %(axis)s[#<_%(axis)s_hd> * #<_%(axis)s_lb> * 1.5] - G0 G53 %(axis)s[#<_%(axis)s_hd> * -#<_%(axis)s_zb> + #<_%(axis)s_sp>] - G28.3 %(axis)s[#<_%(axis)s_hp>] + G91 G0 G53 %(axis)s[#<_%(axis)s_hd> * -#<_%(axis)s_zb>] + G90 G28.3 %(axis)s[#<_%(axis)s_hp>] ''' @@ -63,6 +63,7 @@ class AVR(): def _is_busy(self): return self.ctrl.planner.is_running() + def _i2c_command(self, cmd, byte = None, word = None): log.info('I2C: ' + cmd) retry = 5 @@ -87,9 +88,6 @@ class AVR(): def _start_sending_gcode(self, path): - if self._is_busy(): raise Exception('Busy, cannot start new GCode file') - - log.info('Running ' + path) self.ctrl.planner.load(path) self._set_write(True) @@ -251,14 +249,22 @@ class AVR(): def mdi(self, cmd): - if self._is_busy(): raise Exception('Busy, cannot queue MDI command') - if len(cmd) and cmd[0] == '$': equal = cmd.find('=') if equal == -1: log.info('%s=%s' % (cmd, self.ctrl.state.get(cmd[1:]))) - else: self._queue_command(cmd) + 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 + + self.ctrl.state.config(name, value) elif len(cmd) and cmd[0] == '\\': self._queue_command(cmd[1:]) @@ -300,7 +306,6 @@ class AVR(): def start(self, path): - if self._is_busy(): raise Exception('Busy, cannot start file') if path: self._start_sending_gcode(path) @@ -339,6 +344,5 @@ class AVR(): if self._is_busy(): raise Exception('Busy, cannot set position') if self.ctrl.state.is_axis_homed('%c' % axis): - raise Exception('NYI') # TODO - self._queue_command('G92 %c%f' % (axis, position)) + self.ctrl.planner.mdi('G92 %c%f' % (axis, position)) else: self._queue_command('$%cp=%f' % (axis, position)) diff --git a/src/py/bbctrl/Cmd.py b/src/py/bbctrl/Cmd.py old mode 100644 new mode 100755 index 056a81c..75ac09e --- a/src/py/bbctrl/Cmd.py +++ b/src/py/bbctrl/Cmd.py @@ -1,5 +1,8 @@ +#!/usr/bin/env python3 + import struct import base64 +import json import logging log = logging.getLogger('Cmd') @@ -65,6 +68,7 @@ def line(id, target, exitVel, maxAccel, maxJerk, times): def tool(tool): return '#t=%d' % tool def speed(speed): return '#s=:' + encode_float(speed) +def set_position(axis, value): return '#%sp=:%s' % (axis, encode_float(value)) def output(port, value): if port == 'mist': return '#1oa=' + ('1' if value else '0') @@ -97,3 +101,78 @@ def seek(switch, active, error): cmd += chr(flags + ord('0')) return cmd + + +def decode_command(cmd): + if not len(cmd): return + + data = {} + + if cmd[0] == SET or cmd[0] == SET_SYNC: + data['type'] = 'set' + if cmd[0] == SET_SYNC: data['sync'] = True + + equal = cmd.find('=') + data['name'] = cmd[1:equal] + + value = cmd[equal + 1:] + + if value.lower() == 'true': value = True + elif value.lower() == 'false': value = False + elif value.find('.') == -1: data['value'] = int(value) + else: data['value'] = float(value) + + elif cmd[0] == SEEK: + data['type'] = 'seek' + + data['port'] = int(cmd[2], 16) + flags = int(cmd[2], 16) + + data['active'] = bool(flags & SEEK_ACTIVE) + data['error'] = bool(flags & SEEK_ERROR) + + elif cmd[0] == LINE: + data['type'] = 'line' + data['exit-vel'] = decode_float(cmd[1:7]) + data['max-accel'] = decode_float(cmd[7:13]) + data['max-jerk'] = decode_float(cmd[13:19]) + + data['target'] = {} + data['times'] = [0] * 7 + cmd = cmd[19:] + + while len(cmd): + name = cmd[0] + value = decode_float(cmd[1:7]) + cmd = cmd[7:] + + if name in 'xyzabcuvw': data['target'][name] = value + else: data['times'][int(name)] = value + + elif cmd[0] == REPORT: data['type'] = 'report' + elif cmd[0] == PAUSE: data['type'] = 'pause' + elif cmd[0] == UNPAUSE: data['type'] = 'unpause' + elif cmd[0] == ESTOP: data['type'] = 'estop' + elif cmd[0] == CLEAR: data['type'] = 'clear' + elif cmd[0] == FLUSH: data['type'] = 'flush' + elif cmd[0] == STEP: data['type'] = 'step' + elif cmd[0] == RESUME: data['type'] = 'resume' + + print(json.dumps(data)) + + +def decode(cmd): + for line in cmd.split('\n'): + decode_command(line.strip()) + + +if __name__ == "__main__": + import sys + + if 1 < len(sys.argv): + for arg in sys.argv[1:]: + decode(arg) + + else: + for line in sys.stdin: + decode(line) diff --git a/src/py/bbctrl/Planner.py b/src/py/bbctrl/Planner.py index d43d8ae..6b737e4 100644 --- a/src/py/bbctrl/Planner.py +++ b/src/py/bbctrl/Planner.py @@ -11,7 +11,7 @@ class Planner(): def __init__(self, ctrl): self.ctrl = ctrl self.lastID = -1 - self.done = False + self.mode = 'idle' ctrl.state.add_listener(lambda x: self.update(x)) @@ -58,12 +58,11 @@ class Planner(): def update(self, update): - if 'id' in update: - id = update['id'] - if id: self.planner.release(id - 1) + if 'id' in update: self.planner.set_active(update['id']) - if update.get('x', '') == 'HOLDING' and \ - self.ctrl.state.get('pr', '') == 'Switch found': + if self.ctrl.state.get('x', '') == 'HOLDING' and \ + self.ctrl.state.get('pr', '') == 'Switch found' and \ + self.planner.is_synchronizing(): self.ctrl.avr.unpause() @@ -78,7 +77,6 @@ class Planner(): log.info('Planner restart: %d %s' % (id, json.dumps(position))) self.planner.restart(id, position) - self.done = False def get_var(self, name): @@ -95,28 +93,35 @@ class Planner(): if len(line) < 3: return if line[0] == 'I': log.info(line[3:]) - if line[0] == 'D': log.debug(line[3:]) - if line[0] == 'W': log.warning(line[3:]) - if line[0] == 'E': log.error(line[3:]) - if line[0] == 'C': log.critical(line[3:]) + elif line[0] == 'D': log.debug(line[3:]) + # TODO send these to the LCD and Web + elif line[0] == 'W': log.warning(line[3:]) + elif line[0] == 'E': log.error(line[3:]) + elif line[0] == 'C': log.critical(line[3:]) + else: raise Exception('Could not parse planner log line: ' + line) def mdi(self, cmd): - self.planner.set_config(self.get_config()) - self.planner.mdi(cmd) - self.done = False + if self.mode == 'gcode': + raise Exception('Cannot issue MDI command while GCode running') + + log.info('MDI:' + cmd) + self.planner.load_string(cmd) + self.mode = 'mdi' def load(self, path): - self.planner.set_config(self.get_config()) + if self.mode != 'idle': + raise Exception('Busy, cannot start new GCode program') + + log.info('GCode:' + path) self.planner.load('upload' + path) - self.done = False def reset(self): self.planner = gplan.Planner(self.get_config()) self.planner.set_resolver(self.get_var) - self.planner.set_logger(self.log) + self.planner.set_logger(self.log, 1, 'LinePlanner:3') def encode(self, block): @@ -133,6 +138,9 @@ class Planner(): if name == 'line': return Cmd.line_number(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 \ + name[2:] == '_home': + return Cmd.set_position(name[1], value) if len(name) and name[0] == '_': self.ctrl.state.set(name[1:], value) @@ -154,15 +162,15 @@ class Planner(): def next(self): + if not self.is_running(): + config = self.get_config() + log.info('Planner config:' + json.dumps(config)) + self.planner.set_config(config) + while self.planner.has_more(): cmd = self.planner.next() self.lastID = cmd['id'] cmd = self.encode(cmd) if cmd is not None: return cmd - if not self.done: - self.done = True - - # Cause last cmd to flush when complete - if 0 <= self.lastID: - return '#id=%d' % (self.lastID + 1) + if not self.is_running(): self.mode = 'idle' diff --git a/src/py/bbctrl/State.py b/src/py/bbctrl/State.py index 81d7eff..d4e1f45 100644 --- a/src/py/bbctrl/State.py +++ b/src/py/bbctrl/State.py @@ -26,6 +26,13 @@ class State(object): self.set_callback(str(i) + 'hd', lambda name, i = i: self.motor_home_direction(i)) + # Add home position callbacks + self.set_callback(str(i) + 'hp', + lambda name, i = i: self.motor_home_position(i)) + + # Set not homed + self.set('%dhomed' % i, False) + def _notify(self): if not self.changes: return @@ -82,7 +89,7 @@ class State(object): def config(self, code, value): if code in self.machine_var_set: self.ctrl.avr.set(code, value) - else: self.vars[code] = value + else: self.set(code, value) def add_listener(self, listener): @@ -156,3 +163,10 @@ class State(object): if homing_mode == 1: return -1 # Switch min if homing_mode == 2: return 1 # Switch max return 0 # Disabled + + + def motor_home_position(self, motor): + homing_mode = self.motor_homing_mode(motor) + if homing_mode == 1: return self.vars['%dtn' % motor] # Min soft limit + if homing_mode == 2: return self.vars['%dtm' % motor] # Max soft limit + return 0 # Disabled -- 2.27.0