From: Joseph Coffland Date: Sun, 14 Jan 2018 21:52:15 +0000 (-0800) Subject: Working on seek X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=6c556efd481e14908d8fe358626765cdbd3e6547;p=bbctrl-firmware Working on seek --- diff --git a/src/avr/src/command.def b/src/avr/src/command.def index 6a674b5..8cd94dd 100644 --- a/src/avr/src/command.def +++ b/src/avr/src/command.def @@ -28,7 +28,7 @@ CMD('$', var, 0, "Set or get variable") CMD('#', sync_var, 1, "Set variable synchronous") -CMD('s', seek, 1, "Seek") +CMD('s', seek, 1, "[switch][flags:active|error]") CMD('l', line, 1, "[targetVel][maxJerk][axes][times]") CMD('d', dwell, 1, "[seconds]") CMD('o', out, 1, "Output") @@ -36,7 +36,6 @@ CMD('p', opt_pause, 1, "Set an optional pause") CMD('P', pause, 0, "[optional]") CMD('U', unpause, 0, "Unpause") CMD('j', jog, 0, "[axes]") -CMD('h', help, 0, "Print this help screen") CMD('r', report, 0, "<0|1>[var] Enable or disable var reporting") CMD('R', reboot, 0, "Reboot the controller") CMD('c', resume, 0, "Continue processing after a flush") @@ -45,3 +44,4 @@ CMD('C', clear, 0, "Clear estop") CMD('S', step, 0, "Advance one step") CMD('F', flush, 0, "Flush command queue") CMD('D', dump, 0, "Report all variables") +CMD('h', help, 0, "Print this help screen") diff --git a/src/avr/src/commands.c b/src/avr/src/commands.c index c9b11e2..dc72a52 100644 --- a/src/avr/src/commands.c +++ b/src/avr/src/commands.c @@ -34,17 +34,12 @@ #include "hardware.h" #include "report.h" #include "state.h" +#include "util.h" #include #include -// TODO -stat_t command_seek(char *cmd) {return STAT_OK;} -unsigned command_seek_size() {return 0;} -void command_seek_exec(void *data) {} - - stat_t command_dwell(char *cmd) { float seconds; if (!b64_decode_float(cmd + 1, &seconds)) return STAT_BAD_FLOAT; diff --git a/src/avr/src/estop.c b/src/avr/src/estop.c index a73dd1a..4d8f9e9 100644 --- a/src/avr/src/estop.c +++ b/src/avr/src/estop.c @@ -123,9 +123,7 @@ void estop_clear() { // Var callbacks -bool get_estop() { - return estop_triggered(); -} +bool get_estop() {return estop_triggered();} void set_estop(bool value) { @@ -135,9 +133,7 @@ void set_estop(bool value) { } -PGM_P get_estop_reason() { - return status_to_pgmstr(_get_reason()); -} +PGM_P get_estop_reason() {return status_to_pgmstr(_get_reason());} // Command callbacks @@ -147,7 +143,4 @@ stat_t command_estop(char *cmd) { } -stat_t command_clear(char *cmd) { - estop_clear(); - return STAT_OK; -} +stat_t command_clear(char *cmd) {estop_clear(); return STAT_OK;} diff --git a/src/avr/src/exec.c b/src/avr/src/exec.c index 4b531ea..352a932 100644 --- a/src/avr/src/exec.c +++ b/src/avr/src/exec.c @@ -51,12 +51,6 @@ static struct { float feed_override; float spindle_override; - - float leftover_time; - - bool seek_error; - bool seek_open; - int seek_switch; } ex; @@ -64,8 +58,6 @@ void exec_init() { memset(&ex, 0, sizeof(ex)); ex.feed_override = 1; ex.spindle_override = 1; - ex.seek_switch = -1; - // TODO implement seek // TODO implement pause // TODO implement move stepping // TODO implement overrides @@ -98,15 +90,6 @@ stat_t exec_move_to_target(float time, const float target[]) { // Update position copy_vector(ex.position, target); - // No move if time is too short - if (time < 0.5 * SEGMENT_TIME) { - ex.leftover_time += time; - return STAT_NOP; - } - - time += ex.leftover_time; - ex.leftover_time = 0; - // Call the stepper prep function st_prep_line(time, target); diff --git a/src/avr/src/line.c b/src/avr/src/line.c index 69ff58b..27c9473 100644 --- a/src/avr/src/line.c +++ b/src/avr/src/line.c @@ -31,6 +31,7 @@ #include "command.h" #include "scurve.h" #include "state.h" +#include "seek.h" #include "util.h" #include @@ -131,6 +132,22 @@ static bool _segment_next() { } +static void _done() { + seek_end(); + exec_set_cb(0); +} + + +static stat_t _move(float t, float target[AXES], float v, float a) { + exec_set_velocity(v); + exec_set_acceleration(a); + + if (seek_switch_found()) state_seek_hold(); + + return exec_move_to_target(t, target); +} + + static stat_t _pause() { float t = SEGMENT_TIME - l.current_t; float v = exec_get_velocity(); @@ -142,7 +159,7 @@ static stat_t _pause() { exec_set_acceleration(0); exec_set_jerk(0); state_holding(); - exec_set_cb(0); + _done(); return STAT_NOP; } @@ -165,7 +182,7 @@ static stat_t _pause() { exec_set_velocity(l.line.target_vel); exec_set_acceleration(0); exec_set_jerk(0); - exec_set_cb(0); + _done(); return STAT_AGAIN; } @@ -174,14 +191,10 @@ static stat_t _pause() { float target[AXES]; _segment_target(target, l.dist); - stat_t status = exec_move_to_target(SEGMENT_TIME, target); - l.current_t = 0; - exec_set_velocity(v); - exec_set_acceleration(a); exec_set_jerk(j); - return status; + return _move(SEGMENT_TIME, target, v, a); } @@ -223,25 +236,21 @@ static stat_t _line_exec() { // Do move & update exec stat_t status; - if (lastSeg && l.stop_seg) { + if (lastSeg && l.stop_seg) // Stop exactly on target to correct for floating-point errors - status = exec_move_to_target(delta, l.line.target); - exec_set_velocity(0); - exec_set_acceleration(0); + status = _move(delta, l.line.target, 0, 0); - } else { + else { // Compute target position from distance float target[AXES]; _segment_target(target, d); - status = exec_move_to_target(delta, target); + status = _move(delta, target, v, a); l.dist = d; - exec_set_velocity(v); - exec_set_acceleration(a); } // Release exec if we are done - if (lastSeg) exec_set_cb(0); + if (lastSeg) _done(); return status; } diff --git a/src/avr/src/messages.def b/src/avr/src/messages.def index d1c3070..c3417d9 100644 --- a/src/avr/src/messages.def +++ b/src/avr/src/messages.def @@ -48,3 +48,5 @@ STAT_MSG(INVALID_VALUE, "Invalid value") STAT_MSG(READ_ONLY, "Variable is read only") STAT_MSG(BUFFER_OVERFLOW, "Buffer overflow") STAT_MSG(BAD_SEG_TIME, "Bad s-curve segment time") +STAT_MSG(SEEK_NOT_ENABLED, "Switch not enabled") +STAT_MSG(SEEK_NOT_FOUND, "Switch not found") diff --git a/src/avr/src/seek.c b/src/avr/src/seek.c new file mode 100644 index 0000000..0f8ad1d --- /dev/null +++ b/src/avr/src/seek.c @@ -0,0 +1,97 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 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" + +\******************************************************************************/ + +#include "seek.h" + +#include "command.h" +#include "switch.h" +#include "estop.h" +#include "util.h" + +#include + + +enum { + SEEK_ACTIVE = 1 << 0, + SEEK_ERROR = 1 << 1, + SEEK_FOUND = 1 << 2, +}; + + +typedef struct { + int8_t sw; + uint8_t flags; +} seek_t; + + +static seek_t seek = {-1, 0}; + + +bool seek_switch_found() { + if (seek.sw <= 0) return false; + + bool inactive = !(seek.flags & SEEK_ACTIVE); + + if (switch_is_active(seek.sw) ^ inactive) { + seek.flags |= SEEK_FOUND; + return true; + } + + return false; +} + + +void seek_end() { + if (seek.sw <= 0) return; + + if (!(SEEK_FOUND & seek.flags) && (SEEK_ERROR & seek.flags)) + estop_trigger(STAT_SEEK_NOT_FOUND); + + seek.sw = -1; +} + + +// Command callbacks +stat_t command_seek(char *cmd) { + int8_t sw = decode_hex_nibble(cmd[1]); + if (sw <= 0) return STAT_INVALID_ARGUMENTS; // Don't allow seek to ESTOP + if (!switch_is_enabled(sw)) return STAT_SEEK_NOT_ENABLED; + + int8_t flags = decode_hex_nibble(cmd[2]); + if (flags & 0xfc) return STAT_INVALID_ARGUMENTS; + + seek_t seek = {sw, flags}; + command_push(*cmd, &seek); + + return STAT_OK; +} + + +unsigned command_seek_size() {return sizeof(seek_t);} + + +void command_seek_exec(void *data) {seek = *(seek_t *)data;} diff --git a/src/avr/src/seek.h b/src/avr/src/seek.h new file mode 100644 index 0000000..8d20d89 --- /dev/null +++ b/src/avr/src/seek.h @@ -0,0 +1,34 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 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" + +\******************************************************************************/ + +#pragma once + +#include + + +bool seek_switch_found(); +void seek_end(); diff --git a/src/avr/src/state.c b/src/avr/src/state.c index 9f6a222..1532f52 100644 --- a/src/avr/src/state.c +++ b/src/avr/src/state.c @@ -72,6 +72,7 @@ PGM_P state_get_hold_reason_pgmstr(hold_reason_t reason) { case HOLD_REASON_PALLET_CHANGE: return PSTR("Pallet change"); case HOLD_REASON_TOOL_CHANGE: return PSTR("Tool change"); case HOLD_REASON_STEPPING: return PSTR("Stepping"); + case HOLD_REASON_SEEK: return PSTR("Switch found"); } return PSTR("INVALID"); @@ -107,6 +108,14 @@ bool state_is_quiescent() { } +void state_seek_hold() { + if (state_get() == STATE_RUNNING) { + state_set_hold_reason(HOLD_REASON_SEEK); + _set_state(STATE_STOPPING); + } +} + + void state_holding() {_set_state(STATE_HOLDING);} diff --git a/src/avr/src/state.h b/src/avr/src/state.h index 7e496e2..27d4c04 100644 --- a/src/avr/src/state.h +++ b/src/avr/src/state.h @@ -48,6 +48,7 @@ typedef enum { HOLD_REASON_PALLET_CHANGE, HOLD_REASON_TOOL_CHANGE, HOLD_REASON_STEPPING, + HOLD_REASON_SEEK, } hold_reason_t; @@ -61,6 +62,7 @@ bool state_is_flushing(); bool state_is_resuming(); bool state_is_quiescent(); +void state_seek_hold(); void state_holding(); void state_optional_pause(); void state_running(); diff --git a/src/avr/src/switch.c b/src/avr/src/switch.c index ca676b2..68898fc 100644 --- a/src/avr/src/switch.c +++ b/src/avr/src/switch.c @@ -45,6 +45,8 @@ typedef struct { // Order must match indices in var functions below static switch_t switches[SWITCHES] = { + {.pin = ESTOP_PIN, .type = SW_DISABLED}, + {.pin = PROBE_PIN, .type = SW_DISABLED}, {.pin = MIN_X_PIN, .type = SW_DISABLED}, {.pin = MAX_X_PIN, .type = SW_DISABLED}, {.pin = MIN_Y_PIN, .type = SW_DISABLED}, @@ -53,8 +55,6 @@ static switch_t switches[SWITCHES] = { {.pin = MAX_Z_PIN, .type = SW_DISABLED}, {.pin = MIN_A_PIN, .type = SW_DISABLED}, {.pin = MAX_A_PIN, .type = SW_DISABLED}, - {.pin = ESTOP_PIN, .type = SW_DISABLED}, - {.pin = PROBE_PIN, .type = SW_DISABLED}, }; diff --git a/src/avr/src/switch.h b/src/avr/src/switch.h index 387dcdf..10906fa 100644 --- a/src/avr/src/switch.h +++ b/src/avr/src/switch.h @@ -35,24 +35,24 @@ // macros for finding the index into the switch table give the axis number -#define MIN_SWITCH(axis) (axis * 2) -#define MAX_SWITCH(axis) (axis * 2 + 1) +#define MIN_SWITCH(axis) (2 + axis * 2) +#define MAX_SWITCH(axis) (2 + axis * 2 + 1) typedef enum { SW_DISABLED, SW_NORMALLY_OPEN, - SW_NORMALLY_CLOSED + SW_NORMALLY_CLOSED, } switch_type_t; /// Switch IDs typedef enum { + SW_ESTOP, SW_PROBE, SW_MIN_X, SW_MAX_X, SW_MIN_Y, SW_MAX_Y, SW_MIN_Z, SW_MAX_Z, SW_MIN_A, SW_MAX_A, - SW_ESTOP, SW_PROBE } switch_id_t; diff --git a/src/avr/src/util.c b/src/avr/src/util.c index a1acd90..d6d55ef 100644 --- a/src/avr/src/util.c +++ b/src/avr/src/util.c @@ -54,6 +54,14 @@ float invsqrt(float x) { } +int8_t decode_hex_nibble(char c) { + if ('0' <= c && c <= '9') return c - '0'; + if ('a' <= c && c <= 'f') return c - 'a' + 10; + if ('A' <= c && c <= 'F') return c - 'A' + 10; + return -1; +} + + bool decode_float(char **s, float *f) { bool ok = b64_decode_float(*s, f) && isfinite(*f); *s += 6; diff --git a/src/avr/src/util.h b/src/avr/src/util.h index b431f8a..2a068df 100644 --- a/src/avr/src/util.h +++ b/src/avr/src/util.h @@ -65,6 +65,7 @@ inline static bool fp_ZERO(float a) {return fabs(a) < EPSILON;} inline static bool fp_FALSE(float a) {return fp_ZERO(a);} inline static bool fp_TRUE(float a) {return !fp_ZERO(a);} +int8_t decode_hex_nibble(char c); bool decode_float(char **s, float *f); stat_t decode_axes(char **cmd, float axes[AXES]); diff --git a/src/py/bbctrl/AVR.py b/src/py/bbctrl/AVR.py index 318dffc..96461c1 100644 --- a/src/py/bbctrl/AVR.py +++ b/src/py/bbctrl/AVR.py @@ -30,6 +30,13 @@ axis_homing_procedure = ''' 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): diff --git a/src/py/bbctrl/Cmd.py b/src/py/bbctrl/Cmd.py index ca1ac3a..51513ab 100644 --- a/src/py/bbctrl/Cmd.py +++ b/src/py/bbctrl/Cmd.py @@ -5,14 +5,21 @@ import logging log = logging.getLogger('Cmd') # TODO, sync this up with AVR code -REPORT = 'r' -PAUSE = 'P' -UNPAUSE = 'U' -ESTOP = 'E' -CLEAR = 'C' -FLUSH = 'F' -STEP = 'S' -RESUME = 'c' +SET = '$' +SET_SYNC = '#' +SEEK = 's' +LINE = 'l' +REPORT = 'r' +PAUSE = 'P' +UNPAUSE = 'U' +ESTOP = 'E' +CLEAR = 'C' +FLUSH = 'F' +STEP = 'S' +RESUME = 'c' + +SEEK_OPEN = 1 << 0 +SEEK_ERROR = 1 << 1 def encode_float(x): @@ -32,11 +39,16 @@ def encode_axes(axes): return data +def seek(switch, open, error): + flags = (SEEK_OPEN if open else 0) | (SEEK_ERROR if error else 0) + return '%c%x%x' % (SEEK, switch, flags) + + def line_number(line): return '#ln=%d' % line def line(id, target, exitVel, maxAccel, maxJerk, times): - cmd = '#id=%u\nl' % id + cmd = '#id=%u\n%c' % (id, LINE) cmd += encode_float(exitVel) cmd += encode_float(maxAccel)