From 292a9d79e302b6fd053889e6d3670ca3cd15507c Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sat, 10 Sep 2016 02:59:00 -0700 Subject: [PATCH] More encapsulation, made some queue commands unsynchronized, final arc move to actual target, always return a write buffer --- src/machine.c | 52 ++++++++---------------------- src/machine.h | 3 -- src/plan/arc.c | 40 ++++++++++++++--------- src/plan/buffer.c | 38 +++++++++++----------- src/plan/buffer.h | 4 +-- src/plan/calibrate.c | 7 +--- src/plan/command.c | 5 --- src/plan/dwell.c | 4 --- src/plan/exec.c | 17 +++++----- src/plan/jog.c | 7 ++-- src/plan/line.c | 6 ++-- src/plan/planner.c | 77 ++++++++++++++++++++++---------------------- src/spindle.c | 32 ++++++++++++++---- src/spindle.h | 2 ++ src/varcb.c | 3 +- 15 files changed, 142 insertions(+), 155 deletions(-) diff --git a/src/machine.c b/src/machine.c index 98b3fef..0ec4f90 100644 --- a/src/machine.c +++ b/src/machine.c @@ -44,7 +44,7 @@ * - 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 @@ -197,7 +197,6 @@ path_mode_t mach_get_path_control() {return mach.gm.path_control;} 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;} @@ -393,7 +392,7 @@ float mach_get_active_coord_offset(uint8_t axis) { 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 } @@ -413,10 +412,6 @@ void mach_update_work_offsets() { 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); @@ -425,29 +420,12 @@ void mach_update_work_offsets() { } -/* 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 @@ -618,7 +596,7 @@ void mach_set_model_target(float target[], float flag[]) { /* 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 @@ -875,9 +853,6 @@ stat_t mach_goto_g28_position(float target[], float flags[]) { // 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); @@ -895,9 +870,6 @@ stat_t mach_goto_g30_position(float target[], float flags[]) { // 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); @@ -1065,17 +1037,21 @@ void mach_message(const char *message) { */ -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); } diff --git a/src/machine.h b/src/machine.h index 342d7bf..f41d11f 100644 --- a/src/machine.h +++ b/src/machine.h @@ -317,8 +317,6 @@ path_mode_t mach_get_path_control(); 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); @@ -342,7 +340,6 @@ void mach_set_axis_jerk(uint8_t axis, float jerk); 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[]); diff --git a/src/plan/arc.c b/src/plan/arc.c index b8a346d..6578f73 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -44,10 +44,11 @@ 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 @@ -317,7 +318,7 @@ static stat_t _compute_arc(const float position[], float offset[], 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; @@ -325,8 +326,8 @@ static stat_t _compute_arc(const float position[], float offset[], 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; } @@ -432,7 +433,7 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints // 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 @@ -446,23 +447,32 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints * 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;} diff --git a/src/plan/buffer.c b/src/plan/buffer.c index 6ff9d73..b09cdfc 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -109,26 +109,24 @@ void mp_wait_for_buffer() {while (!mb.buffers_available) continue;} 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; } @@ -183,7 +181,7 @@ mp_buffer_t *mp_get_last_buffer() { 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; diff --git a/src/plan/buffer.h b/src/plan/buffer.h index b19f16c..3108fb4 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -108,7 +108,7 @@ void mp_commit_write_buffer(uint32_t line); 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); diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c index 23cb6fe..696d8fd 100644 --- a/src/plan/calibrate.c +++ b/src/plan/calibrate.c @@ -164,17 +164,12 @@ uint8_t command_calibrate(int argc, char *argv[]) { 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); diff --git a/src/plan/command.c b/src/plan/command.c index 80bb4c6..7b2654c 100644 --- a/src/plan/command.c +++ b/src/plan/command.c @@ -62,11 +62,6 @@ static stat_t _exec_command(mp_buffer_t *bf) { 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 diff --git a/src/plan/dwell.c b/src/plan/dwell.c index 1141bd4..c2dd079 100644 --- a/src/plan/dwell.c +++ b/src/plan/dwell.c @@ -47,10 +47,6 @@ static stat_t _exec_dwell(mp_buffer_t *bf) { /// 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 diff --git a/src/plan/exec.c b/src/plan/exec.c index 5d33bed..2c32853 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -457,10 +457,6 @@ static stat_t _exec_aline_init(mp_buffer_t *bf) { * 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: @@ -498,16 +494,17 @@ stat_t mp_exec_aline(mp_buffer_t *bf) { 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; @@ -552,8 +549,12 @@ stat_t mp_exec_move() { // 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()) { diff --git a/src/plan/jog.c b/src/plan/jog.c index f5dcddb..06a6c7f 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -125,12 +125,9 @@ uint8_t command_jog(int argc, char *argv[]) { 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); } diff --git a/src/plan/line.c b/src/plan/line.c index 9f6b802..2b982bd 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -237,7 +237,8 @@ static void _calc_and_cache_jerk_values(mp_buffer_t *bf) { 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); @@ -285,7 +286,6 @@ stat_t mp_aline(const float target[], int32_t line) { // 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; @@ -304,7 +304,7 @@ stat_t mp_aline(const float target[], int32_t line) { 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 diff --git a/src/plan/planner.c b/src/plan/planner.c index c98a070..729de32 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -262,7 +262,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { // 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; @@ -444,20 +444,19 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { /*** 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: * @@ -497,53 +496,54 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { * * 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; @@ -560,8 +560,9 @@ void mp_replan_blocks() { // 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 diff --git a/src/spindle.c b/src/spindle.c index 5cc12f5..c08f972 100644 --- a/src/spindle.c +++ b/src/spindle.c @@ -40,7 +40,15 @@ typedef enum { 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() { @@ -50,28 +58,38 @@ 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); } } diff --git a/src/spindle.h b/src/spindle.h index c31ec85..ef15e86 100644 --- a/src/spindle.h +++ b/src/spindle.h @@ -33,4 +33,6 @@ 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(); diff --git a/src/varcb.c b/src/varcb.c index 333ce1e..4f48ebf 100644 --- a/src/varcb.c +++ b/src/varcb.c @@ -28,6 +28,7 @@ #include "usart.h" #include "machine.h" +#include "spindle.h" #include "plan/runtime.h" #include "plan/state.h" @@ -37,7 +38,7 @@ float get_position(int index) {return mp_runtime_get_axis_position(index);} // 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();} -- 2.27.0