From 29aba0499646a0c50ec17a391c224457391c722a Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 4 Feb 2018 18:43:37 -0800 Subject: [PATCH] Continued work on new planner integration --- package.json | 2 +- scripts/rpi-chroot.sh | 4 +- src/avr/src/axis.c | 83 -------- src/avr/src/axis.h | 19 -- src/avr/src/command.c | 11 +- src/avr/src/command.h | 2 +- src/avr/src/commands.c | 15 +- src/avr/src/drv8711.c | 7 +- src/avr/src/jog.c | 35 +--- src/avr/src/pwm_spindle.c | 4 - src/avr/src/state.c | 12 +- src/avr/src/state.h | 1 - src/avr/src/usart.c | 6 + src/avr/src/vars.c | 15 +- src/avr/src/vars.def | 24 +-- src/avr/src/vars.h | 2 +- src/jade/templates/control-view.jade | 7 +- src/jade/templates/indicators.jade | 2 +- src/js/control-view.js | 4 +- src/pwr/config.h | 2 +- src/pwr/main.c | 2 - src/py/bbctrl/AVR.py | 287 ++++++++++++--------------- src/py/bbctrl/Cmd.py | 7 + src/py/bbctrl/Config.py | 4 +- src/py/bbctrl/Ctrl.py | 2 + src/py/bbctrl/Planner.py | 108 +++++++--- src/py/bbctrl/Pwr.py | 6 +- src/py/bbctrl/State.py | 158 +++++++++++++++ src/py/bbctrl/Web.py | 25 +-- src/py/bbctrl/__init__.py | 15 +- 30 files changed, 463 insertions(+), 408 deletions(-) create mode 100644 src/py/bbctrl/State.py diff --git a/package.json b/package.json index 87f60a4..46334ea 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "bbctrl", - "version": "0.2.3", + "version": "0.3.1", "homepage": "https://github.com/buildbotics/bbctrl-firmware", "license": "GPL 3+", "dependencies": { diff --git a/scripts/rpi-chroot.sh b/scripts/rpi-chroot.sh index a72d8d8..5668f57 100755 --- a/scripts/rpi-chroot.sh +++ b/scripts/rpi-chroot.sh @@ -33,9 +33,7 @@ trap cleanup EXIT # set up image as loop device losetup $LOOP_DEV "$IMAGE" -if [ ! -e ${LOOP_DEV}p1 ]; then - partprobe $LOOP_DEV -fi +partprobe $LOOP_DEV # check and fix filesystems fsck -f ${LOOP_DEV}p1 diff --git a/src/avr/src/axis.c b/src/avr/src/axis.c index 32ffa3a..77bccaa 100644 --- a/src/avr/src/axis.c +++ b/src/avr/src/axis.c @@ -42,16 +42,7 @@ int motor_map[AXES] = {-1, -1, -1, -1, -1, -1}; typedef struct { float velocity_max; // max velocity in mm/min or deg/min float accel_max; // max acceleration in mm/min^2 - float travel_max; // max work envelope for soft limits - float travel_min; // min work envelope for soft limits float jerk_max; // max jerk (Jm) in km/min^3 - float radius; // radius in mm for rotary axes - float search_velocity; // homing search velocity - float latch_velocity; // homing latch velocity - float latch_backoff; // backoff from switches prior to homing latch movement - float zero_backoff; // backoff from switches for machine zero - homing_mode_t homing_mode; - bool homed; } axis_t; @@ -122,19 +113,6 @@ float axis_get_vector_length(const float a[], const float b[]) { AXIS_VAR_SET(NAME, TYPE) -AXIS_SET(homed, bool) - -AXIS_GET(homed, bool, false) -AXIS_GET(homing_mode, homing_mode_t, HOMING_MANUAL) -AXIS_GET(radius, float, 0) -AXIS_GET(travel_min, float, 0) -AXIS_GET(travel_max, float, 0) -AXIS_GET(search_velocity, float, 0) -AXIS_GET(latch_velocity, float, 0) -AXIS_GET(zero_backoff, float, 0) -AXIS_GET(latch_backoff, float, 0) - - /// Velocity is scaled by 1,000. float axis_get_velocity_max(int axis) { int motor = axis_get_motor(axis); @@ -162,64 +140,3 @@ AXIS_VAR_GET(jerk_max, float) AXIS_VAR_SET(velocity_max, float) AXIS_VAR_SET(accel_max, float) AXIS_VAR_SET(jerk_max, float) -AXIS_VAR_SET(radius, float) -AXIS_VAR_SET(travel_min, float) -AXIS_VAR_SET(travel_max, float) -AXIS_VAR_SET(homing_mode, homing_mode_t) -AXIS_VAR_SET(search_velocity, float) -AXIS_VAR_SET(latch_velocity, float) -AXIS_VAR_SET(zero_backoff, float) -AXIS_VAR_SET(latch_backoff, float) - - -float get_homing_dir(int axis) { - switch (axes[axis].homing_mode) { - case HOMING_MANUAL: break; - case HOMING_STALL_MIN: case HOMING_SWITCH_MIN: return -1; - case HOMING_STALL_MAX: case HOMING_SWITCH_MAX: return 1; - } - return 0; -} - - -float get_home(int axis) { - switch (axes[axis].homing_mode) { - case HOMING_MANUAL: break; - case HOMING_STALL_MIN: case HOMING_SWITCH_MIN: return get_travel_min(axis); - case HOMING_STALL_MAX: case HOMING_SWITCH_MAX: return get_travel_max(axis); - } - return NAN; -} - - -static int _get_homing_switch(int axis) { - switch (axes[axis].homing_mode) { - case HOMING_MANUAL: break; - - case HOMING_STALL_MIN: case HOMING_SWITCH_MIN: - switch (axis) { - case AXIS_X: return SW_MIN_X; - case AXIS_Y: return SW_MIN_Y; - case AXIS_Z: return SW_MIN_Z; - case AXIS_A: return SW_MIN_A; - } - break; - - case HOMING_STALL_MAX: case HOMING_SWITCH_MAX: - switch (axis) { - case AXIS_X: return SW_MAX_X; - case AXIS_Y: return SW_MAX_Y; - case AXIS_Z: return SW_MAX_Z; - case AXIS_A: return SW_MAX_A; - } - break; - } - - return -1; -} - - -bool get_axis_can_home(int axis) { - return axis_is_enabled(axis) && axes[axis].homing_mode != HOMING_MANUAL && - switch_is_enabled(_get_homing_switch(axis)); -} diff --git a/src/avr/src/axis.h b/src/avr/src/axis.h index 866772d..5658cf1 100644 --- a/src/avr/src/axis.h +++ b/src/avr/src/axis.h @@ -38,15 +38,6 @@ enum { }; -typedef enum { - HOMING_MANUAL, - HOMING_STALL_MIN, - HOMING_STALL_MAX, - HOMING_SWITCH_MIN, - HOMING_SWITCH_MAX, -} homing_mode_t; - - bool axis_is_enabled(int axis); char axis_get_char(int axis); int axis_get_id(char axis); @@ -57,13 +48,3 @@ float axis_get_vector_length(const float a[], const float b[]); float axis_get_velocity_max(int axis); float axis_get_accel_max(int axis); float axis_get_jerk_max(int axis); -bool axis_get_homed(int axis); -void axis_set_homed(int axis, bool homed); -homing_mode_t axis_get_homing_mode(int axis); -float axis_get_radius(int axis); -float axis_get_travel_min(int axis); -float axis_get_travel_max(int axis); -float axis_get_search_velocity(int axis); -float axis_get_latch_velocity(int axis); -float axis_get_zero_backoff(int axis); -float axis_get_latch_backoff(int axis); diff --git a/src/avr/src/command.c b/src/avr/src/command.c index c8b98c6..2faeb63 100644 --- a/src/avr/src/command.c +++ b/src/avr/src/command.c @@ -91,7 +91,7 @@ static struct { // Help & name #define CMD(CODE, NAME, SYNC, HELP) \ static const char command_##NAME##_name[] PROGMEM = #NAME; \ - static const char command_##NAME##_help[] PROGMEM = #HELP; + static const char command_##NAME##_help[] PROGMEM = HELP; #include "command.def" #undef CMD @@ -153,10 +153,13 @@ bool command_is_active() {return cmd.active;} unsigned command_get_count() {return cmd.count;} -void command_print_help() { - static const char fmt[] PROGMEM = " %c %-12"PRPSTR" %"PRPSTR"\n"; +void command_print_json() { + bool first = true; + static const char fmt[] PROGMEM = + "\"%c\":{\"name\":\"%"PRPSTR"\",\"help\":\"%"PRPSTR"\"}"; -#define CMD(CODE, NAME, SYNC, HELP) \ +#define CMD(CODE, NAME, SYNC, HELP) \ + if (first) first = false; else putchar(','); \ printf_P(fmt, CODE, command_##NAME##_name, command_##NAME##_help); #include "command.def" diff --git a/src/avr/src/command.h b/src/avr/src/command.h index 4a93d1f..1b20837 100644 --- a/src/avr/src/command.h +++ b/src/avr/src/command.h @@ -44,7 +44,7 @@ typedef enum { void command_init(); bool command_is_active(); unsigned command_get_count(); -void command_print_help(); +void command_print_json(); void command_flush_queue(); void command_push(char code, void *data); bool command_callback(); diff --git a/src/avr/src/commands.c b/src/avr/src/commands.c index dc72a52..ee10a8f 100644 --- a/src/avr/src/commands.c +++ b/src/avr/src/commands.c @@ -59,16 +59,11 @@ void command_out_exec(void *data) {} stat_t command_help(char *cmd) { - puts_P(PSTR("\nLine editing:\n" - " ENTER Submit current command line.\n" - " BS Backspace, delete last character.\n" - " CTRL-X Cancel current line entry.")); - - puts_P(PSTR("\nCommands:")); - command_print_help(); - - puts_P(PSTR("\nVariables:")); - vars_print_help(); + printf_P(PSTR("\n{\"commands\":{")); + command_print_json(); + printf_P(PSTR("},\"variables\":{")); + vars_print_json(); + printf_P(PSTR("}}\n")); return STAT_OK; } diff --git a/src/avr/src/drv8711.c b/src/avr/src/drv8711.c index e6760a3..88ebff6 100644 --- a/src/avr/src/drv8711.c +++ b/src/avr/src/drv8711.c @@ -101,8 +101,8 @@ static void _current_set(current_t *c, float current) { c->current = current; float torque_over_gain = current * CURRENT_SENSE_RESISTOR / CURRENT_SENSE_REF; - float gain = 0; + if (torque_over_gain < 1.0 / 40) { c->isgain = DRV8711_CTRL_ISGAIN_40; gain = 40; @@ -211,9 +211,12 @@ static uint8_t _spi_next_command(uint8_t cmd) { break; case DRV8711_CTRL_REG: // Set microsteps + // NOTE, we disable the driver if it's not active. Otherwise, the chip + // gets hot if when idling with the driver enabled. *command = (*command & 0xfc86) | _driver_get_isgain(driver) | (drv->mode << 3) | - (_driver_get_enabled(driver) ? DRV8711_CTRL_ENBL_bm : 0); + ((_driver_get_enabled(driver) && _driver_get_torque(driver)) ? + DRV8711_CTRL_ENBL_bm : 0); break; default: break; diff --git a/src/avr/src/jog.c b/src/avr/src/jog.c index eaeed05..a5faacd 100644 --- a/src/avr/src/jog.c +++ b/src/avr/src/jog.c @@ -117,7 +117,6 @@ static float _compute_deccel_dist(float vel, float accel, float jerk) { } -#else // Analytical version static float _compute_deccel_dist(float vel, float accel, float jerk) { float dist = 0; @@ -151,31 +150,7 @@ static float _compute_deccel_dist(float vel, float accel, float jerk) { #endif -static float _limit_position(int axis, float p) { - jog_axis_t *a = &jr.axes[axis]; - - // Check if axis is homed - if (!axis_get_homed(axis)) return p; - - // Check if limits are enabled - float min = axis_get_travel_min(axis); - float max = axis_get_travel_max(axis); - if (min == max) return p; - - if (a->velocity < 0 && p < min) { - a->velocity = 0; - return min; - } - - if (0 < a->velocity && max < p) { - a->velocity = 0; - return max; - } - - return p; -} - - +#if 0 static bool _soft_limit(int axis, float V, float A) { jog_axis_t *a = &jr.axes[axis]; @@ -197,6 +172,7 @@ static bool _soft_limit(int axis, float V, float A) { return false; } +#endif static float _compute_axis_velocity(int axis) { @@ -206,7 +182,7 @@ static float _compute_axis_velocity(int axis) { float Vt = fabs(a->target); // Apply soft limits - if (_soft_limit(axis, V, a->accel)) Vt = MIN_VELOCITY; + //if (_soft_limit(axis, V, a->accel)) Vt = MIN_VELOCITY; // Check if velocity has reached its target if (fp_EQ(V, Vt)) { @@ -251,6 +227,7 @@ stat_t jog_exec() { // Check if we are done if (jr.done) { + exec_set_velocity(0); exec_set_cb(0); jr.active = false; @@ -260,10 +237,8 @@ stat_t jog_exec() { // Compute target from velocity float target[AXES]; exec_get_position(target); - for (int axis = 0; axis < AXES; axis++) { + for (int axis = 0; axis < AXES; axis++) target[axis] += jr.axes[axis].velocity * SEGMENT_TIME; - target[axis] = _limit_position(axis, target[axis]); - } // Set velocity and target exec_set_velocity(sqrt(velocity_sqr)); diff --git a/src/avr/src/pwm_spindle.c b/src/avr/src/pwm_spindle.c index dc9a30e..010aba9 100644 --- a/src/avr/src/pwm_spindle.c +++ b/src/avr/src/pwm_spindle.c @@ -143,10 +143,6 @@ float get_spin_min_duty() {return spindle.min_duty * 100;} void set_spin_min_duty(float value) {spindle.min_duty = value / 100;} float get_spin_max_duty() {return spindle.max_duty * 100;} void set_spin_max_duty(float value) {spindle.max_duty = value / 100;} -float get_spin_up() {return 0;} // TODO -void set_spin_up(float value) {} // TODO -float get_spin_down() {return 0;} // TODO -void set_spin_down(float value) {} // TODO uint16_t get_spin_freq() {return spindle.freq;} void set_spin_freq(uint16_t value) {spindle.freq = value;} bool get_pwm_invert() {return spindle.pwm_invert;} diff --git a/src/avr/src/state.c b/src/avr/src/state.c index 1532f52..4d36495 100644 --- a/src/avr/src/state.c +++ b/src/avr/src/state.c @@ -91,7 +91,7 @@ static void _set_state(state_t state) { } -void state_set_hold_reason(hold_reason_t reason) { +static void _set_hold_reason(hold_reason_t reason) { if (s.hold_reason == reason) return; // No change s.hold_reason = reason; report_request(); @@ -110,7 +110,7 @@ bool state_is_quiescent() { void state_seek_hold() { if (state_get() == STATE_RUNNING) { - state_set_hold_reason(HOLD_REASON_SEEK); + _set_hold_reason(HOLD_REASON_SEEK); _set_state(STATE_STOPPING); } } @@ -121,7 +121,7 @@ void state_holding() {_set_state(STATE_HOLDING);} void state_optional_pause() { if (s.optional_pause_requested) { - state_set_hold_reason(HOLD_REASON_USER_PAUSE); + _set_hold_reason(HOLD_REASON_USER_PAUSE); state_holding(); } } @@ -146,21 +146,21 @@ void state_estop() {_set_state(STATE_ESTOPPED);} * * A flush request received: * - during motion is ignored but not reset - * - during a pause is deferred until the feedpause enters HOLDING state. + * - during a hold is deferred until HOLDING state is entered. * I.e. until deceleration is complete. * - when stopped or holding and the exec is not busy, is honored * * A start request received: * - during motion is ignored and reset - * - during a pause is deferred until the feedpause enters HOLDING state. + * - during a hold is deferred until HOLDING state is entered. * I.e. until deceleration is complete. If a queue flush request is also * present the queue flush is done first * - when stopped is honored and starts to run anything in the queue */ void state_callback() { if (s.pause_requested || s.flush_requested) { + if (s.pause_requested) _set_hold_reason(HOLD_REASON_USER_PAUSE); s.pause_requested = false; - state_set_hold_reason(HOLD_REASON_USER_PAUSE); if (state_get() == STATE_RUNNING) _set_state(STATE_STOPPING); } diff --git a/src/avr/src/state.h b/src/avr/src/state.h index 27d4c04..fd71da1 100644 --- a/src/avr/src/state.h +++ b/src/avr/src/state.h @@ -56,7 +56,6 @@ PGM_P state_get_pgmstr(state_t state); PGM_P state_get_hold_reason_pgmstr(hold_reason_t reason); state_t state_get(); -void state_set_hold_reason(hold_reason_t reason); bool state_is_flushing(); bool state_is_resuming(); diff --git a/src/avr/src/usart.c b/src/avr/src/usart.c index 61d3d91..89373fb 100644 --- a/src/avr/src/usart.c +++ b/src/avr/src/usart.c @@ -191,6 +191,12 @@ int8_t usart_getc() { } +/*** Line editing features: + * + * ENTER Submit current command line. + * BS Backspace, delete last character. + * CTRL-X Cancel current line entry. + */ char *usart_readline() { static char line[INPUT_BUFFER_LEN]; static int i = 0; diff --git a/src/avr/src/vars.c b/src/avr/src/vars.c index 8ccc776..4f2fc70 100644 --- a/src/avr/src/vars.c +++ b/src/avr/src/vars.c @@ -327,15 +327,22 @@ float vars_get_number(const char *name) { } -void vars_print_help() { +void vars_print_json() { + bool first = true; static const char fmt[] PROGMEM = - " $%-5s %-20"PRPSTR" %-16"PRPSTR" %"PRPSTR"\n"; + "\"%s\":{\"name\":\"%"PRPSTR"\",\"type\":\"%"PRPSTR"\"," + "\"help\":\"%"PRPSTR"\""; + static const char index_fmt[] PROGMEM = ",\"index\":\"%s\""; // Save and disable watchdog uint8_t wd_state = hw_disable_watchdog(); -#define VAR(NAME, CODE, TYPE, ...) \ - printf_P(fmt, #CODE, NAME##_name, type_get_##TYPE##_name_pgm(), NAME##_help); +#define VAR(NAME, CODE, TYPE, INDEX, ...) \ + if (first) first = false; else putchar(','); \ + printf_P(fmt, #CODE, NAME##_name, type_get_##TYPE##_name_pgm(), \ + NAME##_help); \ + IF(INDEX)(printf_P(index_fmt, INDEX##_LABEL)); \ + putchar('}'); #include "vars.def" #undef VAR diff --git a/src/avr/src/vars.def b/src/avr/src/vars.def index 020ec20..0b7291d 100644 --- a/src/avr/src/vars.def +++ b/src/avr/src/vars.def @@ -25,7 +25,7 @@ \******************************************************************************/ -#define AXES_LABEL "xyzabcuvw" +#define AXES_LABEL "xyzabc" #define MOTORS_LABEL "0123" #define OUTS_LABEL "ed12f" @@ -47,16 +47,13 @@ VAR(status_strings, ds, flags, MOTORS, 0, 1, "Motor driver status") VAR(encoder, en, s32, MOTORS, 0, 0, "Motor encoder") VAR(error, ee, s32, MOTORS, 0, 0, "Motor position error") -VAR(motor_fault, fa, bool, 0, 0, 1, "Motor fault status") - VAR(velocity_max, vm, f32, MOTORS, 1, 1, "Maxium vel in mm/min") VAR(accel_max, am, f32, MOTORS, 1, 1, "Maxium accel in mm/min^2") VAR(jerk_max, jm, f32, MOTORS, 1, 1, "Maxium jerk in mm/min^3") -VAR(radius, ra, f32, MOTORS, 1, 1, "Axis radius or zero") + +VAR(motor_fault, fa, bool, 0, 0, 1, "Motor fault status") // Switches -VAR(travel_min, tn, f32, MOTORS, 1, 1, "Minimum soft limit") -VAR(travel_max, tm, f32, MOTORS, 1, 1, "Maximum soft limit") VAR(min_sw_mode, ls, u8, MOTORS, 1, 1, "Minimum switch mode") VAR(max_sw_mode, xs, u8, MOTORS, 1, 1, "Maximum switch mode") VAR(estop_mode, et, u8, 0, 1, 1, "Estop switch mode") @@ -66,19 +63,8 @@ VAR(max_switch, xw, u8, MOTORS, 0, 1, "Maximum switch state") VAR(estop_switch, ew, u8, 0, 0, 1, "Estop switch state") VAR(probe_switch, pw, u8, 0, 0, 1, "Probe switch state") -// Homing -VAR(homing_mode, ho, u8, MOTORS, 1, 1, "Homing type") -VAR(homing_dir, hd, f32, MOTORS, 0, 1, "Homing direction") -VAR(home, hp, f32, MOTORS, 0, 1, "Home position") -VAR(homed, h, bool, MOTORS, 1, 1, "True if axis is homed") -VAR(search_velocity, sv, f32, MOTORS, 1, 1, "Homing search velocity") -VAR(latch_velocity, lv, f32, MOTORS, 1, 1, "Homing latch velocity") -VAR(latch_backoff, lb, f32, MOTORS, 1, 1, "Homing latch backoff") -VAR(zero_backoff, zb, f32, MOTORS, 1, 1, "Homing zero backoff") - // Axis -VAR(axis_position, p, f32, AXES, 1, 1, "Axis position") -VAR(axis_can_home, ch, bool, AXES, 0, 1, "Axis can home") +VAR(axis_position, p, f32, AXES, 1, 1, "Axis position") // Outputs VAR(output_active, oa, bool, OUTS, 1, 1, "Output pin active") @@ -92,8 +78,6 @@ VAR(max_spin, sx, f32, 0, 1, 1, "Maximum spindle speed") VAR(min_spin, sm, f32, 0, 1, 1, "Minimum spindle speed") VAR(spin_min_duty, nd, f32, 0, 1, 1, "Minimum PWM duty cycle") VAR(spin_max_duty, md, f32, 0, 1, 1, "Maximum PWM duty cycle") -VAR(spin_up, su, f32, 0, 1, 1, "Spin up velocity") -VAR(spin_down, sd, f32, 0, 1, 1, "Spin down velocity") VAR(spin_freq, sf, u16, 0, 1, 1, "Spindle PWM frequency") // PWM spindle diff --git a/src/avr/src/vars.h b/src/avr/src/vars.h index 517a32e..1c8b8f3 100644 --- a/src/avr/src/vars.h +++ b/src/avr/src/vars.h @@ -43,4 +43,4 @@ void vars_report_var(const char *code, bool enable); stat_t vars_print(const char *name); stat_t vars_set(const char *name, const char *value); float vars_get_number(const char *name); -void vars_print_help(); +void vars_print_json(); diff --git a/src/jade/templates/control-view.jade b/src/jade/templates/control-view.jade index cacc72d..4cfcad3 100644 --- a/src/jade/templates/control-view.jade +++ b/src/jade/templates/control-view.jade @@ -12,9 +12,10 @@ script#control-view-template(type="text/x-template") tr.axis(:class="{'homed': is_homed('#{axis}'), 'axis-#{axis}': true}", v-if="enabled('#{axis}')") th.name #{axis} - td.position {{state.#{axis}w || 0 | fixed 3}} + td.position + | {{(state.#{axis}p + get_offset('#{axis}')) || 0 | fixed 3}} td.absolute {{state.#{axis}p || 0 | fixed 3}} - td.offset {{(state.#{axis}w - state.#{axis}p) || 0 | fixed 3}} + td.offset {{get_offset('#{axis}') | fixed 3}} th.actions button.pure-button(:disabled="state.x != 'READY'", title="Set {{'#{axis}' | upper}} axis position.", @@ -157,7 +158,7 @@ script#control-view-template(type="text/x-template") label(for="tab2") MDI input#tab3(type="radio", name="tabs") - label(for="tab3") Manual + label(for="tab3") Jog input#tab4(type="radio", name="tabs") label(for="tab4") Console diff --git a/src/jade/templates/indicators.jade b/src/jade/templates/indicators.jade index 7115517..e0e4520 100644 --- a/src/jade/templates/indicators.jade +++ b/src/jade/templates/indicators.jade @@ -111,4 +111,4 @@ script#indicators-template(type="text/x-template") td .fa.fa-circle-o.logic-tri - th Tristated + th Tristated / Disabled diff --git a/src/js/control-view.js b/src/js/control-view.js index 96107fc..ff7e19e 100644 --- a/src/js/control-view.js +++ b/src/js/control-view.js @@ -259,9 +259,7 @@ module.exports = { }, - get_offset: function (axis) { - return this.state[axis + 'w'] - this.state[axis + 'p']; - }, + get_offset: function (axis) {return this.state['offset_' + axis] || 0}, set_position: function (axis, position) { diff --git a/src/pwr/config.h b/src/pwr/config.h index 34a5ff1..0ae9012 100644 --- a/src/pwr/config.h +++ b/src/pwr/config.h @@ -92,7 +92,7 @@ enum { #define SHUNT_MAX_V 3 #define VOLTAGE_REF 1.1 -#define VOLTAGE_REF_R1 34800 // TODO v10 will have 37.4k +#define VOLTAGE_REF_R1 37400 #define VOLTAGE_REF_R2 1000 #define CURRENT_REF_MUL 1970 diff --git a/src/pwr/main.c b/src/pwr/main.c index 90c766f..77b1692 100644 --- a/src/pwr/main.c +++ b/src/pwr/main.c @@ -165,8 +165,6 @@ static void measure_nominal_voltage() { if (vnom < VOLTAGE_MIN) v = vin; else v = vnom * (1 - VOLTAGE_EXP) + vin * VOLTAGE_EXP; - if (36 < v) v = 36; // TODO remove this when R27 is updated - vnom = v; } diff --git a/src/py/bbctrl/AVR.py b/src/py/bbctrl/AVR.py index f96fdc8..3ba67e2 100644 --- a/src/py/bbctrl/AVR.py +++ b/src/py/bbctrl/AVR.py @@ -11,46 +11,40 @@ import bbctrl.Cmd as Cmd log = logging.getLogger('AVR') -machine_state_vars = ''' - xp yp zp ap bp cp u s f t fm pa cs ao pc dm ad fo so mc fc -'''.split() - -# Axis homing procedure -# - Set axis unhomed -# - Find switch -# - Backoff switch -# - Find switch more accurately -# - Backoff to machine zero -# - Set axis home position + +# Axis homing procedure: +# +# Set 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 + 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 %(axis)s[#<%(axis)s.hd> * -#<%(axis)s.zb> + #<%(axis)sp>] - G28.3 %(axis)s[#<%(axis)s.hp>] + 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>] ''' -# Set axis unhomed -# Seek closed (home_dir * (travel_max - travel_min) * 1.5) at search_vel -# 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 + switched_position)) -# Set axis homed and home_position class AVR(): def __init__(self, ctrl): self.ctrl = ctrl - self.vars = {} - self.stream = None self.queue = deque() self.in_buf = '' self.command = None + self.lcd_page = ctrl.lcd.add_new_page() self.install_page = True + ctrl.state.add_listener(lambda x: self._update_state(x)) + try: self.sp = serial.Serial(ctrl.args.serial, ctrl.args.baud, rtscts = 1, timeout = 0, write_timeout = 0) @@ -61,38 +55,13 @@ class AVR(): log.warning('Failed to open serial port: %s', e) if self.sp is not None: - ctrl.ioloop.add_handler(self.sp, self.serial_handler, + ctrl.ioloop.add_handler(self.sp, self._serial_handler, ctrl.ioloop.READ) self.i2c_addr = ctrl.args.avr_addr - def _start_sending_gcode(self, path): - if self.stream is not None: - raise Exception('Busy, cannot start new GCode file') - - log.info('Running ' + path) - self.stream = bbctrl.Planner(self.ctrl, path) - self.set_write(True) - - - def _stop_sending_gcode(self): - if self.stream is not None: - self.stream.reset() - self.stream = None - - - def connect(self): - try: - # Reset AVR communication - self.stop(); - self.ctrl.config.config_avr() - self._restore_machine_state() - - except Exception as e: - log.warning('Connect failed: %s', e) - self.ctrl.ioloop.call_later(1, self.connect) - + def _is_busy(self): return self.ctrl.planner.is_running() def _i2c_command(self, cmd, byte = None, word = None): log.info('I2C: ' + cmd) @@ -117,27 +86,18 @@ class AVR(): raise - def _restore_machine_state(self): - for var in machine_state_vars: - if var in self.vars: - value = self.vars[var] - if isinstance(value, str): value = '"' + value + '"' - if isinstance(value, bool): value = int(value) - - self.set('', var, value) - - self.queue_command('$$') # Refresh all vars, must come after above + 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) - def report(self): self._i2c_command(Cmd.REPORT) + def _stop_sending_gcode(self): self.ctrl.planner.reset() - def load_next_command(self, cmd): - log.info('< ' + json.dumps(cmd).strip('"')) - self.command = bytes(cmd.strip() + '\n', 'utf-8') - - def set_write(self, enable): + def _set_write(self, enable): if self.sp is None: return flags = self.ctrl.ioloop.READ @@ -145,22 +105,32 @@ class AVR(): self.ctrl.ioloop.update_handler(self.sp, flags) - def serial_handler(self, fd, events): + def _load_next_command(self, cmd): + log.info('< ' + json.dumps(cmd).strip('"')) + self.command = bytes(cmd.strip() + '\n', 'utf-8') + + + def _queue_command(self, cmd): + self.queue.append(cmd) + self._set_write(True) + + + def _serial_handler(self, fd, events): try: - if self.ctrl.ioloop.READ & events: self.serial_read() - if self.ctrl.ioloop.WRITE & events: self.serial_write() + if self.ctrl.ioloop.READ & events: self._serial_read() + if self.ctrl.ioloop.WRITE & events: self._serial_write() except Exception as e: log.error('Serial handler error: %s', traceback.format_exc()) - def serial_write(self): + def _serial_write(self): # Finish writing current command if self.command is not None: try: count = self.sp.write(self.command) except Exception as e: - self.set_write(False) + self._set_write(False) raise e self.command = self.command[count:] @@ -168,20 +138,20 @@ class AVR(): self.command = None # Load next command from queue - if len(self.queue): self.load_next_command(self.queue.popleft()) + if len(self.queue): self._load_next_command(self.queue.popleft()) # Load next GCode command, if running or paused - elif self.stream is not None: - cmd = self.stream.next() + elif self._is_busy(): + cmd = self.ctrl.planner.next() - if cmd is None: self.set_write(False) - else: self.load_next_command(cmd) + if cmd is None: self._set_write(False) + else: self._load_next_command(cmd) # Else stop writing - else: self.set_write(False) + else: self._set_write(False) - def serial_read(self): + def _serial_read(self): try: data = self.sp.read(self.sp.in_waiting) self.in_buf += data.decode('utf-8') @@ -208,119 +178,121 @@ class AVR(): log.error('%s, data: %s', e, line) continue - update.update(msg) - log.debug(line) + if 'variables' in msg: + try: + self.ctrl.state.machine_cmds_and_vars(msg) + self._queue_command('D') # Refresh all vars + + except Exception as e: + log.warning('AVR reload failed: %s', + traceback.format_exc()) + self.ctrl.ioloop.call_later(1, self.connect) - # Don't overwrite duplicate `msg` - if 'msg' in msg: break + continue + + update.update(msg) if update: if 'firmware' in update: log.error('AVR rebooted') self.connect() - if 'x' in update and update['x'] == 'ESTOPPED': - self._stop_sending_gcode() + self.ctrl.state.update(update) - self.vars.update(update) + # Must be after AVR vars have loaded + if self.install_page: + self.install_page = False + self.ctrl.lcd.set_current_page(self.lcd_page.id) - if self.stream is not None: - self.stream.update(update) - if not self.stream.is_running(): - self.stream = None - try: - self._update_lcd(update) + def _update_state(self, update): + if 'x' in update and update['x'] == 'ESTOPPED': + self._stop_sending_gcode() - if self.install_page: - self.install_page = False - self.ctrl.lcd.set_current_page(self.lcd_page.id) + self._update_lcd(update) - except Exception as e: - log.error('Updating LCD: %s', e) - try: - self.ctrl.web.broadcast(update) - except Exception as e: - log.error('Updating Web: %s', e) - - - def _find_motor(self, axis): - for motor in range(6): - if not ('%dan' % motor) in self.vars: continue - motor_axis = 'xyzabc'[self.vars['%dan' % motor]] - if motor_axis == axis.lower() and self.vars['%dpm' % motor]: - return motor - - - def _is_axis_homed(self, axis): - motor = self._find_motor(axis) - if axis is None: return False - return self.vars['%dh' % motor] - - - def _update_lcd(self, msg): - if 'x' in msg: self.lcd_page.text('%-9s' % self.vars['x'], 0, 0) + def _update_lcd(self, update): + if 'x' in update: + self.lcd_page.text('%-9s' % self.ctrl.state.get('x'), 0, 0) # Show enabled axes row = 0 for axis in 'xyzabc': - motor = self._find_motor(axis) + motor = self.ctrl.state.find_motor(axis) if motor is not None: - if (axis + 'p') in msg: + if (axis + 'p') in update: self.lcd_page.text('% 10.3f%s' % ( - msg[axis + 'p'], axis.upper()), 9, row) + update[axis + 'p'], axis.upper()), 9, row) row += 1 - if 't' in msg: self.lcd_page.text('%2uT' % msg['t'], 6, 1) - if 'u' in msg: self.lcd_page.text('%-6s' % msg['u'], 0, 1) - if 'f' in msg: self.lcd_page.text('%8uF' % msg['f'], 0, 2) - if 's' in msg: self.lcd_page.text('%8dS' % msg['s'], 0, 3) + # Show tool, units, feed and speed + # TODO Units not in state + if 't' in update: self.lcd_page.text('%2uT' % update['t'], 6, 1) + if 'u' in update: self.lcd_page.text('%-6s' % update['u'], 0, 1) + if 'f' in update: self.lcd_page.text('%8uF' % update['f'], 0, 2) + if 's' in update: self.lcd_page.text('%8dS' % update['s'], 0, 3) - def queue_command(self, cmd): - self.queue.append(cmd) - self.set_write(True) + def connect(self): + try: + # Reset AVR communication + self.stop(); + self._queue_command('h') # Load AVR commands and variables + + except Exception as e: + log.warning('Connect failed: %s', e) + self.ctrl.ioloop.call_later(1, self.connect) + + + def set(self, code, value): + self._queue_command('${}={}'.format(code, value)) def mdi(self, cmd): - if self.stream is not None: - raise Exception('Busy, cannot queue MDI command') + 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) + + elif len(cmd) and cmd[0] == '\\': self._queue_command(cmd[1:]) - self.queue_command(cmd) + else: + self.ctrl.planner.mdi(cmd) + self._set_write(True) def jog(self, axes): - if self.stream is not None: raise Exception('Busy, cannot jog') + if self._is_busy(): raise Exception('Busy, cannot jog') _axes = {} for i in range(len(axes)): _axes["xyzabc"[i]] = axes[i] - self.queue_command(Cmd.jog(_axes)) - - - def set(self, index, code, value): - self.queue_command('${}{}={}'.format(index, code, value)) + self._queue_command(Cmd.jog(_axes)) def home(self, axis, position = None): - if self.stream is not None: raise Exception('Busy, cannot home') - raise Exception('NYI') # TODO + if self._is_busy(): raise Exception('Busy, cannot home') if position is not None: - self.queue_command('G28.3 %c%f' % (axis, position)) + self.ctrl.planner.mdi('G28.3 %c%f' % (axis, position)) else: if axis is None: axes = 'zxyabc' # TODO This should be configurable else: axes = '%c' % axis for axis in axes: - if not self.vars.get('%sch' % axis, 0): continue + if not self.ctrl.state.axis_can_home(axis): continue + log.info('Homing %s axis' % axis) gcode = axis_homing_procedure % {'axis': axis} - for line in gcode.splitlines(): - self.queue_command(line.strip()) + self.ctrl.planner.mdi(gcode) + self._set_write(True) def estop(self): self._i2c_command(Cmd.ESTOP) @@ -328,13 +300,14 @@ class AVR(): def start(self, path): - if self.stream is not None: raise Exception('Busy, cannot start file') + if self._is_busy(): raise Exception('Busy, cannot start file') if path: self._start_sending_gcode(path) def step(self, path): self._i2c_command(Cmd.STEP) - if self.stream is None and path and self.vars.get('x', '') == 'READY': + if not self._is_busy() and path and \ + self.ctrl.state.get('x', '') == 'READY': self._start_sending_gcode(path) @@ -342,19 +315,20 @@ class AVR(): self._i2c_command(Cmd.FLUSH) self._stop_sending_gcode() # Resume processing once current queue of GCode commands has flushed - self.queue_command(Cmd.RESUME) + self._queue_command(Cmd.RESUME) def pause(self): self._i2c_command(Cmd.PAUSE, byte = 0) def unpause(self): - if self.vars.get('x', '') != 'HOLDING' or self.stream is None: return + if self.ctrl.state.get('x', '') != 'HOLDING' or not self._is_busy(): + return self._i2c_command(Cmd.FLUSH) - self.queue_command(Cmd.RESUME) - self.stream.restart() - self.set_write(True) + self._queue_command(Cmd.RESUME) + self.ctrl.planner.restart() + self._set_write(True) self._i2c_command(Cmd.UNPAUSE) @@ -362,8 +336,9 @@ class AVR(): def set_position(self, axis, position): - if self.stream is not None: raise Exception('Busy, cannot set position') - if self._is_axis_homed('%c' % axis): + 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)) - else: self.queue_command('$%cp=%f' % (axis, position)) + self._queue_command('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 index 64d5148..056a81c 100644 --- a/src/py/bbctrl/Cmd.py +++ b/src/py/bbctrl/Cmd.py @@ -65,6 +65,13 @@ def line(id, target, exitVel, maxAccel, maxJerk, times): def tool(tool): return '#t=%d' % tool def speed(speed): return '#s=:' + encode_float(speed) + +def output(port, value): + if port == 'mist': return '#1oa=' + ('1' if value else '0') + if port == 'flood': return '#2oa=' + ('1' if value else '0') + raise Exception('Unsupported output "%s"' % port) + + def dwell(seconds): return 'd' + encode_float(seconds) def pause(optional = False): 'P' + ('1' if optional else '0') def jog(axes): return 'j' + encode_axes(axes) diff --git a/src/py/bbctrl/Config.py b/src/py/bbctrl/Config.py index c377cae..2e63861 100644 --- a/src/py/bbctrl/Config.py +++ b/src/py/bbctrl/Config.py @@ -82,7 +82,7 @@ class Config(object): elif spec['type'] == 'bool': value = 1 if value else 0 elif spec['type'] == 'percent': value /= 100.0 - self.ctrl.avr.set(index, spec['code'], value) + self.ctrl.state.config(str(index) + spec['code'], value) def encode_category(self, index, config, category, with_defaults): @@ -110,4 +110,4 @@ class Config(object): with_defaults) - def config_avr(self): self.update(self.load(), True) + def reload(self): self.update(self.load(), True) diff --git a/src/py/bbctrl/Ctrl.py b/src/py/bbctrl/Ctrl.py index 5e2749e..bb2cbf9 100644 --- a/src/py/bbctrl/Ctrl.py +++ b/src/py/bbctrl/Ctrl.py @@ -31,6 +31,8 @@ class Ctrl(object): self.args = args self.ioloop = ioloop + self.state = bbctrl.State(self) + self.planner = bbctrl.Planner(self) self.i2c = bbctrl.I2C(args.i2c_port) self.config = bbctrl.Config(self) self.lcd = bbctrl.LCD(self) diff --git a/src/py/bbctrl/Planner.py b/src/py/bbctrl/Planner.py index 658ed8c..d43d8ae 100644 --- a/src/py/bbctrl/Planner.py +++ b/src/py/bbctrl/Planner.py @@ -8,19 +8,27 @@ log = logging.getLogger('Planner') class Planner(): - def __init__(self, ctrl, path): + def __init__(self, ctrl): self.ctrl = ctrl - self.path = path self.lastID = -1 self.done = False - vars = ctrl.avr.vars + ctrl.state.add_listener(lambda x: self.update(x)) + + self.reset() + + + def is_running(self): return self.planner.is_running() + + + def get_config(self): + state = self.ctrl.state # Axis mapping for enabled motors axis2motor = {} for i in range(3): - if vars.get('%dpm' % i, False): - axis = 'xyzabc'[int(vars.get('%dan' % i))] + if state.get('%dpm' % i, False): + axis = 'xyzabc'[int(state.get('%dan' % i))] axis2motor[axis] = i def get_vector(name, scale = 1): @@ -28,7 +36,7 @@ class Planner(): for axis in 'xyzabc': if axis in axis2motor: motor = axis2motor[axis] - value = vars.get(str(motor) + name, None) + value = state.get(str(motor) + name, None) if value is not None: v[axis] = value * scale return v @@ -37,23 +45,16 @@ class Planner(): start = {} for axis in 'xyzabc': if not axis in axis2motor: continue - value = vars.get(axis + 'p', None) + value = state.get(axis + 'p', None) if value is not None: start[axis] = value - # Planner config - self.config = { + return { "start": start, "max-vel": get_vector('vm', 1000), "max-accel": get_vector('am', 1000), "max-jerk": get_vector('jm', 1000000), # TODO junction deviation & accel } - log.info('Planner config: ' + json.dumps(self.config)) - - self.reset() - - - def is_running(self): return self.planner.is_running() def update(self, update): @@ -61,26 +62,63 @@ class Planner(): id = update['id'] if id: self.planner.release(id - 1) + if update.get('x', '') == 'HOLDING' and \ + self.ctrl.state.get('pr', '') == 'Switch found': + self.ctrl.avr.unpause() + def restart(self): - vars = self.ctrl.avr.vars - id = vars['id'] + state = self.ctrl.state + id = state.get('id') position = {} for axis in 'xyzabc': - if (axis + 'p') in vars: - position[axis] = vars[axis + 'p'] + 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) self.done = False - def reset(self): - self.planner = gplan.Planner(self.config) - self.planner.load('upload' + self.path) + def get_var(self, name): + value = 0 + if len(name) and name[0] == '_': + value = self.ctrl.state.get(name[1:], 0) + + log.info('Get: %s=%s' % (name, value)) + return value + + + def log(self, line): + line = line.strip() + 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:]) + + + def mdi(self, cmd): + self.planner.set_config(self.get_config()) + self.planner.mdi(cmd) + self.done = False + + + def load(self, path): + self.planner.set_config(self.get_config()) + 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) + + def encode(self, block): type = block['type'] @@ -89,9 +127,21 @@ class Planner(): block['max-accel'], block['max-jerk'], block['times']) - if type == 'ln': return Cmd.line_number(block['line']) - if type == 'tool': return Cmd.tool(block['tool']) - if type == 'speed': return Cmd.speed(block['speed']) + if type == 'set': + name, value = block['name'], block['value'] + + if name == 'line': return Cmd.line_number(value) + if name == 'tool': return Cmd.tool(value) + if name == 'speed': return Cmd.speed(value) + + if len(name) and name[0] == '_': + self.ctrl.state.set(name[1:], value) + + return + + if type == 'output': + return Cmd.output(block['port'], int(float(block['value']))) + if type == 'dwell': return Cmd.dwell(block['seconds']) if type == 'pause': return Cmd.pause(block['optional']) if type == 'seek': @@ -100,11 +150,15 @@ class Planner(): raise Exception('Unknown planner type "%s"' % type) + def has_move(self): return self.planner.has_more() + + def next(self): - if self.planner.has_more(): + while self.planner.has_more(): cmd = self.planner.next() self.lastID = cmd['id'] - return self.encode(cmd) + cmd = self.encode(cmd) + if cmd is not None: return cmd if not self.done: self.done = True diff --git a/src/py/bbctrl/Pwr.py b/src/py/bbctrl/Pwr.py index 6cb99ea..d1659aa 100644 --- a/src/py/bbctrl/Pwr.py +++ b/src/py/bbctrl/Pwr.py @@ -38,8 +38,10 @@ class Pwr(): if i == TEMP_REG: value -= 273 else: value /= 100.0 + key = ['temp', 'vin', 'vout', 'motor', 'load1', 'load2'][i] + self.ctrl.state.set(key, value) + if self.regs[i] != value: - key = ['temp', 'vin', 'vout', 'motor', 'load1', 'load2'][i] update[key] = value self.regs[i] = value @@ -56,6 +58,6 @@ class Pwr(): self.lcd_page.text('%5.1fA Ld1' % self.regs[LOAD1_REG], 10, 1) self.lcd_page.text('%5.1fA Ld2' % self.regs[LOAD2_REG], 10, 2) - if len(update): self.ctrl.web.broadcast(update) + if len(update): self.ctrl.state.update(update) self.ctrl.ioloop.call_later(0.25, self._update) diff --git a/src/py/bbctrl/State.py b/src/py/bbctrl/State.py new file mode 100644 index 0000000..81d7eff --- /dev/null +++ b/src/py/bbctrl/State.py @@ -0,0 +1,158 @@ +import logging +import bbctrl + + +log = logging.getLogger('State') + + +machine_state_vars = 'xp yp zp ap bp cp t fo so'.split() + + +class State(object): + def __init__(self, ctrl): + self.ctrl = ctrl + self.vars = {} + self.callbacks = {} + self.changes = {} + self.next_id = 1 + self.listeners = {} + self.timeout = None + self.machine_vars = {} + self.machine_var_set = set() + self.machine_cmds = {} + + for i in range(4): + # Add home direction callbacks + self.set_callback(str(i) + 'hd', + lambda name, i = i: self.motor_home_direction(i)) + + + def _notify(self): + if not self.changes: return + + for listener in self.listeners.values(): + try: + listener(self.changes) + + except Exception as e: + log.error('Updating 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) + + 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) + + + def update(self, update): + for name, value in update.items(): + self.set(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: log.error('State variable "%s" not found' % name) + return default + + + def config(self, code, value): + if code in self.machine_var_set: self.ctrl.avr.set(code, value) + else: self.vars[code] = value + + + def add_listener(self, listener): + sid = self.next_id + self.next_id += 1 + + self.listeners[sid] = listener + if self.vars: listener(self.vars) + + return sid + + + def remove_listener(self, sid): del self.listeners[sid] + + + def machine_cmds_and_vars(self, data): + self.machine_cmds = data['commands'] + self.machine_vars = data['variables'] + + # Record all machine vars, indexed or not + self.machine_var_set = set() + for code, spec in self.machine_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) + + self.ctrl.config.reload() # Indirectly configures AVR + self.restore_machine_state() + + + def restore_machine_state(self): + for name in machine_state_vars: + if name in self.vars: + value = self.vars[name] + if isinstance(value, str): value = '"' + value + '"' + if isinstance(value, bool): value = int(value) + + self.ctrl.avr.set(name, value) + + + def find_motor(self, axis): + for motor in range(6): + if not ('%dan' % motor) in self.vars: continue + motor_axis = 'xyzabc'[self.vars['%dan' % motor]] + if motor_axis == axis.lower() and self.vars['%dpm' % motor]: + return motor + + + def is_axis_homed(self, axis): + return self.get('%s_homed' % axis, False) + + + def axis_can_home(self, axis): + motor = self.find_motor(axis) + if motor is None: return False + if not self.motor_enabled(motor): return False + + homing_mode = self.motor_homing_mode(motor) + if homing_mode == 1: return bool(int(self.get(axis + '_ls'))) # min sw + if homing_mode == 2: return bool(int(self.get(axis + '_xs'))) # max sw + return False + + + def motor_enabled(self, motor): return bool(int(self.vars['%dpm' % motor])) + def motor_homing_mode(self, motor): return int(self.vars['%dho' % motor]) + + + def motor_home_direction(self, motor): + homing_mode = self.motor_homing_mode(motor) + if homing_mode == 1: return -1 # Switch min + if homing_mode == 2: return 1 # Switch max + return 0 # Disabled diff --git a/src/py/bbctrl/Web.py b/src/py/bbctrl/Web.py index 57b7a39..01fe4a2 100644 --- a/src/py/bbctrl/Web.py +++ b/src/py/bbctrl/Web.py @@ -206,6 +206,7 @@ class OverrideSpeedHandler(bbctrl.APIHandler): def put_ok(self, value): self.ctrl.avr.override_speed(float(value)) +# Used by CAMotics class WSConnection(tornado.websocket.WebSocketHandler): def __init__(self, app, request, **kwargs): super(WSConnection, self).__init__(app, request, **kwargs) @@ -220,23 +221,20 @@ class WSConnection(tornado.websocket.WebSocketHandler): def open(self): - self.clients = self.ctrl.web.ws_clients - self.timer = self.ctrl.ioloop.call_later(3, self.heartbeat) self.count = 0; - - self.clients.append(self) - self.write_message(self.ctrl.avr.vars) + self.sid = self.ctrl.state.add_listener(lambda x: self.write_message(x)) def on_close(self): if self.timer is not None: self.ctrl.ioloop.remove_timeout(self.timer) - self.clients.remove(self) + self.ctrl.state.remove_listener(self.sid) def on_message(self, msg): pass +# Used by Web frontend class SockJSConnection(sockjs.tornado.SockJSConnection): def heartbeat(self): self.timer = self.ctrl.ioloop.call_later(3, self.heartbeat) @@ -246,18 +244,16 @@ class SockJSConnection(sockjs.tornado.SockJSConnection): def on_open(self, info): self.ctrl = self.session.server.ctrl - self.clients = self.ctrl.web.sockjs_clients self.timer = self.ctrl.ioloop.call_later(3, self.heartbeat) self.count = 0; - self.clients.append(self) - self.send(self.ctrl.avr.vars) + self.sid = self.ctrl.state.add_listener(lambda x: self.send(x)) def on_close(self): self.ctrl.ioloop.remove_timeout(self.timer) - self.clients.remove(self) + self.ctrl.state.remove_listener(self.sid) def on_message(self, data): @@ -273,8 +269,6 @@ class StaticFileHandler(tornado.web.StaticFileHandler): class Web(tornado.web.Application): def __init__(self, ctrl): self.ctrl = ctrl - self.ws_clients = [] - self.sockjs_clients = [] handlers = [ (r'/websocket', WSConnection), @@ -319,10 +313,3 @@ class Web(tornado.web.Application): sys.exit(1) log.info('Listening on http://%s:%d/', ctrl.args.addr, ctrl.args.port) - - - def broadcast(self, msg): - if len(self.sockjs_clients): - self.sockjs_clients[0].broadcast(self.sockjs_clients, msg) - - for client in self.ws_clients: client.write_message(msg) diff --git a/src/py/bbctrl/__init__.py b/src/py/bbctrl/__init__.py index dba6c24..e152283 100755 --- a/src/py/bbctrl/__init__.py +++ b/src/py/bbctrl/__init__.py @@ -21,6 +21,7 @@ from bbctrl.Ctrl import Ctrl from bbctrl.Pwr import Pwr from bbctrl.I2C import I2C from bbctrl.Planner import Planner +from bbctrl.State import State import bbctrl.Cmd as Cmd @@ -65,9 +66,17 @@ def run(): args = parse_args() # Init logging - log = logging.getLogger() - log.setLevel(logging.DEBUG if args.verbose else logging.INFO) - if args.log: log.addHandler(logging.FileHandler(args.log, mode = 'w')) + root = logging.getLogger() + root.setLevel(logging.DEBUG if args.verbose else logging.INFO) + f = logging.Formatter('{levelname[0]}:{name}:{message}', style = '{') + h = logging.StreamHandler() + h.setFormatter(f) + root.addHandler(h) + + if args.log: + h = logging.FileHandler(args.log) + h.setFormatter(f) + root.addHandler(h) # Set signal handler signal.signal(signal.SIGTERM, on_exit) -- 2.27.0