From 43eefefb3f78e73df36d73026e0d98c086edb732 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Tue, 6 Sep 2016 01:56:56 -0700 Subject: [PATCH] Fixed several bugs, cleaned up util.c, Automatically deallocate planning buffers and manage state, Fixed hold planning. --- src/homing.c | 8 +- src/machine.c | 4 +- src/plan/arc.c | 8 +- src/plan/buffer.c | 2 - src/plan/buffer.h | 6 +- src/plan/calibrate.c | 11 +-- src/plan/command.c | 28 ++----- src/plan/command.h | 3 +- src/plan/dwell.c | 7 +- src/plan/exec.c | 162 ++++++++++++++++++------------------ src/plan/jog.c | 18 ++-- src/plan/line.c | 3 +- src/plan/planner.c | 16 ++-- src/plan/runtime.c | 42 ++++++---- src/plan/runtime.h | 13 ++- src/plan/state.c | 6 +- src/probing.c | 1 - src/stepper.c | 22 +++-- src/stepper.h | 4 +- src/util.c | 190 +------------------------------------------ src/util.h | 88 ++++---------------- src/varcb.c | 2 +- 22 files changed, 200 insertions(+), 444 deletions(-) diff --git a/src/homing.c b/src/homing.c index 046361d..48eb2dc 100644 --- a/src/homing.c +++ b/src/homing.c @@ -26,6 +26,7 @@ \******************************************************************************/ +#include "homing.h" #include "machine.h" #include "switch.h" #include "util.h" @@ -190,7 +191,6 @@ static void _homing_finalize_exit() { mach_set_feed_rate_mode(hm.saved_feed_rate_mode); mach.gm.feed_rate = hm.saved_feed_rate; mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); - mp_set_cycle(CYCLE_MACHINING); } @@ -360,9 +360,8 @@ bool mach_is_homing() { void mach_set_not_homed() { - // TODO save homed to EEPROM for (int axis = 0; axis < AXES; axis++) - hm.homed[axis] = false; + mach_set_homed(axis, false); } @@ -372,6 +371,7 @@ bool mach_get_homed(int axis) { void mach_set_homed(int axis, bool homed) { + // TODO save homed to EEPROM hm.homed[axis] = homed; } @@ -388,7 +388,7 @@ void mach_homing_cycle_start() { // set working values mach_set_units_mode(MILLIMETERS); mach_set_distance_mode(INCREMENTAL_MODE); - mach_set_coord_system(ABSOLUTE_COORDS); // in machine coordinates + mach_set_coord_system(ABSOLUTE_COORDS); // in machine coordinates mach_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); hm.set_coordinates = true; diff --git a/src/machine.c b/src/machine.c index 1c159fc..b13fe3a 100644 --- a/src/machine.c +++ b/src/machine.c @@ -693,7 +693,7 @@ void mach_set_position(int axis, float position) { mach.position[axis] = position; mach.ms.target[axis] = position; mp_set_planner_position(axis, position); - mp_runtime_set_position(axis, position); + mp_runtime_set_axis_position(axis, position); mp_runtime_set_steps_from_position(); } @@ -702,7 +702,7 @@ void mach_set_position(int axis, float position) { static void _exec_absolute_origin(float *value, float *flag) { for (int axis = 0; axis < AXES; axis++) if (fp_TRUE(flag[axis])) { - mp_runtime_set_position(axis, value[axis]); + mp_runtime_set_axis_position(axis, value[axis]); mach_set_homed(axis, true); // G28.3 is not considered homed until here } diff --git a/src/plan/arc.c b/src/plan/arc.c index 42b3008..027a1f3 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -208,7 +208,7 @@ static stat_t _compute_arc_offsets_from_radius() { // issues. float disc = 4 * square(arc.radius) - (square(x) + square(y)); - float h_x2_div_d = (disc > 0) ? -sqrt(disc) / hypotf(x, y) : 0; + float h_x2_div_d = disc > 0 ? -sqrt(disc) / hypotf(x, y) : 0; // Invert the sign of h_x2_div_d if circle is counter clockwise (see header // notes) @@ -282,7 +282,7 @@ static stat_t _compute_arc() { // g18_correction is used to invert G18 XZ plane arcs for proper CW // orientation - float g18_correction = (mach.gm.select_plane == PLANE_XZ) ? -1 : 1; + float g18_correction = mach.gm.select_plane == PLANE_XZ ? -1 : 1; if (arc.full_circle) { // angular travel always starts as zero for full circles @@ -314,8 +314,8 @@ static stat_t _compute_arc() { // Add in travel for rotations if (mach.gm.motion_mode == MOTION_MODE_CW_ARC) - arc.angular_travel += (2*M_PI * arc.rotations * g18_correction); - else arc.angular_travel -= (2*M_PI * arc.rotations * g18_correction); + arc.angular_travel += 2 * M_PI * arc.rotations * g18_correction; + else arc.angular_travel -= 2 * M_PI * arc.rotations * g18_correction; // Calculate travel in the depth axis of the helix and compute the time it // should take to perform the move arc.length is the total mm of travel of diff --git a/src/plan/buffer.c b/src/plan/buffer.c index 8e70599..e090ed4 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -176,8 +176,6 @@ void mp_free_run_buffer() { // EMPTY current run buf & adv to next mb.r = mb.r->nx; // advance to next run buffer mb.buffers_available++; report_request(); - - if (mp_queue_empty()) mp_state_idle(); // if queue empty } diff --git a/src/plan/buffer.h b/src/plan/buffer.h index cde4784..4288ad3 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -44,7 +44,9 @@ typedef enum { typedef enum { MOVE_OFF, // move inactive (MUST BE ZERO) MOVE_NEW, // initial value + MOVE_INIT, // first run MOVE_RUN, // general run state (for non-acceleration moves) + MOVE_RESTART, // restart buffer when done } run_state_t; @@ -58,7 +60,7 @@ typedef enum { // Callbacks -typedef void (*mach_exec_t)(float[], float[]); +typedef void (*mach_func_t)(float[], float[]); struct mp_buffer_t; typedef stat_t (*bf_func_t)(struct mp_buffer_t *bf); @@ -68,7 +70,7 @@ typedef struct mp_buffer_t { // See Planning Velocity Notes struct mp_buffer_t *nx; // pointer to next buffer bf_func_t bf_func; // callback to buffer exec function - mach_exec_t mach_func; // callback to machine + mach_func_t mach_func; // callback to machine buffer_state_t buffer_state; // used to manage queuing/dequeuing move_type_t move_type; // used to dispatch to run routine diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c index 0709f48..5bf4344 100644 --- a/src/plan/calibrate.c +++ b/src/plan/calibrate.c @@ -77,8 +77,6 @@ static calibrate_t cal = {0}; static stat_t _exec_calibrate(mp_buffer_t *bf) { - if (bf->run_state == MOVE_NEW) bf->run_state = MOVE_RUN; - const float time = MIN_SEGMENT_TIME; // In minutes const float max_delta_v = JOG_ACCELERATION * time; @@ -111,9 +109,8 @@ static stat_t _exec_calibrate(mp_buffer_t *bf) { STATUS_DEBUG("%"PRIi32" steps %0.2f mm", steps, mm); tmc2660_set_stallguard_threshold(cal.motor, 63); - mp_free_run_buffer(); // Release buffer - mp_set_cycle(CYCLE_MACHINING); - return STAT_OK; + + return STAT_OK; // Done } else { motor_set_encoder(cal.motor, 0); @@ -127,7 +124,7 @@ static stat_t _exec_calibrate(mp_buffer_t *bf) { break; } - if (!cal.velocity) return STAT_OK; + if (!cal.velocity) return STAT_EAGAIN; // Compute travel float travel[AXES] = {0}; // In mm @@ -141,7 +138,7 @@ static stat_t _exec_calibrate(mp_buffer_t *bf) { float error[MOTORS] = {0}; st_prep_line(steps, error, time); - return STAT_OK; + return STAT_EAGAIN; } diff --git a/src/plan/command.c b/src/plan/command.c index 8778f7f..3d0f2ca 100644 --- a/src/plan/command.c +++ b/src/plan/command.c @@ -34,11 +34,10 @@ * (bf buffer) which sets some parameters and registers a callback to the * execution function in the machine. * - When the planning queue gets to the function it calls _exec_command() - * which loads a pointer to the bf buffer in stepper.c's next move. + * which loads a pointer to the function and its args in stepper.c's next + * move. * - When the runtime gets to the end of the current activity (sending steps, - * counting a dwell) it executes mp_command_callback which uses the callback - * function in the bf and the saved parameters in the vectors. - * - To finish up mp_command_callback() frees the bf buffer. + * counting a dwell) it executes the callback function passing the args. * * Doing it this way instead of synchronizing on queue empty simplifies the * handling of feedholds, feed overrides, buffer flushes, and thread blocking, @@ -54,13 +53,13 @@ /// Callback to execute command static stat_t _exec_command(mp_buffer_t *bf) { - st_prep_command(bf); - return STAT_OK; + st_prep_command(bf->mach_func, bf->ms.target, bf->unit); + return STAT_OK; // Done } /// Queue a synchronous Mcode, program control, or other command -void mp_queue_command(mach_exec_t mach_exec, float *value, float *flag) { +void mp_queue_command(mach_func_t mach_func, float values[], float flags[]) { mp_buffer_t *bf = mp_get_write_buffer(); if (!bf) { @@ -69,23 +68,14 @@ void mp_queue_command(mach_exec_t mach_exec, float *value, float *flag) { } bf->bf_func = _exec_command; // callback to planner queue exec function - bf->mach_func = mach_exec; // callback to machine exec function + bf->mach_func = mach_func; // callback to machine exec function // Store values and flags in planner buffer for (int axis = 0; axis < AXES; axis++) { - bf->ms.target[axis] = value[axis]; - bf->unit[axis] = flag[axis]; // flag vector in unit + bf->ms.target[axis] = values[axis]; + bf->unit[axis] = flags[axis]; // flag vector in unit } // Must be final operation before exit mp_commit_write_buffer(mach_get_line(), MOVE_TYPE_COMMAND); } - - -void mp_command_callback(mp_buffer_t *bf) { - // Use values & flags stored in mp_queue_command() - bf->mach_func(bf->ms.target, bf->unit); - - // Free buffer & perform cycle_end if planner is empty - mp_free_run_buffer(); -} diff --git a/src/plan/command.h b/src/plan/command.h index 8395f58..2a3c46e 100644 --- a/src/plan/command.h +++ b/src/plan/command.h @@ -29,5 +29,4 @@ #include "plan/buffer.h" -void mp_queue_command(mach_exec_t mach_exec, float *value, float *flag); -void mp_command_callback(mp_buffer_t *bf); +void mp_queue_command(mach_func_t mach_exec, float *value, float *flag); diff --git a/src/plan/dwell.c b/src/plan/dwell.c index 8eda647..5728131 100644 --- a/src/plan/dwell.c +++ b/src/plan/dwell.c @@ -40,11 +40,7 @@ /// Dwell execution static stat_t _exec_dwell(mp_buffer_t *bf) { st_prep_dwell(bf->ms.move_time); // in seconds - - // free buffer & perform cycle_end if planner is empty - mp_free_run_buffer(); - - return STAT_OK; + return STAT_OK; // Done } @@ -57,7 +53,6 @@ stat_t mp_dwell(float seconds, int32_t line) { bf->bf_func = _exec_dwell; // register callback to dwell start bf->ms.move_time = seconds; // in seconds, not minutes - bf->run_state = MOVE_NEW; // must be final operation before exit mp_commit_write_buffer(line, MOVE_TYPE_DWELL); diff --git a/src/plan/exec.c b/src/plan/exec.c index 648e712..7f33053 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -74,18 +74,18 @@ static stat_t _exec_aline_segment() { // waypoint, synchronize to the head, body or tail end. Otherwise, if not at // a section waypoint compute target from segment time and velocity. Don't // do waypoint correction if you are going into a hold. - if (!--ex.segment_count && !ex.section_new && mp_get_state() == STATE_RUNNING) + if (!--ex.segment_count && !ex.section_new && !ex.hold_planned) copy_vector(target, ex.waypoint[ex.section]); else { float segment_length = ex.segment_velocity * ex.segment_time; for (int i = 0; i < AXES; i++) - target[i] = mp_runtime_get_position(i) + ex.unit[i] * segment_length; + target[i] = mp_runtime_get_axis_position(i) + ex.unit[i] * segment_length; } - RITORNO(mp_runtime_move_to_target - (target, ex.segment_velocity, ex.segment_time)); + mp_runtime_set_velocity(ex.segment_velocity); + RITORNO(mp_runtime_move_to_target(target, ex.segment_time)); // Return EAGAIN to continue or OK if this segment is done return ex.segment_count ? STAT_EAGAIN : STAT_OK; @@ -330,8 +330,13 @@ static stat_t _exec_aline_head() { static float _compute_next_segment_velocity() { - if (ex.section == SECTION_BODY) return ex.segment_velocity; - return ex.segment_velocity + ex.forward_diff[4]; + if (ex.section_new) { + if (ex.section == SECTION_HEAD) return ex.entry_velocity; + else return ex.cruise_velocity; + } + + return ex.segment_velocity + + (ex.section == SECTION_BODY ? 0 : ex.forward_diff[4]); } @@ -348,24 +353,19 @@ static float _compute_next_segment_velocity() { * speed. */ static void _plan_hold() { - mp_buffer_t *bp = mp_get_run_buffer(); // working buffer pointer - if (!bp) return; // Oops! nothing's running + mp_buffer_t *bf = mp_get_run_buffer(); // working buffer pointer + if (!bf) return; // Oops! nothing's running // Examine and process current buffer and compute length left for decel float available_length = - get_axis_vector_length(ex.final_target, mp_runtime_get_position_vector()); + get_axis_vector_length(ex.final_target, mp_runtime_get_position()); // Compute next_segment velocity, velocity left to shed to brake to zero float braking_velocity = _compute_next_segment_velocity(); - // Distance to brake to zero from braking_velocity, bp is OK to use here - float braking_length = mp_get_target_length(braking_velocity, 0, bp); - - // Hack to prevent Case 2 moves for perfect-fit decels. Happens in - // homing situations. The real fix: The braking velocity cannot - // simply be the ex.segment_velocity as this is the velocity of the - // last segment, not the one that's going to be executed next. The - // braking_velocity needs to be the velocity of the next segment - // that has not yet been computed. In the mean time, this hack will work. - if (available_length < braking_length && fp_ZERO(bp->exit_velocity)) + // Distance to brake to zero from braking_velocity, bf is OK to use here + float braking_length = mp_get_target_length(braking_velocity, 0, bf); + + // Hack to prevent Case 2 moves for perfect-fit decels. Happens when homing. + if (available_length < braking_length && fp_ZERO(bf->exit_velocity)) braking_length = available_length; // Replan to decelerate @@ -380,17 +380,17 @@ static void _plan_hold() { ex.exit_velocity = 0; ex.tail_length = braking_length; - // Re-use bp+0 to be the hold point and to run the remaining block length - bp->length = available_length - braking_length; - bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); - bp->entry_vmax = 0; // set bp+0 as hold point - bp->run_state = MOVE_NEW; // tell _exec to re-use the bf buffer + // Re-use bf to run the remaining block length + bf->length = available_length - braking_length; + bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); + bf->entry_vmax = 0; + bf->run_state = MOVE_RESTART; // Restart the buffer when done } else { // Case 2: deceleration exceeds length remaining in buffer // Replan to minimum (but non-zero) exit velocity ex.tail_length = available_length; ex.exit_velocity = - braking_velocity - mp_get_target_velocity(0, available_length, bp); + braking_velocity - mp_get_target_velocity(0, available_length, bf); } } @@ -398,17 +398,7 @@ static void _plan_hold() { /// Initializes move runtime with a new planner buffer static stat_t _exec_aline_init(mp_buffer_t *bf) { // Remove zero length lines. Short lines have already been removed. - if (fp_ZERO(bf->length)) { - mp_runtime_set_busy(false); - bf->nx->replannable = false; // prevent overplanning (Note 2) - mp_free_run_buffer(); // free buffer - - return STAT_NOOP; - } - - // Take control of buffer - bf->run_state = MOVE_RUN; - bf->replannable = false; + if (fp_ZERO(bf->length)) return STAT_NOOP; // Initialize move copy_vector(ex.unit, bf->unit); @@ -426,16 +416,15 @@ static stat_t _exec_aline_init(mp_buffer_t *bf) { ex.hold_planned = false; // Update runtime - mp_runtime_set_busy(true); mp_runtime_set_work_offsets(bf->ms.work_offset); // Generate waypoints for position correction at section ends. This helps // negate floating point errors in the forward differencing code. for (int axis = 0; axis < AXES; axis++) { ex.waypoint[SECTION_HEAD][axis] = - mp_runtime_get_position(axis) + ex.unit[axis] * ex.head_length; + mp_runtime_get_axis_position(axis) + ex.unit[axis] * ex.head_length; - ex.waypoint[SECTION_BODY][axis] = mp_runtime_get_position(axis) + + ex.waypoint[SECTION_BODY][axis] = mp_runtime_get_axis_position(axis) + ex.unit[axis] * (ex.head_length + ex.body_length); ex.waypoint[SECTION_TAIL][axis] = ex.final_target[axis]; @@ -497,29 +486,22 @@ static stat_t _exec_aline_init(mp_buffer_t *bf) { * Period 3 V = Vi - Jm * (T^2) / 2 * Period 4 V = Vh + As * T + Jm * (T^2) / 2 * - * These routines play some games with the acceleration and move timing - * to make sure this actually work out. move_time is the actual time of - * the move, accel_time is the time value needed to compute the velocity - - * taking the initial velocity into account (move_time does not need to). - * - * State transitions - hierarchical state machine: - * - * bf->run_state transitions: - * - * from _NEW to _RUN on first call (sub_state set to _OFF) - * from _RUN to _OFF on final call or just remain _OFF + * move_time is the actual time of the move, accel_time is the time value + * needed to compute the velocity taking the initial velocity into account. + * move_time does not need to. */ stat_t mp_exec_aline(mp_buffer_t *bf) { - if (bf->run_state == MOVE_OFF) return STAT_NOOP; // No more moves - stat_t status = STAT_OK; // Start a new move - if (!mp_runtime_is_busy()) { + if (bf->run_state == MOVE_INIT) { + bf->run_state = MOVE_RUN; status = _exec_aline_init(bf); if (status != STAT_OK) return status; } + 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. switch (ex.section) { @@ -528,28 +510,7 @@ stat_t mp_exec_aline(mp_buffer_t *bf) { case SECTION_TAIL: status = _exec_aline_tail(); break; } - // There are 3 things that can happen here depending on return conditions: - // - // status bf->run_state Description - // ----------- -------------- ---------------------------------- - // STAT_EAGAIN buffer has more segments to run - // STAT_OK MOVE_RUN ex and bf buffers are done - // STAT_OK MOVE_NEW ex done; bf must be run again - // (it's been reused) - if (status != STAT_EAGAIN) { - mp_runtime_set_busy(false); - bf->nx->replannable = false; // prevent overplanning (Note 2) - if (fp_ZERO(ex.exit_velocity)) ex.segment_velocity = 0; - // Note, _plan_hold() may change bf->run_state to reuse this buffer so it - // can plan the deceleration. - if (bf->run_state == MOVE_RUN) mp_free_run_buffer(); - } - - if (mp_get_state() == STATE_STOPPING && - (status == STAT_OK || status == STAT_EAGAIN)) { - if (!mp_runtime_is_busy() && !ex.segment_velocity) mp_state_holding(); - else if (mp_runtime_is_busy() && !ex.hold_planned) _plan_hold(); - } + if (status != STAT_EAGAIN) mp_runtime_set_velocity(ex.exit_velocity); return status; } @@ -561,11 +522,52 @@ stat_t mp_exec_move() { if (mp_get_state() == STATE_HOLDING) return STAT_NOOP; mp_buffer_t *bf = mp_get_run_buffer(); - if (!bf) return STAT_NOOP; // nothing running - if (!bf->bf_func) return CM_ALARM(STAT_INTERNAL_ERROR); + if (!bf) return STAT_NOOP; // Nothing running + if (!bf->bf_func) return STAT_INTERNAL_ERROR; // Should never happen - // Update runtime - mp_runtime_set_line(bf->ms.line); + if (bf->run_state == MOVE_NEW) { + // Take control of buffer + bf->run_state = MOVE_INIT; + bf->replannable = false; + + // Update runtime + mp_runtime_set_busy(true); + mp_runtime_set_line(bf->ms.line); + } + + stat_t status = bf->bf_func(bf); // Move callback + + if (status != STAT_EAGAIN) { + bool idle = false; + + // Enter HOLDING state + if (mp_get_state() == STATE_STOPPING && + fp_ZERO(mp_runtime_get_velocity())) { + mp_state_holding(); + idle = true; + } - return bf->bf_func(bf); // move callback + // 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 + + // Enter READY state + if (mp_queue_empty()) { + mp_state_idle(); + idle = true; + } + + mp_set_cycle(CYCLE_MACHINING); // Default cycle + } + + // Queue idle + if (idle) { + mp_runtime_set_velocity(0); + mp_runtime_set_busy(false); + } + } + + return status; } diff --git a/src/plan/jog.c b/src/plan/jog.c index bd29da2..d652bbf 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -53,9 +53,6 @@ static jog_runtime_t jr; static stat_t _exec_jog(mp_buffer_t *bf) { - if (bf->run_state == MOVE_OFF) return STAT_NOOP; - if (bf->run_state == MOVE_NEW) bf->run_state = MOVE_RUN; - // Load next velocity if (!jr.writing) for (int axis = 0; axis < AXES; axis++) @@ -82,7 +79,7 @@ static stat_t _exec_jog(mp_buffer_t *bf) { // Compute target from axis velocity target[axis] = - mp_runtime_get_position(axis) + time * jr.current_velocity[axis]; + mp_runtime_get_axis_position(axis) + time * jr.current_velocity[axis]; // Accumulate velocities squared velocity_sqr += square(jr.current_velocity[axis]); @@ -94,15 +91,14 @@ static stat_t _exec_jog(mp_buffer_t *bf) { for (int axis = 0; axis < AXES; axis++) mach_set_position(axis, mp_runtime_get_work_position(axis)); - // Release buffer - mp_free_run_buffer(); - - mp_set_cycle(CYCLE_MACHINING); - - return STAT_NOOP; + return STAT_NOOP; // Done } - return mp_runtime_move_to_target(target, sqrt(velocity_sqr), time); + mp_runtime_set_velocity(sqrt(velocity_sqr)); + stat_t status = mp_runtime_move_to_target(target, time); + if (status != STAT_OK) return status; + + return STAT_EAGAIN; } diff --git a/src/plan/line.c b/src/plan/line.c index dc4b5b3..0718e6d 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -232,8 +232,7 @@ stat_t mp_aline(move_state_t *ms) { if (move_time < MIN_BLOCK_TIME) return STAT_MINIMUM_TIME_MOVE; } - // Get a *cleared* buffer and setup move variables - // Note, mp_free_run_buffer() initializes all buffer variables to zero + // 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); // never fails diff --git a/src/plan/planner.c b/src/plan/planner.c index b6090af..1fc625a 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -69,9 +69,7 @@ #include -void planner_init() { - mp_init_buffers(); -} +void planner_init() {mp_init_buffers();} /*** Flush all moves in the planner @@ -81,9 +79,7 @@ void planner_init() { * during a hold to reset the planner. This function should not usually * be directly called. Call mp_request_flush() instead. */ -void mp_flush_planner() { - mp_init_buffers(); -} +void mp_flush_planner() {mp_init_buffers();} /* Performs axis mapping & conversion of length units to steps (and deals @@ -432,7 +428,7 @@ 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 + * (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 @@ -540,15 +536,15 @@ void mp_plan_block_list(mp_buffer_t *bf) { void mp_replan_blocks() { mp_buffer_t *bf = mp_get_run_buffer(); + if (!bf) return; + mp_buffer_t *bp = bf; // Mark all blocks replanable while (true) { - if (bp->run_state != MOVE_OFF || fp_ZERO(bp->exit_velocity)) break; bp->replannable = true; - + if (bp->nx->run_state == MOVE_OFF || bp->nx == bf) break; bp = mp_get_next_buffer(bp); - if (bp == bf) break; // Avoid wrap around } // Plan blocks diff --git a/src/plan/runtime.c b/src/plan/runtime.c index ef0c680..04dc93a 100644 --- a/src/plan/runtime.c +++ b/src/plan/runtime.c @@ -78,6 +78,12 @@ void mp_runtime_set_line(int32_t line) { float mp_runtime_get_velocity() {return rt.velocity;} +void mp_runtime_set_velocity(float velocity) { + rt.velocity = velocity; + report_request(); +} + + /// Set encoder counts to the runtime position void mp_runtime_set_steps_from_position() { // Convert lengths to steps in floating point @@ -120,14 +126,21 @@ void mp_runtime_set_steps_from_position() { /// Set runtime position for a single axis -void mp_runtime_set_position(uint8_t axis, const float position) { +void mp_runtime_set_axis_position(uint8_t axis, const float position) { rt.position[axis] = position; + report_request(); } /// Returns current axis position in machine coordinates -float mp_runtime_get_position(uint8_t axis) {return rt.position[axis];} -float *mp_runtime_get_position_vector() {return rt.position;} +float mp_runtime_get_axis_position(uint8_t axis) {return rt.position[axis];} +float *mp_runtime_get_position() {return rt.position;} + + +void mp_runtime_set_position(float position[]) { + copy_vector(rt.position, position); + report_request(); +} /// Returns axis position in work coordinates that were in effect at plan time @@ -162,16 +175,16 @@ void mp_runtime_set_work_offsets(float offset[]) { * 90 100 -10 encoder is 10 steps behind commanded steps * -100 -90 -10 encoder is 10 steps behind commanded steps */ -stat_t mp_runtime_move_to_target(float target[], float velocity, float time) { +stat_t mp_runtime_move_to_target(float target[], float time) { // Bucket-brigade old target down the chain before getting new target - for (int i = 0; i < MOTORS; i++) { + for (int motor = 0; motor < MOTORS; motor++) { // previous segment's position, delayed by 1 segment - rt.steps.commanded[i] = rt.steps.position[i]; + rt.steps.commanded[motor] = rt.steps.position[motor]; // previous segment's target becomes position - rt.steps.position[i] = rt.steps.target[i]; + rt.steps.position[motor] = rt.steps.target[motor]; // current encoder position (aligns to commanded_steps) - rt.steps.encoder[i] = motor_get_encoder(i); - rt.steps.error[i] = rt.steps.encoder[i] - rt.steps.commanded[i]; + rt.steps.encoder[motor] = motor_get_encoder(motor); + rt.steps.error[motor] = rt.steps.encoder[motor] - rt.steps.commanded[motor]; } // Convert target position to steps. @@ -183,17 +196,14 @@ stat_t mp_runtime_move_to_target(float target[], float velocity, float time) { // works for Cartesian kinematics. Other kinematics may require // transforming travel distance as opposed to simply subtracting steps. float travel_steps[MOTORS]; - for (int i = 0; i < MOTORS; i++) - travel_steps[i] = rt.steps.target[i] - rt.steps.position[i]; + for (int motor = 0; motor < MOTORS; motor++) + travel_steps[motor] = rt.steps.target[motor] - rt.steps.position[motor]; // Call the stepper prep function RITORNO(st_prep_line(travel_steps, rt.steps.error, time)); - // Update position and velocity - copy_vector(rt.position, target); - rt.velocity = velocity; - - report_request(); // Position and possibly velocity have changed + // Update position + mp_runtime_set_position(target); return STAT_OK; } diff --git a/src/plan/runtime.h b/src/plan/runtime.h index 6a00dfa..802bbb0 100644 --- a/src/plan/runtime.h +++ b/src/plan/runtime.h @@ -39,12 +39,17 @@ int32_t mp_runtime_get_line(); void mp_runtime_set_line(int32_t line); float mp_runtime_get_velocity(); +void mp_runtime_set_velocity(float velocity); void mp_runtime_set_steps_from_position(); -void mp_runtime_set_position(uint8_t axis, const float position); -float mp_runtime_get_position(uint8_t axis); -float *mp_runtime_get_position_vector(); + +void mp_runtime_set_axis_position(uint8_t axis, const float position); +float mp_runtime_get_axis_position(uint8_t axis); + +float *mp_runtime_get_position(); +void mp_runtime_set_position(float position[]); + float mp_runtime_get_work_position(uint8_t axis); void mp_runtime_set_work_offsets(float offset[]); -stat_t mp_runtime_move_to_target(float target[], float velocity, float time); +stat_t mp_runtime_move_to_target(float target[], float time); diff --git a/src/plan/state.c b/src/plan/state.c index f700b79..310cd2c 100644 --- a/src/plan/state.c +++ b/src/plan/state.c @@ -95,7 +95,7 @@ static void _set_state(mp_state_t state) { void mp_set_cycle(mp_cycle_t cycle) { if (ps.cycle == cycle) return; // No change - if (ps.state != STATE_READY) { + if (ps.state != STATE_READY && cycle != CYCLE_MACHINING) { STATUS_ERROR(STAT_INTERNAL_ERROR, "Cannot transition to %S while %S", mp_get_cycle_pgmstr(cycle), mp_get_state_pgmstr(ps.state)); @@ -186,7 +186,7 @@ void mp_state_callback() { // Reset to actual machine position. Otherwise machine is set to the // position of the last queued move. for (int axis = 0; axis < AXES; axis++) - mach_set_position(axis, mp_runtime_get_position(axis)); + mach_set_position(axis, mp_runtime_get_axis_position(axis)); } // Resume @@ -203,7 +203,7 @@ void mp_state_callback() { if (mp_get_state() == STATE_HOLDING) { // Check if any moves are buffered - if (mp_get_run_buffer()) { + if (!mp_queue_empty()) { mp_replan_blocks(); _set_state(STATE_RUNNING); diff --git a/src/probing.c b/src/probing.c index 3c1dc5a..c9e2bee 100644 --- a/src/probing.c +++ b/src/probing.c @@ -99,7 +99,6 @@ static void _probe_restore_settings() { // update the model with actual position mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); - mp_set_cycle(CYCLE_MACHINING); } diff --git a/src/stepper.c b/src/stepper.c index f4db85b..112f54f 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -55,7 +55,9 @@ typedef struct { move_type_t move_type; uint16_t seg_period; uint32_t prep_dwell; - mp_buffer_t *bf; // used for command moves + mach_func_t mach_func; // used for command moves + float values[AXES]; + float flags[AXES]; } stepper_t; @@ -81,13 +83,19 @@ void st_shutdown() { /// Return true if motors or dwell are running -uint8_t st_is_busy() {return st.busy;} +bool st_is_busy() {return st.busy;} /// Interrupt handler for calling move exec function. /// ADC channel 0 triggered by load ISR as a "software" interrupt. ISR(ADCB_CH0_vect) { - mp_exec_move(); + stat_t status = mp_exec_move(); + + switch (status) { + case STAT_OK: case STAT_NOOP: case STAT_EAGAIN: break; // Ok + default: CM_ALARM(status); break; + } + ADCB_CH0_INTCTRL = 0; st.requesting = false; } @@ -143,7 +151,7 @@ ISR(STEP_TIMER_ISR) { st.dwell = st.prep_dwell; } else if (st.move_type == MOVE_TYPE_COMMAND) - mp_command_callback(st.bf); // Execute command + st.mach_func(st.values, st.flags); // Execute command // We are done with this move st.move_type = MOVE_TYPE_NULL; @@ -198,10 +206,12 @@ stat_t st_prep_line(float travel_steps[], float error[], float seg_time) { /// Stage command to execution -void st_prep_command(mp_buffer_t *bf) { +void st_prep_command(mach_func_t mach_func, float values[], float flags[]) { if (st.move_ready) CM_ALARM(STAT_INTERNAL_ERROR); st.move_type = MOVE_TYPE_COMMAND; - st.bf = bf; + st.mach_func = mach_func; + copy_vector(st.values, values); + copy_vector(st.flags, flags); st.move_ready = true; // signal prep buffer ready } diff --git a/src/stepper.h b/src/stepper.h index 60ff1c9..12edee8 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -37,8 +37,8 @@ void stepper_init(); void st_shutdown(); -uint8_t st_is_busy(); +bool st_is_busy(); stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time); -void st_prep_command(mp_buffer_t *bf); +void st_prep_command(mach_func_t mach_func, float values[], float flags[]); void st_prep_dwell(float seconds); diff --git a/src/util.c b/src/util.c index a092782..bbcad9e 100644 --- a/src/util.c +++ b/src/util.c @@ -26,195 +26,11 @@ \******************************************************************************/ -/* util contains: - * - math and min/max utilities and extensions - * - vector manipulation utilities - */ - #include "util.h" -#include - -#include -#include -#include -#include -#include - -float vector[AXES]; // statically allocated global for vector utilities - - -/// Test if vectors are equal -uint8_t vector_equal(const float a[], const float b[]) { - return - fp_EQ(a[AXIS_X], b[AXIS_X]) && fp_EQ(a[AXIS_Y], b[AXIS_Y]) && - fp_EQ(a[AXIS_Z], b[AXIS_Z]) && fp_EQ(a[AXIS_A], b[AXIS_A]) && - fp_EQ(a[AXIS_B], b[AXIS_B]) && fp_EQ(a[AXIS_C], b[AXIS_C]); -} - -/// Return the length of an axis vector float get_axis_vector_length(const float a[], const float b[]) { - return sqrt(square(a[AXIS_X] - b[AXIS_X] + - square(a[AXIS_Y] - b[AXIS_Y]) + - square(a[AXIS_Z] - b[AXIS_Z]) + - square(a[AXIS_A] - b[AXIS_A]) + - square(a[AXIS_B] - b[AXIS_B]) + - square(a[AXIS_C] - b[AXIS_C]))); -} - - -/// Load values into vector form -float *set_vector(float x, float y, float z, float a, float b, float c) { - vector[AXIS_X] = x; - vector[AXIS_Y] = y; - vector[AXIS_Z] = z; - vector[AXIS_A] = a; - vector[AXIS_B] = b; - vector[AXIS_C] = c; - - return vector; -} - - -/// load a single value into a zero vector -float *set_vector_by_axis(float value, uint8_t axis) { - clear_vector(vector); - - switch (axis) { - case AXIS_X: vector[AXIS_X] = value; break; - case AXIS_Y: vector[AXIS_Y] = value; break; - case AXIS_Z: vector[AXIS_Z] = value; break; - case AXIS_A: vector[AXIS_A] = value; break; - case AXIS_B: vector[AXIS_B] = value; break; - case AXIS_C: vector[AXIS_C] = value; break; - } - - return vector; -} - - -/**** Math and other general purpose functions ****/ - -/* Slightly faster (*) multi-value min and max functions - * min3() - return minimum of 3 numbers - * min4() - return minimum of 4 numbers - * max3() - return maximum of 3 numbers - * max4() - return maximum of 4 numbers - * - * Implementation tip: Order the min and max values from most to least likely - * in the calling args - * - * (*) Macro min4 is about 20usec, inline function version is closer to 10 - * usec (Xmega 32 MHz) - * #define min3(a,b,c) (min(min(a,b),c)) - * #define min4(a,b,c,d) (min(min(a,b),min(c,d))) - * #define max3(a,b,c) (max(max(a,b),c)) - * #define max4(a,b,c,d) (max(max(a,b),max(c,d))) - */ -float min3(float x1, float x2, float x3) { - float min = x1; - if (x2 < min) min = x2; - if (x3 < min) return x3; - return min; -} - - -float min4(float x1, float x2, float x3, float x4) { - float min = x1; - if (x2 < min) min = x2; - if (x3 < min) min = x3; - if (x4 < min) return x4; - return min; -} - - -float max3(float x1, float x2, float x3) { - float max = x1; - if (x2 > max) max = x2; - if (x3 > max) return x3; - return max; -} - - -float max4(float x1, float x2, float x3, float x4) { - float max = x1; - if (x2 > max) max = x2; - if (x3 > max) max = x3; - if (x4 > max) return x4; - return max; -} - - -/// isdigit that also accepts plus, minus, and decimal point -uint8_t isnumber(char c) { - return c == '.' || c == '-' || c == '+' || isdigit(c); -} - - -/// Add escapes to a string - currently for quotes only -char *escape_string(char *dst, char *src) { - char c; - char *start_dst = dst; - - while ((c = *(src++)) != 0) { // 0 - if (c == '"') *(dst++) = '\\'; - *(dst++) = c; - } - - return start_dst; -} - - -/* - * Return ASCII string given a float and a decimal precision value - * - * Returns length of string, less the terminating 0 character - */ -char fntoa(char *str, float n, uint8_t precision) { - // handle special cases - if (isnan(n)) { - strcpy(str, "nan"); - return 3; - } - - if (isinf(n)) { - strcpy(str, "inf"); - return 3; - } - - switch (precision) { - case 0: return (char)sprintf((char *)str, "%0.0f", (double)n); - case 1: return (char)sprintf((char *)str, "%0.1f", (double)n); - case 2: return (char)sprintf((char *)str, "%0.2f", (double)n); - case 3: return (char)sprintf((char *)str, "%0.3f", (double)n); - case 4: return (char)sprintf((char *)str, "%0.4f", (double)n); - case 5: return (char)sprintf((char *)str, "%0.5f", (double)n); - case 6: return (char)sprintf((char *)str, "%0.6f", (double)n); - case 7: return (char)sprintf((char *)str, "%0.7f", (double)n); - default: return (char)sprintf((char *)str, "%f", (double)n); - } -} - - -/* - * Calculate the checksum for a string - * - * Stops calculation on null termination or length value if non-zero. - * - * This is based on the the Java hashCode function. - * See http://en.wikipedia.org/wiki/Java_hashCode() - */ -#define HASHMASK 9999 - -uint16_t compute_checksum(char const *string, const uint16_t length) { - uint32_t h = 0; - uint16_t len = strlen(string); - - if (length) len = min(len, length); - - for (uint16_t i = 0; i < len; i++) - h = 31 * h + string[i]; - - return h % HASHMASK; + return sqrt(square(a[AXIS_X] - b[AXIS_X]) + square(a[AXIS_Y] - b[AXIS_Y]) + + square(a[AXIS_Z] - b[AXIS_Z]) + square(a[AXIS_A] - b[AXIS_A]) + + square(a[AXIS_B] - b[AXIS_B]) + square(a[AXIS_C] - b[AXIS_C])); } diff --git a/src/util.h b/src/util.h index d2e164e..d0a5bd2 100644 --- a/src/util.h +++ b/src/util.h @@ -26,13 +26,6 @@ \******************************************************************************/ -/* Supporting functions including: - * - * - math and min/max utilities and extensions - * - vector manipulation utilities - * - support for debugging routines - */ - #pragma once @@ -40,86 +33,35 @@ #include #include +#include -// Vector utilities -extern float vector[AXES]; // vector of axes for passing to subroutines +// Vector utilities #define clear_vector(a) memset(a, 0, sizeof(a)) #define copy_vector(d, s) memcpy(d, s, sizeof(d)) - float get_axis_vector_length(const float a[], const float b[]); -uint8_t vector_equal(const float a[], const float b[]); -float *set_vector(float x, float y, float z, float a, float b, float c); -float *set_vector_by_axis(float value, uint8_t axis); // Math utilities -float min3(float x1, float x2, float x3); -float min4(float x1, float x2, float x3, float x4); -float max3(float x1, float x2, float x3); -float max4(float x1, float x2, float x3, float x4); - - -// String utilities -uint8_t isnumber(char c); -char *escape_string(char *dst, char *src); -char fntoa(char *str, float n, uint8_t precision); -uint16_t compute_checksum(char const *string, const uint16_t length); - - -// side-effect safe forms of min and max -#ifndef max -#define max(a, b) \ - ({ __typeof__ (a) termA = (a); \ - __typeof__ (b) termB = (b); \ - termA > termB ? termA : termB;}) -#endif - -#ifndef min -#define min(a, b) \ - ({ __typeof__ (a) term1 = (a); \ - __typeof__ (b) term2 = (b); \ - term1 < term2 ? term1 : term2;}) -#endif - -#ifndef avg -#define avg(a, b) ((a + b) / 2) -#endif - -// allowable rounding error for floats -#ifndef EPSILON -#define EPSILON 0.00001 -#endif - -#ifndef fp_EQ +inline float min(float a, float b) {return a < b ? a : b;} +inline float max(float a, float b) {return a < b ? b : a;} +inline float min3(float a, float b, float c) {return min(min(a, b), c);} +inline float max3(float a, float b, float c) {return max(max(a, b), c);} +inline float min4(float a, float b, float c, float d) +{return min(min(a, b), min(c, d));} +inline float max4(float a, float b, float c, float d) +{return max(max(a, b), max(c, d));} + +// Floating-point utilities +#define EPSILON 0.00001 // allowable rounding error for floats #define fp_EQ(a, b) (fabs(a - b) < EPSILON) -#endif -#ifndef fp_NE #define fp_NE(a, b) (fabs(a - b) > EPSILON) -#endif -#ifndef fp_ZERO #define fp_ZERO(a) (fabs(a) < EPSILON) -#endif -#ifndef fp_NOT_ZERO #define fp_NOT_ZERO(a) (fabs(a) > EPSILON) -#endif -/// float is interpreted as FALSE (equals zero) -#ifndef fp_FALSE -#define fp_FALSE(a) (a < EPSILON) -#endif -/// float is interpreted as TRUE (not equal to zero) -#ifndef fp_TRUE -#define fp_TRUE(a) (a > EPSILON) -#endif +#define fp_FALSE(a) fp_ZERO(a) +#define fp_TRUE(a) fp_NOT_ZERO(a) // Constants -#define MAX_LONG 2147483647 -#define MAX_ULONG 4294967295 #define MM_PER_INCH 25.4 #define INCHES_PER_MM (1 / 25.4) #define MICROSECONDS_PER_MINUTE 60000000.0 #define usec(a) ((a) * MICROSECONDS_PER_MINUTE) - -#define RADIAN 57.2957795 -#ifndef M_SQRT3 -#define M_SQRT3 1.73205080756888 -#endif diff --git a/src/varcb.c b/src/varcb.c index e723bf7..d48e2a6 100644 --- a/src/varcb.c +++ b/src/varcb.c @@ -34,7 +34,7 @@ #include "plan/buffer.h" -float get_position(int index) {return mp_runtime_get_position(index);} +float get_position(int index) {return mp_runtime_get_axis_position(index);} float get_velocity() {return mp_runtime_get_velocity();} float get_speed() {return mach_get_spindle_speed();} float get_feed() {return mach_get_feed_rate();} -- 2.27.0