}
-void command_callback() {
- if (!_command_next()) return;
+bool command_callback() {
+ static char *block = 0;
+ if (!block) block = usart_readline();
+ if (!block) return false; // No command
+
+ cmd.active = true; // Disables LCD booting message
stat_t status = STAT_OK;
- switch (*_cmd) {
- case 0: break; // Empty line
- case '{': status = vars_parser(_cmd); break; // TODO is this necessary?
- case '$': status = command_parser(_cmd); break;
+ // Special processing for synchronous commands
+ if (_is_synchronous(*block)) {
+ if (estop_triggered()) status = STAT_MACHINE_ALARMED;
+ else if (state_is_flushing()) status = STAT_NOOP; // Flush
+ else if (state_is_resuming() || _space() < _size(*block))
+ return false; // Wait
+ }
default:
if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;}
_cmd = 0; // Command consumed
_active = true;
report_request();
+ if (status && status != STAT_NOOP) status_error(status);
- if (status) status_error(status);
+ return true;
}
void command_print_help();
void command_flush_queue();
void command_push(char code, void *data);
-void command_callback();
+bool command_callback();
bool command_exec();
stat_t command_pause(char *cmd) {
if (cmd[1] == '1') state_request_optional_pause();
- else state_request_hold();
+ else state_request_pause();
return STAT_OK;
}
// Timer setup for stepper and dwells
-#define STEP_TIMER_ENABLE TC_CLKSEL_DIV4_gc
-#define STEP_TIMER_DIV 4
+#define STEP_TIMER_ENABLE TC_CLKSEL_DIV8_gc
+#define STEP_TIMER_DIV 8
#define STEP_TIMER_FREQ (F_CPU / STEP_TIMER_DIV)
-#define STEP_TIMER_POLL ((uint16_t)(STEP_TIMER_FREQ * 0.001))
+#define STEP_TIMER_POLL ((uint16_t)(STEP_TIMER_FREQ * 0.001)) // 1ms
#define STEP_TIMER_WGMODE TC_WGMODE_NORMAL_gc // count to TOP & rollover
#define STEP_TIMER_ISR TCC0_OVF_vect
#define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc
drv8711_state_t state;
current_t drive;
current_t idle;
- float stall_threshold;
+ float stall_threspause;
uint8_t mode; // microstepping mode
stall_callback_t stall_cb;
ex.spindle_override = 1;
ex.seek_switch = -1;
// TODO implement seek
- // TODO implement feedhold
+ // TODO implement pause
// TODO implement move stepping
// TODO implement overrides
// TODO implement optional pause
float exec_get_velocity() {return ex.velocity;}
void exec_set_acceleration(float a) {ex.accel = a;}
void exec_set_jerk(float j) {ex.jerk = j;}
-void exec_set_line(int line) {ex.line = line;}
+void exec_set_line(int32_t line) {ex.line = line;}
+int32_t exec_get_line() {return ex.line;}
void exec_set_cb(exec_cb_t cb) {ex.cb = cb;}
#include "status.h"
#include <stdbool.h>
+#include <stdint.h>
typedef stat_t (*exec_cb_t)();
float exec_get_velocity();
void exec_set_acceleration(float a);
void exec_set_jerk(float j);
-void exec_set_line(int line);
+void exec_set_line(int32_t line);
+int32_t exec_get_line();
void exec_set_cb(exec_cb_t cb);
#include "config.h"
+#include <stdint.h>
#include <stdbool.h>
typedef void (*i2c_read_cb_t)(uint8_t *data, uint8_t length);
static struct {
- float next_start[AXES];
-
line_t line;
- bool new;
int seg;
- uint16_t steps;
- uint16_t step;
- float delta;
+ bool stop_seg;
+ float current_t;
float dist;
float vel;
float accel;
float jerk;
-} l = {};
+} l = {.current_t = SEGMENT_TIME};
static float _compute_distance(float t, float v, float a, float j) {
static bool _segment_next() {
- while (++l.seg < 7)
- if (l.line.times[l.seg]) return (l.new = true);
- return false;
-}
-
-
-static void _segment_init() {
- l.new = false;
-
- // Jerk
- switch (l.seg) {
- case 0: case 6: l.jerk = l.line.max_jerk; break;
- case 2: case 4: l.jerk = -l.line.max_jerk; break;
- default: l.jerk = 0;
+ while (++l.seg < 7) {
+ float seg_t = l.line.times[l.seg];
+ if (!seg_t) continue;
+
+ // Check if last seg
+ bool last_seg = true;
+ for (int i = l.seg + 1; i < 7; i++)
+ if (l.line.times[i]) last_seg = false;
+
+ // Check if stop seg
+ l.stop_seg = last_seg && !l.line.target_vel;
+
+ // Jerk
+ switch (l.seg) {
+ case 0: case 6: l.jerk = l.line.max_jerk; break;
+ case 2: case 4: l.jerk = -l.line.max_jerk; break;
+ default: l.jerk = 0;
+ }
+ exec_set_jerk(l.jerk);
+
+ // Acceleration
+ switch (l.seg) {
+ case 1: case 2: l.accel = l.line.max_jerk * l.line.times[0]; break;
+ case 5: case 6: l.accel = -l.line.max_jerk * l.line.times[4]; break;
+ default: l.accel = 0;
+ }
+ exec_set_acceleration(l.accel);
+
+ // Skip segs which do not land on a SEGMENT_TIME
+ if (seg_t < l.current_t && !l.stop_seg) {
+ l.current_t -= l.line.times[l.seg];
+ l.dist += _compute_distance(seg_t, l.vel, l.accel, l.jerk);
+ l.vel += _compute_velocity(seg_t, l.accel, l.jerk);
+ continue;
+ }
+
+ return true;
}
- exec_set_jerk(l.jerk);
- // Acceleration
- switch (l.seg) {
- case 1: case 2: l.accel = l.line.max_jerk * l.line.times[0]; break;
- case 5: case 6: l.accel = -l.line.max_jerk * l.line.times[4]; break;
- default: l.accel = 0;
- }
- exec_set_acceleration(l.accel);
-
- // Compute interpolation steps
- l.step = 0;
- float time = l.line.times[l.seg];
- l.steps = round(time / SEGMENT_TIME);
- if (!l.steps) {l.steps = 1; l.delta = time;}
- else l.delta = time / l.steps;
+ return false;
}
static stat_t _line_exec() {
- if (l.new) _segment_init();
+ float seg_t = l.line.times[l.seg];
- bool lastStep = l.step == l.steps - 1;
+ // Adjust time if near a stopping point
+ float delta = SEGMENT_TIME;
+ if (l.stop_seg && seg_t - l.current_t < SEGMENT_TIME) {
+ delta += seg_t - l.current_t;
+ l.current_t = seg_t;
+ }
- // Compute time, distance and velocity offsets
- float t = lastStep ? l.line.times[l.seg] : (l.delta * (l.step + 1));
- float d = l.dist + _compute_distance(t, l.vel, l.accel, l.jerk);
- float v = l.vel + _compute_velocity(t, l.accel, l.jerk);
+ // Compute distance and velocity offsets
+ float d = l.dist + _compute_distance(l.current_t, l.vel, l.accel, l.jerk);
+ float v = l.vel + _compute_velocity(l.current_t, l.accel, l.jerk);
// Compute target position from distance
float target[AXES];
for (int i = 0; i < AXES; i++)
target[i] = l.line.start[i] + l.line.unit[i] * d;
- // Update dist and vel for next seg
- if (lastStep) {
- l.dist = d;
- l.vel = v;
+ // Advance time
+ l.current_t += SEGMENT_TIME;
- } else l.step++;
+ // Check if segment complete
+ bool lastSeg = false;
+ if (seg_t < l.current_t) {
+ l.current_t -= seg_t;
+ l.dist += _compute_distance(seg_t, l.vel, l.accel, l.jerk);
+ l.vel += _compute_velocity(seg_t, l.accel, l.jerk);
- // Advance curve
- bool lastCurve = lastStep && !_segment_next();
+ // Next segment
+ lastSeg = !_segment_next();
+ }
// Do move
- stat_t status =
- exec_move_to_target(l.delta, lastCurve ? l.line.target : target);
+ // Stop exactly on target to correct for floating-point errors
+ stat_t status = exec_move_to_target
+ (delta, (lastSeg && l.stop_seg) ? l.line.target : target);
// Check if we're done
- if (lastCurve) {
+ if (lastSeg) {
exec_set_velocity(l.vel = l.line.target_vel);
exec_set_cb(0);
stat_t command_line(char *cmd) {
+ static float next_start[AXES];
line_t line = {};
cmd++; // Skip command code
// Get start position
- copy_vector(line.start, l.next_start);
+ copy_vector(line.start, next_start);
// Get target velocity
if (!decode_float(&cmd, &line.target_vel)) return STAT_BAD_FLOAT;
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) {
if (*cmd) return STAT_INVALID_ARGUMENTS;
// Set next start position
- copy_vector(l.next_start, line.target);
+ copy_vector(next_start, line.target);
// Compute direction vector
float length = 0;
// Find first segment
l.seg = -1;
- _segment_next();
+ if (!_segment_next()) return;
// Set callback
exec_set_cb(_line_exec);
#define PRPSTR "s"
#define PROGMEM
-#define PGM_P char *
+#define PGM_P const char *
#define PSTR(X) X
#define vfprintf_P vfprintf
#define printf_P printf
state_t state;
hold_reason_t hold_reason;
- bool hold_requested;
+ bool pause_requested;
bool flush_requested;
bool start_requested;
bool resume_requested;
}
-void state_optional_pause() {
- if (s.optional_pause_requested) {
- state_set_hold_reason(HOLD_REASON_USER_PAUSE);
- state_holding();
- }
-}
-
-
static void _set_plan_steps(bool plan_steps) {} // TODO
}
+void state_optional_pause() {
+ if (s.optional_pause_requested) {
+ state_set_hold_reason(HOLD_REASON_USER_PAUSE);
+ state_holding();
+ }
+}
+
+
void state_running() {
if (state_get() == STATE_READY) _set_state(STATE_RUNNING);
}
void state_idle() {if (state_get() == STATE_RUNNING) _set_state(STATE_READY);}
void state_estop() {_set_state(STATE_ESTOPPED);}
-void state_request_hold() {s.hold_requested = true;}
+void state_request_pause() {s.pause_requested = true;}
void state_request_start() {s.start_requested = true;}
void state_request_flush() {s.flush_requested = true;}
void state_request_resume() {if (s.flush_requested) s.resume_requested = true;}
}
-/*** Feedholds, queue flushes and starts are all related. Request functions
+/*** Pauses, queue flushes and starts are all related. Request functions
* set flags. The callback interprets the flags according to these rules:
*
- * A hold request received:
+ * A pause request received:
* - during motion is honored
- * - during a feedhold is ignored and reset
+ * - during a pause is ignored and reset
* - when already stopped is ignored and reset
*
* A flush request received:
* - during motion is ignored but not reset
- * - during a feedhold is deferred until the feedhold enters HOLDING state.
+ * - during a pause is deferred until the feedpause enters HOLDING state.
* 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 feedhold is deferred until the feedhold enters HOLDING state.
+ * - during a pause is deferred until the feedpause enters HOLDING state.
* 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.hold_requested || s.flush_requested) {
- s.hold_requested = false;
+ if (s.pause_requested || s.flush_requested) {
+ s.pause_requested = false;
state_set_hold_reason(HOLD_REASON_USER_PAUSE);
if (state_get() == STATE_RUNNING) _set_state(STATE_STOPPING);
bool state_is_resuming();
bool state_is_quiescent();
-void state_optional_pause();
void state_holding();
+void state_optional_pause();
void state_running();
void state_idle();
void state_estop();
-void state_request_hold();
+void state_request_pause();
void state_request_start();
void state_request_flush();
void state_request_resume();
move_type_t move_type;
float prep_dwell;
uint16_t clock_period;
+
+ uint32_t underflow;
} stepper_t;
// If the next move is not ready try to load it
if (!st.move_ready) {
+ if (exec_get_velocity()) st.underflow++;
_request_exec_move();
_end_move();
return;
// Trap conditions that would prevent queuing the line
ASSERT(!st.move_ready);
ASSERT(isfinite(time));
+ ASSERT(time * STEP_TIMER_FREQ * 60 <= 0xffff);
// Setup segment parameters
st.move_type = MOVE_TYPE_LINE;
// Prepare motor moves
for (int motor = 0; motor < MOTORS; motor++)
- //if (motor_is_enabled(motor))
- motor_prep_move(motor, time, target[motor_get_axis(motor)]);
+ motor_prep_move(motor, time, target[motor_get_axis(motor)]);
st.move_queued = true; // signal prep buffer ready (do this last)
}
st.prep_dwell = seconds;
st.move_queued = true; // signal prep buffer ready
}
+
+
+// Var callbacks
+uint32_t get_underflow() {return st.underflow;}
#include <math.h>
#include <ctype.h>
#include <stdlib.h>
+#include <inttypes.h>
#define TYPEDEF(TYPE, DEF) \
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")
+VAR(axis_can_home, ch, bool, AXES, 0, 1, "Axis can home")
// Outputs
VAR(output_state, os, u8, OUTS, 0, 1, "Output pin state")
VAR(estop_reason, er, pstr, 0, 0, 1, "Emergency stop reason")
VAR(state, x, pstr, 0, 0, 1, "Machine state")
VAR(hold_reason, pr, pstr, 0, 0, 1, "Machine pause reason")
+VAR(underflow, un, u32, 0, 0, 1, "Stepper underflow count")
projects / bbctrl-firmware / commitdiff