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")
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")
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")
#include "hardware.h"
#include "report.h"
#include "state.h"
+#include "util.h"
#include <string.h>
#include <stdio.h>
-// 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;
// Var callbacks
-bool get_estop() {
- return estop_triggered();
-}
+bool get_estop() {return estop_triggered();}
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
}
-stat_t command_clear(char *cmd) {
- estop_clear();
- return STAT_OK;
-}
+stat_t command_clear(char *cmd) {estop_clear(); return STAT_OK;}
float feed_override;
float spindle_override;
-
- float leftover_time;
-
- bool seek_error;
- bool seek_open;
- int seek_switch;
} ex;
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
// 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);
#include "command.h"
#include "scurve.h"
#include "state.h"
+#include "seek.h"
#include "util.h"
#include <math.h>
}
+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();
exec_set_acceleration(0);
exec_set_jerk(0);
state_holding();
- exec_set_cb(0);
+ _done();
return STAT_NOP;
}
exec_set_velocity(l.line.target_vel);
exec_set_acceleration(0);
exec_set_jerk(0);
- exec_set_cb(0);
+ _done();
return STAT_AGAIN;
}
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);
}
// 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;
}
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")
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "seek.h"
+
+#include "command.h"
+#include "switch.h"
+#include "estop.h"
+#include "util.h"
+
+#include <stdint.h>
+
+
+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;}
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <stdbool.h>
+
+
+bool seek_switch_found();
+void seek_end();
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");
}
+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);}
HOLD_REASON_PALLET_CHANGE,
HOLD_REASON_TOOL_CHANGE,
HOLD_REASON_STEPPING,
+ HOLD_REASON_SEEK,
} hold_reason_t;
bool state_is_resuming();
bool state_is_quiescent();
+void state_seek_hold();
void state_holding();
void state_optional_pause();
void state_running();
// 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},
{.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},
};
// 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;
}
+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;
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]);
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):
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):
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)
projects / bbctrl-firmware / commitdiff