/* Jerk functions
*
- * Jerk values can be rather large, often in the billions. This makes
- * for some pretty big numbers for people to deal with. Jerk values
- * are stored in the system in truncated format; values are divided by
- * 1,000,000 then reconstituted before use.
+ * Jerk values can be rather large. Jerk values are stored in the system in
+ * truncated format; values are divided by 1,000,000 then multiplied before use.
*
- * The axis_jerk() functions expect the jerk in divided-by 1,000,000 form
+ * The axis_jerk() functions expect the jerk in divided by 1,000,000 form.
*/
-/// returns jerk for an axis
-float axes_get_jerk(uint8_t axis) {
- return axes[axis].jerk_max;
-}
+/// Returns axis jerk
+float axes_get_jerk(uint8_t axis) {return axes[axis].jerk_max;}
-/// sets the jerk for an axis, including recirpcal and cached values
+/// Sets jerk and its reciprocal for axis
void axes_set_jerk(uint8_t axis, float jerk) {
axes[axis].jerk_max = jerk;
axes[axis].recip_jerk = 1 / (jerk * JERK_MULTIPLIER);
}
-uint8_t get_axis_mode(int axis) {
- return axes[axis].axis_mode;
-}
+uint8_t get_axis_mode(int axis) {return axes[axis].axis_mode;}
void set_axis_mode(int axis, uint8_t value) {
- if (value < AXIS_MODE_MAX)
- axes[axis].axis_mode = value;
-}
-
-
-float get_max_velocity(int axis) {
- return axes[axis].velocity_max;
-}
-
-
-void set_max_velocity(int axis, float value) {
- axes[axis].velocity_max = value;
-}
-
-
-float get_max_feedrate(int axis) {
- return axes[axis].feedrate_max;
-}
-
-
-void set_max_feedrate(int axis, float value) {
- axes[axis].feedrate_max = value;
-}
-
-
-float get_max_jerk(int axis) {
- return axes[axis].jerk_max;
-}
-
-
-void set_max_jerk(int axis, float value) {
- axes[axis].jerk_max = value;
-}
-
-
-float get_junction_dev(int axis) {
- return axes[axis].junction_dev;
-}
-
-
-void set_junction_dev(int axis, float value) {
- axes[axis].junction_dev = value;
-}
-
-
-float get_travel_min(int axis) {
- return axes[axis].travel_min;
-}
-
-
-void set_travel_min(int axis, float value) {
- axes[axis].travel_min = value;
-}
-
-
-float get_travel_max(int axis) {
- return axes[axis].travel_max;
-}
-
-
-void set_travel_max(int axis, float value) {
- axes[axis].travel_max = value;
-}
-
-
-float get_jerk_homing(int axis) {
- return axes[axis].jerk_homing;
-}
-
-
-void set_jerk_homing(int axis, float value) {
- axes[axis].jerk_homing = value;
-}
-
-
-float get_search_vel(int axis) {
- return axes[axis].search_velocity;
-}
-
-
-void set_search_vel(int axis, float value) {
- axes[axis].search_velocity = value;
-}
-
-
-float get_latch_vel(int axis) {
- return axes[axis].latch_velocity;
-}
-
-
-void set_latch_vel(int axis, float value) {
- axes[axis].latch_velocity = value;
-}
-
-
-float get_latch_backoff(int axis) {
- return axes[axis].latch_backoff;
-}
+ if (value <= AXIS_RADIUS) axes[axis].axis_mode = value;
+}
+
+
+float get_max_velocity(int axis) {return axes[axis].velocity_max;}
+void set_max_velocity(int axis, float value) {axes[axis].velocity_max = value;}
+float get_max_feedrate(int axis) {return axes[axis].feedrate_max;}
+void set_max_feedrate(int axis, float value) {axes[axis].feedrate_max = value;}
+float get_max_jerk(int axis) {return axes[axis].jerk_max;}
+void set_max_jerk(int axis, float value) {axes[axis].jerk_max = value;}
+float get_junction_dev(int axis) {return axes[axis].junction_dev;}
+void set_junction_dev(int axis, float value) {axes[axis].junction_dev = value;}
+float get_travel_min(int axis) {return axes[axis].travel_min;}
+void set_travel_min(int axis, float value) {axes[axis].travel_min = value;}
+float get_travel_max(int axis) {return axes[axis].travel_max;}
+void set_travel_max(int axis, float value) {axes[axis].travel_max = value;}
+float get_jerk_homing(int axis) {return axes[axis].jerk_homing;}
+void set_jerk_homing(int axis, float value) {axes[axis].jerk_homing = value;}
+float get_search_vel(int axis) {return axes[axis].search_velocity;}
+void set_search_vel(int axis, float value) {axes[axis].search_velocity = value;}
+float get_latch_vel(int axis) {return axes[axis].latch_velocity;}
+void set_latch_vel(int axis, float value) {axes[axis].latch_velocity = value;}
+float get_latch_backoff(int axis) {return axes[axis].latch_backoff;}
void set_latch_backoff(int axis, float value) {
}
-float get_zero_backoff(int axis) {
- return axes[axis].zero_backoff;
-}
-
-
-void set_zero_backoff(int axis, float value) {
- axes[axis].zero_backoff = value;
-}
+float get_zero_backoff(int axis) {return axes[axis].zero_backoff;}
+void set_zero_backoff(int axis, float value) {axes[axis].zero_backoff = value;}
typedef enum {
- AXIS_DISABLED, // disable axis
- AXIS_STANDARD, // axis in coordinated motion w/standard behaviors
- AXIS_INHIBITED, // axis is computed but not activated
- AXIS_RADIUS, // rotary axis calibrated to circumference
- AXIS_MODE_MAX,
+ AXIS_DISABLED, // disabled axis
+ AXIS_STANDARD, // axis in coordinated motion w/standard behaviors
+ AXIS_RADIUS, // rotary axis calibrated to circumference
} axis_mode_t;
void mach_set_spindle_speed(float speed) {
mp_buffer_t *bf = mp_queue_get_tail();
bf->value = speed * mach_get_spindle_override();
- mp_queue_push(_exec_spindle_speed, false, mach_get_line());
+ mp_queue_push_nonstop(_exec_spindle_speed, mach_get_line());
}
void mach_set_spindle_mode(spindle_mode_t mode) {
mp_buffer_t *bf = mp_queue_get_tail();
bf->value = mode;
- mp_queue_push(_exec_spindle_mode, false, mach_get_line());
+ mp_queue_push_nonstop(_exec_spindle_mode, mach_get_line());
}
if (!same) {
mp_buffer_t *bf = mp_queue_get_tail();
copy_vector(bf->target, work_offset);
- mp_queue_push(_exec_update_work_offsets, false, mach_get_line());
+ mp_queue_push_nonstop(_exec_update_work_offsets, mach_get_line());
}
}
* - conversion of relative mode to absolute (internal canonical form)
* - translation of work coordinates to machine coordinates (internal
* canonical form)
- * - computation and application of axis modes as so:
- *
- * DISABLED - Incoming value is ignored. Target value is not changed
- * ENABLED - Convert axis values to canonical format and store as target
- * INHIBITED - Same processing as ENABLED, but axis will not actually be run
- * RADIUS - ABC axis value is provided in Gcode block in linear units
- * - Target is set to degrees based on axis' Radius value
- * - Radius mode is only processed for ABC axes. Application to
- * XYZ is ignored.
- *
- * Target coordinates are provided in target[]
- * Axes that need processing are signaled in flag[]
+ * - application of axis modes:
+ *
+ * DISABLED - Incoming value is ignored.
+ * ENABLED - Convert axis values to canonical format.
+ * RADIUS - ABC axis value is provided in Gcode block in linear units.
+ * - Target is set to degrees based on axis' Radius value.
+ *
+ * Target coordinates are provided in @param values.
+ * Axes that need processing are signaled in @param flags.
*/
-void mach_calc_model_target(float target[], const float values[],
- const bool flags[]) {
- // process XYZABC for lower modes
- for (int axis = AXIS_X; axis <= AXIS_Z; axis++) {
- if (!flags[axis] || axes[axis].axis_mode == AXIS_DISABLED)
- continue; // skip axis if not flagged for update or its disabled
-
- if (axes[axis].axis_mode == AXIS_STANDARD ||
- axes[axis].axis_mode == AXIS_INHIBITED) {
- if (mach.gm.distance_mode == ABSOLUTE_MODE)
- target[axis] =
- mach_get_active_coord_offset(axis) + TO_MILLIMETERS(values[axis]);
- else target[axis] = mach.position[axis] + TO_MILLIMETERS(values[axis]);
- }
- }
+void mach_calc_target(float target[], const float values[],
+ const bool flags[]) {
+ for (int axis = 0; axis < AXES; axis++) {
+ target[axis] = mach.position[axis];
+ if (!flags[axis]) continue;
- // Note: The ABC loop below relies on the XYZ loop having been run first
- for (int axis = AXIS_A; axis <= AXIS_C; axis++) {
- if (!flags[axis] || axes[axis].axis_mode == AXIS_DISABLED)
- continue; // skip axis if not flagged for update or its disabled
+ const float offset = mach.gm.distance_mode == ABSOLUTE_MODE ?
+ mach_get_active_coord_offset(axis) : mach.position[axis];
- float tmp;
switch (axes[axis].axis_mode) {
+ case AXIS_DISABLED: break;
case AXIS_STANDARD:
- case AXIS_INHIBITED:
- tmp = values[axis]; // no mm conversion - it's in degrees
-
- default:
- tmp = TO_MILLIMETERS(values[axis]) * 360 / (2 * M_PI * axes[axis].radius);
+ // For ABC axes no mm conversion - it's already in degrees
+ target[axis] =
+ offset + (AXIS_Z < axis ? values[axis] : TO_MM(values[axis]));
+ break;
+
+ case AXIS_RADIUS:
+ target[axis] =
+ offset + TO_MM(values[axis]) * 360 / (2 * M_PI * axes[axis].radius);
+ break;
}
-
- if (mach.gm.distance_mode == ABSOLUTE_MODE)
- // sacidu93's fix to Issue #22
- target[axis] = tmp + mach_get_active_coord_offset(axis);
- else target[axis] = mach.position[axis] + tmp;
}
}
/*** Return error code if soft limit is exceeded
*
* Must be called with target properly set in GM struct. Best done
- * after mach_calc_model_target().
+ * after mach_calc_target().
*
* Tests for soft limit for any homed axis if min and max are
* different values. You can set min and max to 0,0 to disable soft
for (int axis = 0; axis < AXES; axis++)
if (flags[axis])
- mach.offset[coord_system][axis] = TO_MILLIMETERS(offset[axis]);
+ mach.offset[coord_system][axis] = TO_MM(offset[axis]);
}
for (int axis = 0; axis < AXES; axis++)
if (flags[axis]) {
- value[axis] = TO_MILLIMETERS(origin[axis]);
+ value[axis] = TO_MM(origin[axis]);
mach.position[axis] = value[axis]; // set model position
mp_set_axis_position(axis, value[axis]); // set mm position
}
mp_buffer_t *bf = mp_queue_get_tail();
copy_vector(bf->target, origin);
copy_vector(bf->unit, flags);
- mp_queue_push(_exec_absolute_origin, false, mach_get_line());
+ mp_queue_push_nonstop(_exec_absolute_origin, mach_get_line());
}
for (int axis = 0; axis < AXES; axis++)
if (flags[axis])
mach.origin_offset[axis] = mach.position[axis] -
- mach.offset[mach.gm.coord_system][axis] - TO_MILLIMETERS(offset[axis]);
+ mach.offset[mach.gm.coord_system][axis] - TO_MM(offset[axis]);
}
// Free Space Motion (4.3.4)
static stat_t _feed(float values[], bool flags[]) {
float target[AXES];
- copy_vector(target, mach.position);
- mach_calc_model_target(target, values, flags);
+ mach_calc_target(target, values, flags);
// test soft limits
stat_t status = mach_test_soft_limits(target);
// normalize to minutes (active for this gcode block only)
mach.gm.feed_rate = feed_rate ? 1 / feed_rate : 0; // Avoid div by zero
- else mach.gm.feed_rate = TO_MILLIMETERS(feed_rate);
+ else mach.gm.feed_rate = TO_MM(feed_rate);
}
void mach_change_tool(bool x) {
mp_buffer_t *bf = mp_queue_get_tail();
bf->value = mach.gm.tool;
- mp_queue_push(_exec_change_tool, false, mach_get_line());
+ mp_queue_push_nonstop(_exec_change_tool, mach_get_line());
}
void mach_mist_coolant_control(bool mist_coolant) {
mp_buffer_t *bf = mp_queue_get_tail();
bf->value = mist_coolant;
- mp_queue_push(_exec_mist_coolant, false, mach_get_line());
+ mp_queue_push_nonstop(_exec_mist_coolant, mach_get_line());
}
void mach_flood_coolant_control(bool flood_coolant) {
mp_buffer_t *bf = mp_queue_get_tail();
bf->value = flood_coolant;
- mp_queue_push(_exec_flood_coolant, false, mach_get_line());
+ mp_queue_push_nonstop(_exec_flood_coolant, mach_get_line());
}
/// M0 Queue a program stop
void mach_program_stop() {
- mp_queue_push(_exec_program_stop, true, mach_get_line());
+ mp_queue_push(_exec_program_stop, mach_get_line());
}
#include "gcode_state.h"
-#define TO_MILLIMETERS(a) (mach_get_units() == INCHES ? (a) * MM_PER_INCH : a)
+#define TO_MM(a) (mach_get_units() == INCHES ? (a) * MM_PER_INCH : a)
// Model state getters and setters
uint32_t mach_get_line();
// Critical helpers
float mach_calc_move_time(const float axis_length[], const float axis_square[]);
-void mach_calc_model_target(float target[], const float values[],
+void mach_calc_target(float target[], const float values[],
const bool flags[]);
stat_t mach_test_soft_limits(float target[]);
// Test radius arcs for radius tolerance
if (radius_f) {
- arc.radius = TO_MILLIMETERS(radius); // set to internal format (mm)
+ arc.radius = TO_MM(radius); // set to internal format (mm)
if (fabs(arc.radius) < MIN_ARC_RADIUS) // radius value must be > minimum
return STAT_ARC_RADIUS_OUT_OF_TOLERANCE;
// Set model target
const float *position = mach_get_position();
- copy_vector(arc.target, position);
- mach_calc_model_target(arc.target, values, values_f);
+ mach_calc_target(arc.target, values, values_f);
// in radius mode it's an error for start == end
if (radius_f && fp_EQ(position[AXIS_X], arc.target[AXIS_X]) &&
mach_set_motion_mode(motion_mode);
mach_update_work_offsets(); // Update resolved offsets
arc.line = mach_get_line(); // copy line number
- arc.radius = TO_MILLIMETERS(radius); // set arc radius or zero
+ arc.radius = TO_MM(radius); // set arc radius or zero
float offset[3];
- for (int i = 0; i < 3; i++) offset[i] = TO_MILLIMETERS(offsets[i]);
+ for (int i = 0; i < 3; i++) offset[i] = TO_MM(offsets[i]);
if (mach_get_arc_distance_mode() == ABSOLUTE_MODE) {
if (offsets_f[0]) offset[0] -= position[0];
#include "buffer.h"
#include "state.h"
#include "rtc.h"
+#include "util.h"
#include <string.h>
+#include <math.h>
+#include <stdio.h>
typedef struct {
/// Get pointer to next buffer, waiting until one is available.
mp_buffer_t *mp_queue_get_tail() {
while (!mb.space) continue; // Wait for a buffer
- mb.tail->run_state = MOVE_NEW;
return mb.tail;
}
* buffer once it has been queued. Action may start on the buffer immediately,
* invalidating its contents
*/
-void mp_queue_push(buffer_cb_t cb, bool plan, uint32_t line) {
+void mp_queue_push(buffer_cb_t cb, uint32_t line) {
mp_state_running();
mb.tail->ts = rtc_get_time();
- mb.tail->plan = plan;
mb.tail->cb = cb;
mb.tail->line = line;
+ mb.tail->run_state = MOVE_NEW;
_push();
}
_clear_buffer(mb.head);
_pop();
}
-
-
-mp_buffer_t *mp_buffer_prev_plan(mp_buffer_t *bp) {
- for (int i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) {
- bp = mp_buffer_prev(bp);
- if (bp->run_state == MOVE_OFF) break;
- if (bp->plan) return bp;
- }
-
- return 0;
-}
-
-
-mp_buffer_t *mp_buffer_next_plan(mp_buffer_t *bp) {
- for (int i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) {
- bp = mp_buffer_next(bp);
- if (bp->run_state == MOVE_OFF) break;
- if (bp->plan) return bp;
- }
-
- return 0;
-}
buffer_cb_t cb; // callback to buffer exec function
run_state_t run_state; // run state machine sequence
- bool plan; // if false, ignored by the planner
bool replannable; // true if move can be re-planned
float value; // used in dwell and other callbacks
bool mp_queue_is_empty();
mp_buffer_t *mp_queue_get_tail();
-void mp_queue_push(buffer_cb_t func, bool plan, uint32_t line);
+void mp_queue_push(buffer_cb_t func, uint32_t line);
mp_buffer_t *mp_queue_get_head();
void mp_queue_pop();
static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->prev;}
static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->next;}
-mp_buffer_t *mp_buffer_prev_plan(mp_buffer_t *bp);
-mp_buffer_t *mp_buffer_next_plan(mp_buffer_t *bp);
mp_set_cycle(CYCLE_CALIBRATING);
cal.motor = 1;
- mp_queue_push(_exec_calibrate, false, -1);
+ mp_queue_push_nonstop(_exec_calibrate, -1);
return 0;
}
stat_t mp_dwell(float seconds, int32_t line) {
mp_buffer_t *bf = mp_queue_get_tail();
bf->value = seconds; // in seconds, not minutes
- mp_queue_push(_exec_dwell, true, line);
+ mp_queue_push(_exec_dwell, line);
return STAT_OK;
}
#include "runtime.h"
#include "state.h"
#include "rtc.h"
+#include "usart.h"
#include "config.h"
#include <string.h>
static float _compute_next_segment_velocity() {
if (ex.section_new) {
- if (ex.section == SECTION_HEAD) return ex.entry_velocity;
+ if (ex.section == SECTION_HEAD) return mp_runtime_get_velocity();
else return ex.cruise_velocity;
}
ex.cruise_velocity = braking_velocity;
ex.hold_planned = true;
- // Case 1: deceleration fits entirely into the length remaining in buffer
- if (braking_length <= available_length) {
- // Set to a tail to perform the deceleration
+ // Avoid creating segments before or after the hold which are too small.
+ if (fabs(available_length - braking_length) < HOLD_DECELERATION_TOLERANCE) {
+ // Case 0: deceleration fits almost exactly
+ ex.exit_velocity = 0;
+ ex.tail_length = available_length;
+
+ } else if (braking_length <= available_length) {
+ // Case 1: deceleration fits entirely into the remaining length
+ // Setup tail to perform the deceleration
ex.exit_velocity = 0;
ex.tail_length = braking_length;
bf->entry_vmax = 0;
bf->run_state = MOVE_RESTART; // Restart the buffer when done
- } else { // Case 2: deceleration exceeds length remaining in buffer
+ } 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 =
if (bf->run_state == MOVE_NEW) {
// On restart wait a bit to give planner queue a chance to fill
if (!mp_runtime_is_busy() && mp_queue_get_fill() < 4 &&
- !rtc_expired(bf->ts + 250)) return STAT_NOOP;
+ !rtc_expired(bf->ts + 250)) return STAT_NOOP;
// Take control of buffer
bf->run_state = MOVE_INIT;
bf->replannable = false;
// Update runtime
- mp_runtime_set_busy(true);
mp_runtime_set_line(bf->line);
}
stat_t status = bf->cb(bf); // Move callback
+ // Busy only if a move was queued
+ if (status == STAT_EAGAIN || status == STAT_OK) mp_runtime_set_busy(true);
+
if (status != STAT_EAGAIN) {
bool idle = false;
// 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_t *bp = mp_buffer_next_plan(bf);
- if (bp) bp->replannable = false;
+ mp_buffer_next(bf)->replannable = false;
- mp_queue_pop(); // Free buffer
+ mp_queue_pop(); // Release buffer
// Enter READY state
if (mp_queue_is_empty()) {
if (!mp_jog_busy()) {
mp_set_cycle(CYCLE_JOGGING);
- mp_queue_push(_exec_jog, false, -1);
+ mp_queue_push_nonstop(_exec_jog, -1);
}
return STAT_OK;
b_delta += square(b_unit[axis] * axes[axis].junction_dev);
}
+ if (!a_delta || !b_delta) return 0; // One or both unit vectors are null
+
float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2;
float sintheta_over2 = sqrt((1 - costheta) / 2);
float radius = delta * sintheta_over2 / (1 - sintheta_over2);
static void _calc_max_velocities(mp_buffer_t *bf, float move_time) {
- mp_buffer_t *bp = mp_buffer_prev_plan(bf);
- float junction_velocity = bp ? _get_junction_vmax(bp->unit, bf->unit) : 0;
+ 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);
_calc_max_velocities(bf, time);
// Note, the following lines must remain in order.
- bf->plan = true;
mp_plan_block_list(bf); // Plan block list
mp_set_position(target); // Set planner position before committing buffer
- mp_queue_push(mp_exec_aline, true, line); // After position update
+ mp_queue_push(mp_exec_aline, line); // After position update
return STAT_OK;
}
#include "stepper.h"
#include "motor.h"
#include "state.h"
+#include "usart.h"
#include <string.h>
#include <stdbool.h>
void mp_flush_planner() {mp_queue_init();}
-/* Performs axis mapping & conversion of length units to steps (and deals
- * with inhibited axes)
+/*** Performs axis mapping & conversion of length units to steps.
*
* The reason steps are returned as floats (as opposed to, say,
* uint32_t) is to accommodate fractional steps. stepper.c deals
// Most of the conversion math has already been done during config in
// steps_per_unit() which takes axis travel, step angle and microsteps into
// account.
- for (int motor = 0; motor < MOTORS; motor++) {
- int axis = motor_get_axis(motor);
- if (axes[axis].axis_mode == AXIS_INHIBITED) steps[motor] = 0;
- else steps[motor] = travel[axis] * motor_get_steps_per_unit(motor);
- }
+ for (int motor = 0; motor < MOTORS; motor++)
+ steps[motor] =
+ travel[motor_get_axis(motor)] * motor_get_steps_per_unit(motor);
}
*/
void mp_calculate_trapezoid(mp_buffer_t *bf) {
// RULE #1 of mp_calculate_trapezoid(): Don't change bf->length
- //
+
+ if (!bf->length) return;
+
// F case: Block is too short - run time < minimum segment time
// Force block into a single segment body with limited velocities
// Accept the entry velocity, limit the cruise, and go for the best exit
if (naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) {
bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN;
- bf->exit_velocity = max(0.0, min(bf->cruise_velocity,
- (bf->entry_velocity - bf->delta_vmax)));
+ bf->exit_velocity =
+ max(0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax)));
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
// B" case: Block is short, but fits into a single body segment
if (naive_move_time <= NOM_SEGMENT_TIME) {
- mp_buffer_t *bp = mp_buffer_prev_plan(bf);
- bf->entry_velocity = bp ? bp->exit_velocity : 0;
+ bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity;
if (fp_NOT_ZERO(bf->entry_velocity)) {
bf->cruise_velocity = bf->entry_velocity;
}
+void mp_print_queue(mp_buffer_t *bf) {
+ printf_P(PSTR("{\"msg\":\",id,replannable,callback,"
+ "length,head_length,body_length,tail_length,"
+ "entry_velocity,cruise_velocity,exit_velocity,braking_velocity,"
+ "entry_vmax,cruise_vmax,exit_vmax,\"}\n"));
+
+ int i = 0;
+ mp_buffer_t *bp = bf;
+ while (bp) {
+ printf_P(PSTR("{\"msg\":\",%d,%d,0x%04x,"
+ "%0.2f,%0.2f,%0.2f,%0.2f,"
+ "%0.2f,%0.2f,%0.2f,%0.2f,"
+ "%0.2f,%0.2f,%0.2f,\"}\n"),
+ i++, bp->replannable, bp->cb,
+ bp->length, bp->head_length, bp->body_length, bp->tail_length,
+ bp->entry_velocity, bp->cruise_velocity, bp->exit_velocity,
+ bp->braking_velocity,
+ bp->entry_vmax, bp->cruise_vmax, bp->exit_vmax);
+
+ bp = mp_buffer_prev(bp);
+ if (bp == bf || bp->run_state == MOVE_OFF) break;
+ }
+
+ while (!usart_tx_empty()) continue;
+}
+
+
/*** Plans the entire block list
*
* The block list is the circular buffer of planner buffers (bf's). The block
* optimizations.
*/
void mp_plan_block_list(mp_buffer_t *bf) {
- ASSERT(bf->plan); // Must start with a plannable buffer
-
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.
mp_buffer_t *next = bp;
while ((bp = mp_buffer_prev(bp)) != bf) {
- if (!bp->plan && bp->run_state != MOVE_OFF) continue;
if (!bp->replannable) break;
bp->braking_velocity =
min(next->entry_vmax, next->braking_velocity) + bp->delta_vmax;
// Forward planning pass. Recompute trapezoids from the first block to bf.
mp_buffer_t *prev = bp;
while ((bp = mp_buffer_next(bp)) != bf) {
- if (!bp->plan) continue;
-
- if (prev == bf) bp->entry_velocity = bp->entry_vmax; // first block
- else bp->entry_velocity = prev->exit_velocity; // other blocks
-
- // Note, next cannot be null. Since bp != bf, bf is yet to come.
- mp_buffer_t *next = mp_buffer_next_plan(bp);
- ASSERT(next);
+ mp_buffer_t *next = mp_buffer_next(bp);
+ bp->entry_velocity = prev == bf ? bp->entry_vmax : prev->exit_velocity;
bp->cruise_velocity = bp->cruise_vmax;
bp->exit_velocity = min4(bp->exit_vmax, next->entry_vmax,
next->braking_velocity,
}
// Finish last block
- bp->entry_velocity = prev->exit_velocity;
- bp->cruise_velocity = bp->cruise_vmax;
- bp->exit_velocity = 0;
+ bf->entry_velocity = prev->exit_velocity;
+ bf->cruise_velocity = bf->cruise_vmax;
+ bf->exit_velocity = 0;
- mp_calculate_trapezoid(bp);
+ mp_calculate_trapezoid(bf);
+
+ //mp_print_queue(bf);
}
mp_buffer_t *bp = bf;
- // Skip leading non-plannable blocks
- while (!bp->plan) {
- bp = mp_buffer_next(bp);
- if (bp->run_state == MOVE_OFF || bp == bf) return; // Nothing to plan
- }
-
// Mark all blocks replanable
while (true) {
bp->replannable = true;
}
+/// Push a non-stop command to the queue. I.e. one that does not cause the
+/// planner to plan to zero.
+void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line) {
+ mp_buffer_t *bp = mp_queue_get_tail();
+
+ bp->entry_vmax = bp->cruise_vmax = bp->exit_vmax = INFINITY;
+ copy_vector(bp->unit, bp->prev->unit);
+ bp->replannable = true;
+
+ mp_queue_push(cb, line);
+}
+
+
/*** Derive accel/decel length from delta V and jerk
*
* A convenient function for determining the optimal_length (L) of a line
/// Adaptive velocity tolerance term
#define TRAPEZOID_VELOCITY_TOLERANCE (max(2, bf->entry_velocity / 100))
+/*** If the absolute value of the remaining deceleration length would be less
+ * than this value then finish the deceleration in the current move. This is
+ * used to avoid creating segements before or after the hold which are too
+ * short to process correctly.
+ */
+#define HOLD_DECELERATION_TOLERANCE 1 // In mm
typedef enum {
SECTION_HEAD, // acceleration
void mp_kinematics(const float travel[], float steps[]);
void mp_plan_block_list(mp_buffer_t *bf);
void mp_replan_blocks();
+void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line);
float mp_get_target_length(float Vi, float Vf, const mp_buffer_t *bf);
float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf);
VAR(max_feedrate, "fr", float, AXES, 1, 1, "Maxium feedrate in mm/min")
VAR(max_jerk, "jm", float, AXES, 1, 1, "Maxium jerk in mm/min^3")
VAR(junction_dev, "jd", float, AXES, 1, 1, "Junction deviation")
-
VAR(travel_min, "tn", float, AXES, 1, 1, "Minimum soft limit")
VAR(travel_max, "tm", float, AXES, 1, 1, "Maximum soft limit")
-
VAR(jerk_homing, "jh", float, AXES, 1, 1, "Maxium homing jerk")
VAR(search_vel, "sv", float, AXES, 1, 1, "Homing search velocity")
VAR(latch_vel, "lv", float, AXES, 1, 1, "Homing latch velocity")
projects / bbctrl-firmware / commitdiff