* - Call the mach_xxx_xxx() function which will do any input validation and
* return an error if it detects one.
*
- * - The mach_ function calls mp_queue_command(). Arguments are a callback to
+ * - The mach_ function calls mp_queue_command(). Arguments are a callback to
* the _exec_...() function, which is the runtime execution routine, and any
* arguments that are needed by the runtime. See typedef for *exec in
* planner.h for details
distance_mode_t mach_get_distance_mode() {return mach.gm.distance_mode;}
feed_rate_mode_t mach_get_feed_rate_mode() {return mach.gm.feed_rate_mode;}
uint8_t mach_get_tool() {return mach.gm.tool;}
-spindle_mode_t mach_get_spindle_mode() {return mach.gm.spindle_mode;}
float mach_get_feed_rate() {return mach.gm.feed_rate;}
static stat_t _exec_update_work_offsets(mp_buffer_t *bf) {
mp_runtime_set_work_offsets(bf->target);
- return STAT_NOOP;
+ return STAT_NOOP; // No move queued
}
if (!same) {
mp_buffer_t *bf = mp_get_write_buffer();
-
- // never supposed to fail
- if (!bf) {CM_ALARM(STAT_BUFFER_FULL_FATAL); return;}
-
bf->bf_func = _exec_update_work_offsets;
copy_vector(bf->target, work_offset);
}
-/* Get position of axis in absolute coordinates
- *
- * NOTE: Machine position is always returned in mm mode. No units conversion
- * is performed
- */
-float mach_get_absolute_position(uint8_t axis) {
- return mach.position[axis];
-}
-
-
-/* Return work position in prevailing units (mm/inch) and with all offsets
- * applied.
+/*** Get position of axis in absolute coordinates
*
- * NOTE: This function only works after the gcode_state struct has had the
- * work_offsets setup by calling mach_get_model_coord_offset_vector() first.
+ * NOTE: Machine position is always returned in mm mode. No units conversion
+ * is performed.
*/
-float mach_get_work_position(uint8_t axis) {
- float position = mach.position[axis] - mach_get_active_coord_offset(axis);
-
- if (mach.gm.units_mode == INCHES) position /= MM_PER_INCH;
-
- return position;
-}
+float mach_get_absolute_position(uint8_t axis) {return mach.position[axis];}
/* Critical helpers
/* Return error code if soft limit is exceeded
*
- * Must be called with target properly set in GM struct. Best done
+ * Must be called with target properly set in GM struct. Best done
* after mach_set_model_target().
*
* Tests for soft limit for any homed axis if min and max are
// move through intermediate point, or skip
mach_rapid(target, flags);
- // make sure you have an available buffer
- mp_wait_for_buffer();
-
// execute actual stored move
float f[] = {1, 1, 1, 1, 1, 1};
return mach_rapid(mach.g28_position, f);
// move through intermediate point, or skip
mach_rapid(target, flags);
- // make sure you have an available buffer
- mp_wait_for_buffer();
-
// execute actual stored move
float f[] = {1, 1, 1, 1, 1, 1};
return mach_rapid(mach.g30_position, f);
*/
-static void _exec_program_stop(float *value, float *flag) {
+static stat_t _exec_program_stop(mp_buffer_t *bf) {
// Machine should be stopped at this point. Go into hold so that a start is
// needed before executing further instructions.
mp_state_holding();
+ return STAT_NOOP; // No move queued
}
/// M0 Queue a program stop
void mach_program_stop() {
- float value[AXES] = {0};
- mp_queue_command(_exec_program_stop, value, value);
+ mp_buffer_t *bf = mp_get_write_buffer();
+ if (!bf) {CM_ALARM(STAT_BUFFER_FULL_FATAL); return;} // Should not fail
+
+ bf->bf_func = _exec_program_stop;
+ mp_commit_write_buffer(mach.gm.line);
}
distance_mode_t mach_get_distance_mode();
feed_rate_mode_t mach_get_feed_rate_mode();
uint8_t mach_get_tool();
-spindle_mode_t mach_get_spindle_mode();
-inline float mach_get_spindle_speed() {return mach.gm.spindle_speed;}
float mach_get_feed_rate();
PGM_P mp_get_units_mode_pgmstr(units_mode_t mode);
float mach_get_active_coord_offset(uint8_t axis);
void mach_update_work_offsets();
float mach_get_absolute_position(uint8_t axis);
-float mach_get_work_position(uint8_t axis);
// Critical helpers
float mach_calc_move_time(const float axis_length[], const float axis_square[]);
typedef struct {
- run_state_t run_state; // runtime state machine sequence
+ bool running;
int32_t line; // gcode block line number
float target[AXES]; // XYZABC where the move should go
+ float position[AXES]; // end point of the current segment
float theta; // total angle specified by arc
float radius; // Raw R value, or computed via offsets
float segments = floor(min3(segments_for_chordal_accuracy,
segments_for_minimum_distance,
segments_for_minimum_time));
- segments = max(segments, 1); // at least 1 segment
+ if (segments < 1) segments = 1; // at least 1 segment
arc.segments = (uint32_t)segments;
arc.segment_theta = angular_travel / segments;
arc.center_0 = position[arc.plane_axis_0] - sin(arc.theta) * arc.radius;
arc.center_1 = position[arc.plane_axis_1] - cos(arc.theta) * arc.radius;
- // initialize the linear target
- arc.target[arc.linear_axis] = position[arc.linear_axis];
+ // initialize the linear position
+ arc.position[arc.linear_axis] = position[arc.linear_axis];
return STAT_OK;
}
// compute arc runtime values
RITORNO(_compute_arc(mach.position, offset, rotations, full_circle));
- arc.run_state = MOVE_RUN; // Enable arc run in callback
+ arc.running = true; // Enable arc run in callback
mach_arc_callback(); // Queue initial arc moves
copy_vector(mach.position, mach.gm.target); // update model position
* as many arc segments (lines) as it can before it blocks, then returns.
*/
void mach_arc_callback() {
- while (arc.run_state != MOVE_OFF && mp_get_planner_buffer_room()) {
- arc.theta += arc.segment_theta;
-
- arc.target[arc.plane_axis_0] = arc.center_0 + sin(arc.theta) * arc.radius;
- arc.target[arc.plane_axis_1] = arc.center_1 + cos(arc.theta) * arc.radius;
- arc.target[arc.linear_axis] += arc.segment_linear_travel;
+ while (arc.running && mp_get_planner_buffer_room()) {
+ if (arc.segments == 1) { // Final segment
+ arc.position[arc.plane_axis_0] = arc.target[arc.plane_axis_0];
+ arc.position[arc.plane_axis_1] = arc.target[arc.plane_axis_1];
+ arc.position[arc.linear_axis] = arc.target[arc.linear_axis];
+
+ } else {
+ arc.theta += arc.segment_theta;
+
+ arc.position[arc.plane_axis_0] =
+ arc.center_0 + sin(arc.theta) * arc.radius;
+ arc.position[arc.plane_axis_1] =
+ arc.center_1 + cos(arc.theta) * arc.radius;
+ arc.position[arc.linear_axis] += arc.segment_linear_travel;
+ }
- mp_aline(arc.target, arc.line); // run the line
+ mp_aline(arc.position, arc.line); // run the line
- if (!--arc.segments) arc.run_state = MOVE_OFF;
+ if (!--arc.segments) arc.running = false;
}
}
-bool mach_arc_active() {return arc.run_state != MOVE_OFF;}
+bool mach_arc_active() {return arc.running;}
/// Stop arc movement without maintaining position
/// OK to call if no arc is running
-void mach_abort_arc() {arc.run_state = MOVE_OFF;}
+void mach_abort_arc() {arc.running = false;}
bool mp_queue_empty() {return mb.w == mb.r;}
-/// Get pointer to next available write buffer
-/// Returns pointer or 0 if no buffer available.
+/// Get pointer to next available write buffer. Wait until one is available.
mp_buffer_t *mp_get_write_buffer() {
- // get & clear a buffer
- if (mb.w->buffer_state == MP_BUFFER_EMPTY) {
- mp_buffer_t *w = mb.w;
- mp_buffer_t *nx = mb.w->nx; // save linked list pointers
- mp_buffer_t *pv = mb.w->pv;
- memset(mb.w, 0, sizeof(mp_buffer_t)); // clear all values
- w->nx = nx; // restore pointers
- w->pv = pv;
- w->buffer_state = MP_BUFFER_LOADING;
- mb.w = w->nx;
-
- mb.buffers_available--;
-
- return w;
- }
-
- return 0;
+ // Wait for a buffer
+ while (!mb.buffers_available) continue;
+
+ // Get & clear write buffer
+ mp_buffer_t *w = mb.w;
+ mp_buffer_t *nx = mb.w->nx; // save linked list pointers
+ mp_buffer_t *pv = mb.w->pv;
+ memset(mb.w, 0, sizeof(mp_buffer_t)); // clear all values
+ w->nx = nx; // restore pointers
+ w->pv = pv;
+ w->buffer_state = MP_BUFFER_LOADING;
+ mb.w = w->nx;
+
+ mb.buffers_available--;
+
+ return w;
}
mp_buffer_t *bf = mp_get_run_buffer();
mp_buffer_t *bp;
- for (bp = bf; bp && bp->nx != bf; bp = mp_get_next_buffer(bp))
+ for (bp = bf; bp && bp->nx != bf; bp = mp_buffer_next(bp))
if (bp->nx->run_state == MOVE_OFF) break;
return bp;
mp_buffer_t *mp_get_run_buffer();
void mp_free_run_buffer();
mp_buffer_t *mp_get_last_buffer();
-#define mp_get_prev_buffer(b) (b->pv)
-#define mp_get_next_buffer(b) (b->nx)
+static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->pv;}
+static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->nx;}
void mp_clear_buffer(mp_buffer_t *bf);
void mp_copy_buffer(mp_buffer_t *bf, const mp_buffer_t *bp);
if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY)
return 0;
- mp_buffer_t *bf = mp_get_write_buffer();
- if (!bf) {
- CM_ALARM(STAT_BUFFER_FULL_FATAL);
- return 0;
- }
-
// Start
memset(&cal, 0, sizeof(cal));
mp_set_cycle(CYCLE_CALIBRATING);
cal.motor = 1;
+ mp_buffer_t *bf = mp_get_write_buffer();
bf->bf_func = _exec_calibrate; // register callback
mp_commit_write_buffer(-1);
void mp_queue_command(mach_func_t mach_func, float values[], float flags[]) {
mp_buffer_t *bf = mp_get_write_buffer();
- if (!bf) {
- CM_ALARM(STAT_BUFFER_FULL_FATAL);
- return; // Shouldn't happen, buffer availability was checked upstream.
- }
-
bf->bf_func = _exec_command; // callback to planner queue exec function
bf->mach_func = mach_func; // callback to machine exec function
/// Queue a dwell
stat_t mp_dwell(float seconds, int32_t line) {
mp_buffer_t *bf = mp_get_write_buffer();
-
- // never supposed to fail
- if (!bf) return CM_ALARM(STAT_BUFFER_FULL_FATAL);
-
bf->bf_func = _exec_dwell; // register callback to dwell start
bf->dwell = seconds; // in seconds, not minutes
* Returning STAT_OK at does NOT advance position meaning
* any position error will be compensated by the next move.
*
- * [2] Solves a potential race condition where the current move ends but
- * the new move has not started because the previous move is still
- * being run by the steppers. Planning can overwrite the new move.
- *
* Operation:
*
* Aline generates jerk-controlled S-curves as per Ed Red's course notes:
if (status != STAT_OK) return status;
}
+ // Plan holds
if (mp_get_state() == STATE_STOPPING && !ex.hold_planned) _plan_hold();
- // Main segment dispatch. From this point on the contents of the bf buffer
- // do not affect execution.
+ // Main segment dispatch
switch (ex.section) {
case SECTION_HEAD: status = _exec_aline_head(); break;
case SECTION_BODY: status = _exec_aline_body(); break;
case SECTION_TAIL: status = _exec_aline_tail(); break;
}
+ // Set runtime velocity on exit
if (status != STAT_EAGAIN) mp_runtime_set_velocity(ex.exit_velocity);
return status;
// Handle buffer run state
if (bf->run_state == MOVE_RESTART) bf->run_state = MOVE_NEW;
else {
- bf->nx->replannable = false; // Prevent overplanning (Note 2)
- mp_free_run_buffer(); // Free buffer
+ // Solves a potential race condition where the current move ends but
+ // the new move has not started because the current move is still
+ // being run by the steppers. Planning can overwrite the new move.
+ mp_buffer_next(bf)->replannable = false;
+
+ mp_free_run_buffer(); // Free buffer
// Enter READY state
if (mp_queue_empty()) {
jr.writing = false;
if (!mp_jog_busy()) {
- // Should always be at least one free buffer
- mp_buffer_t *bf = mp_get_write_buffer();
- if (!bf) return STAT_BUFFER_FULL_FATAL;
-
- // Start
mp_set_cycle(CYCLE_JOGGING);
+
+ mp_buffer_t *bf = mp_get_write_buffer();
bf->bf_func = _exec_jog; // register callback
mp_commit_write_buffer(-1);
}
static void _calc_max_velocities(mp_buffer_t *bf, float move_time) {
- float junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit);
+ float junction_velocity =
+ _get_junction_vmax(mp_buffer_prev(bf)->unit, bf->unit);
bf->cruise_vmax = bf->length / move_time; // target velocity requested
bf->entry_vmax = min(bf->cruise_vmax, junction_velocity);
// Get a buffer. Note, new buffers are initialized to zero.
mp_buffer_t *bf = mp_get_write_buffer(); // current move pointer
- if (!bf) return CM_ALARM(STAT_BUFFER_FULL_FATAL); // should never fail
// Set buffer values
bf->bf_func = mp_exec_aline;
float time = mach_calc_move_time(axis_length, axis_square);
_calc_max_velocities(bf, time);
- // Note, next the following lines must remain in order.
+ // Note, the following lines must remain in order.
mp_plan_block_list(bf); // Plan block list
mp_set_position(target); // Set planner position before committing buffer
mp_commit_write_buffer(line); // Commit current block after position update
// B" case: Block is short, but fits into a single body segment
if (naive_move_time <= NOM_SEGMENT_TIME) {
- bf->entry_velocity = bf->pv->exit_velocity;
+ bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity;
if (fp_NOT_ZERO(bf->entry_velocity)) {
bf->cruise_velocity = bf->entry_velocity;
/*** Plans the entire block list
*
- * The block list is the circular buffer of planner buffers
- * (bf's). The block currently being planned is the "bf" block. The
- * "first block" is the next block to execute; queued immediately
- * behind the currently executing block, aka the "running" block.
- * In some cases, there is no first block because the list is empty
- * or there is only one block and it is already running.
+ * The block list is the circular buffer of planner buffers (bf's). The block
+ * currently being planned is the "bf" block. The "first block" is the next
+ * block to execute; queued immediately behind the currently executing block,
+ * aka the "running" block. In some cases, there is no first block because the
+ * list is empty or there is only one block and it is already running.
*
- * If blocks following the first block are already optimally
- * planned (non replannable) the first block that is not optimally
- * planned becomes the effective first block.
+ * If blocks following the first block are already optimally planned (non
+ * replannable) the first block that is not optimally planned becomes the
+ * effective first block.
*
- * mp_plan_block_list() plans all blocks between and including the
- * (effective) first block and the bf. It sets entry, exit and
- * cruise v's from vmax's then calls trapezoid generation.
+ * mp_plan_block_list() plans all blocks between and including the (effective)
+ * first block and the bf. It sets entry, exit and cruise v's from vmax's then
+ * calls trapezoid generation.
*
* Variables that must be provided in the mp_buffer_ts that will be processed:
*
*
* Notes:
*
- * [1] Whether or not a block is planned is controlled by the
- * bf->replannable setting (set TRUE if it should be). Replan flags
- * are checked during the backwards pass and prune the replan list
- * to include only the the latest blocks that require planning
- *
- * In normal operation the first block (currently running
- * block) is not replanned, but may be for feedholds and feed
- * overrides. In these cases the prep routines modify the
- * contents of the (ex) buffer and re-shuffle the block list,
- * re-enlisting the current bf buffer with new parameters.
- * These routines also set all blocks in the list to be
- * replannable so the list can be recomputed regardless of
- * exact stops and previous replanning optimizations.
+ * [1] Whether or not a block is planned is controlled by the bf->replannable
+ * setting. Replan flags are checked during the backwards pass. They prune
+ * the replan list to include only the the latest blocks that require
+ * planning.
+ *
+ * In normal operation, the first block (currently running block) is not
+ * replanned, but may be for feedholds and feed overrides. In these cases,
+ * the prep routines modify the contents of the (ex) buffer and re-shuffle
+ * the block list, re-enlisting the current bf buffer with new parameters.
+ * These routines also set all blocks in the list to be replannable so the
+ * list can be recomputed regardless of exact stops and previous replanning
+ * optimizations.
*/
void mp_plan_block_list(mp_buffer_t *bf) {
mp_buffer_t *bp = bf;
// Backward planning pass. Find first block and update braking velocities.
// By the end bp points to the buffer before the first block.
- while ((bp = mp_get_prev_buffer(bp)) != bf) {
+ while ((bp = mp_buffer_prev(bp)) != bf) {
if (!bp->replannable) break;
bp->braking_velocity =
- min(bp->nx->entry_vmax, bp->nx->braking_velocity) + bp->delta_vmax;
+ min(mp_buffer_next(bp)->entry_vmax,
+ mp_buffer_next(bp)->braking_velocity) + bp->delta_vmax;
}
// Forward planning pass. Recompute trapezoids from the first block to bf.
- while ((bp = mp_get_next_buffer(bp)) != bf) {
- if (bp->pv == bf) bp->entry_velocity = bp->entry_vmax; // first block
- else bp->entry_velocity = bp->pv->exit_velocity; // other blocks
+ while ((bp = mp_buffer_next(bp)) != bf) {
+ if (mp_buffer_prev(bp) == bf)
+ bp->entry_velocity = bp->entry_vmax; // first block
+ else bp->entry_velocity = mp_buffer_prev(bp)->exit_velocity; // other blocks
bp->cruise_velocity = bp->cruise_vmax;
- bp->exit_velocity = min4(bp->exit_vmax, bp->nx->entry_vmax,
- bp->nx->braking_velocity,
+ bp->exit_velocity = min4(bp->exit_vmax, mp_buffer_next(bp)->entry_vmax,
+ mp_buffer_next(bp)->braking_velocity,
bp->entry_velocity + bp->delta_vmax);
mp_calculate_trapezoid(bp);
// Test for optimally planned trapezoids by checking exit conditions
if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) ||
- fp_EQ(bp->exit_velocity, bp->nx->entry_vmax)) ||
- (!bp->pv->replannable &&
+ fp_EQ(bp->exit_velocity, mp_buffer_next(bp)->entry_vmax)) ||
+ (!mp_buffer_prev(bp)->replannable &&
fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax))))
bp->replannable = false;
}
// Finish last block
- bp->entry_velocity = bp->pv->exit_velocity;
+ bp->entry_velocity = mp_buffer_prev(bp)->exit_velocity;
bp->cruise_velocity = bp->cruise_vmax;
bp->exit_velocity = 0;
// Mark all blocks replanable
while (true) {
bp->replannable = true;
- if (bp->nx->run_state == MOVE_OFF || bp->nx == bf) break;
- bp = mp_get_next_buffer(bp);
+ if (mp_buffer_next(bp)->run_state == MOVE_OFF || mp_buffer_next(bp) == bf)
+ break;
+ bp = mp_buffer_next(bp);
}
// Plan blocks
SPINDLE_TYPE_HUANYANG,
} spindle_type_t;
-static spindle_type_t _type = SPINDLE_TYPE;
+
+typedef struct {
+ spindle_type_t type;
+ spindle_mode_t mode;
+ float speed;
+} spindle_t;
+
+
+static spindle_t spindle = {.type = SPINDLE_TYPE};
void spindle_init() {
void spindle_set(spindle_mode_t mode, float speed) {
- switch (_type) {
+ spindle.mode = mode;
+ spindle.speed = speed;
+
+ switch (spindle.type) {
case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, speed); break;
case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, speed); break;
}
}
+spindle_mode_t spindle_get_mode() {return spindle.mode;}
+float spindle_get_speed() {return spindle.speed;}
+
+
void spindle_estop() {
- switch (_type) {
+ switch (spindle.type) {
case SPINDLE_TYPE_PWM: pwm_spindle_estop(); break;
case SPINDLE_TYPE_HUANYANG: huanyang_estop(); break;
}
}
-uint8_t get_spindle_type(int index) {return _type;}
+uint8_t get_spindle_type(int index) {return spindle.type;}
void set_spindle_type(int index, uint8_t value) {
- if (value != _type) {
+ if (value != spindle.type) {
+ spindle_mode_t mode = spindle.mode;
+ float speed = spindle.speed;
+
spindle_set(SPINDLE_OFF, 0);
- _type = value;
- spindle_set(mach.gm.spindle_mode, mach.gm.spindle_speed);
+ spindle.type = value;
+ spindle_set(mode, speed);
}
}
void spindle_init();
void spindle_set(spindle_mode_t spindle_mode, float speed);
+spindle_mode_t spindle_get_mode();
+float spindle_get_speed();
void spindle_estop();
#include "usart.h"
#include "machine.h"
+#include "spindle.h"
#include "plan/runtime.h"
#include "plan/state.h"
// GCode
int32_t get_line() {return mp_runtime_get_line();}
PGM_P get_unit() {return mp_get_units_mode_pgmstr(mach_get_units_mode());}
-float get_speed() {return mach_get_spindle_speed();}
+float get_speed() {return spindle_get_speed();}
float get_feed() {return mach_get_feed_rate();}
uint8_t get_tool() {return mach_get_tool();}
projects / bbctrl-firmware / commitdiff