// Compile-time settings
-//#define __STEP_CORRECTION
#define __CLOCK_EXTERNAL_16MHZ // uses PLL to provide 32 MHz system clock
//#define __CLOCK_INTERNAL_32MHZ
+
#define AXES 6 // number of axes
#define MOTORS 4 // number of motors on the board
#define COORDS 6 // number of supported coordinate systems
} axis_t;
-// Motor settings
+// Motor settings. See motor.c
#define MOTOR_CURRENT 0.8 // 1.0 is full power
#define MOTOR_IDLE_CURRENT 0.1 // 1.0 is full power
#define MOTOR_MICROSTEPS 16
-#define MOTOR_POWER_MODE MOTOR_POWERED_ONLY_WHEN_MOVING // See stepper.c
+#define MOTOR_POWER_MODE MOTOR_POWERED_ONLY_WHEN_MOVING
#define MOTOR_IDLE_TIMEOUT 2 // secs, motor off after this time
#define M1_MOTOR_MAP AXIS_X
// Machine settings
-#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing
-#define JERK_MAX 50 // yes, that's km/min^3
-#define JUNCTION_DEVIATION 0.05 // default value, in mm
-#define JUNCTION_ACCELERATION 100000 // centripetal corner acceleration
-#define JOG_ACCELERATION 500000 // mm/min^2
+#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arcs
+#define JERK_MAX 50 // yes, that's km/min^3
+#define JUNCTION_DEVIATION 0.05 // default value, in mm
+#define JUNCTION_ACCELERATION 100000 // centripetal corner accel
+#define JOG_JERK_MULT 4 // Jogging jerk multipler
+#define JOG_MIN_VELOCITY 10 // mm/min
+#define CAL_ACCELERATION 500000 // mm/min^2
// Axis settings
-#define VELOCITY_MAX 13000 // mm/min
+#define VELOCITY_MAX 13000 // mm/min
#define FEEDRATE_MAX VELOCITY_MAX
#define X_AXIS_MODE AXIS_STANDARD // See machine.h
#define Y_ZERO_BACKOFF 1
#define Z_AXIS_MODE AXIS_STANDARD
-#define Z_VELOCITY_MAX 2000 //VELOCITY_MAX
+#define Z_VELOCITY_MAX 2000 // VELOCITY_MAX
#define Z_FEEDRATE_MAX FEEDRATE_MAX
#define Z_TRAVEL_MIN 0
#define Z_TRAVEL_MAX 75
// Gcode defaults
#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES
-#define GCODE_DEFAULT_PLANE PLANE_XY // See machine.h
-#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58 or G59
+#define GCODE_DEFAULT_PLANE PLANE_XY // See machine.h
+#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58, G59
#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS
#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE
#define GCODE_DEFAULT_ARC_DISTANCE_MODE INCREMENTAL_MODE
*/
// Timer assignments - see specific modules for details
-#define TIMER_STEP TCC0 // Step timer (see stepper.h)
-#define TIMER_TMC2660 TCC1 // TMC2660 timer (see tmc2660.h)
-#define TIMER_PWM TCD1 // PWM timer (see pwm_spindle.c)
+#define TIMER_STEP TCC0 // Step timer (see stepper.h)
+#define TIMER_TMC2660 TCC1 // TMC2660 timer (see tmc2660.h)
+#define TIMER_PWM TCD1 // PWM timer (see pwm_spindle.c)
-#define M1_TIMER TCE1
-#define M2_TIMER TCF0
-#define M3_TIMER TCE0
-#define M4_TIMER TCD0
+#define M1_TIMER TCE1
+#define M2_TIMER TCF0
+#define M3_TIMER TCE0
+#define M4_TIMER TCD0
-#define M1_DMA_CH DMA.CH0
-#define M2_DMA_CH DMA.CH1
-#define M3_DMA_CH DMA.CH2
-#define M4_DMA_CH DMA.CH3
+#define M1_DMA_CH DMA.CH0
+#define M2_DMA_CH DMA.CH1
+#define M3_DMA_CH DMA.CH2
+#define M4_DMA_CH DMA.CH3
-#define M1_DMA_TRIGGER DMA_CH_TRIGSRC_TCE1_CCA_gc
-#define M2_DMA_TRIGGER DMA_CH_TRIGSRC_TCF0_CCA_gc
-#define M3_DMA_TRIGGER DMA_CH_TRIGSRC_TCE0_CCA_gc
-#define M4_DMA_TRIGGER DMA_CH_TRIGSRC_TCD0_CCA_gc
+#define M1_DMA_TRIGGER DMA_CH_TRIGSRC_TCE1_CCA_gc
+#define M2_DMA_TRIGGER DMA_CH_TRIGSRC_TCF0_CCA_gc
+#define M3_DMA_TRIGGER DMA_CH_TRIGSRC_TCE0_CCA_gc
+#define M4_DMA_TRIGGER DMA_CH_TRIGSRC_TCD0_CCA_gc
// Timer setup for stepper and dwells
#define TMC2660_TIMER TIMER_TMC2660
#define TMC2660_TIMER_ENABLE TC_CLKSEL_DIV64_gc
#define TMC2660_POLL_RATE 0.001 // sec. Must be in (0, 1]
-#define TMC2660_STABILIZE_TIME 0.01 // sec. Must be at least 1ms
+#define TMC2660_STABILIZE_TIME 0.01 // sec. Must be at least 1ms
// Huanyang settings
-#define HUANYANG_PORT USARTD1
-#define HUANYANG_DRE_vect USARTD1_DRE_vect
-#define HUANYANG_TXC_vect USARTD1_TXC_vect
-#define HUANYANG_RXC_vect USARTD1_RXC_vect
-#define HUANYANG_TIMEOUT 50 // ms. response timeout
-#define HUANYANG_RETRIES 4 // Number of retries before failure
-#define HUANYANG_ID 1 // Default ID
+#define HUANYANG_PORT USARTD1
+#define HUANYANG_DRE_vect USARTD1_DRE_vect
+#define HUANYANG_TXC_vect USARTD1_TXC_vect
+#define HUANYANG_RXC_vect USARTD1_RXC_vect
+#define HUANYANG_TIMEOUT 50 // ms. response timeout
+#define HUANYANG_RETRIES 4 // Number of retries before failure
+#define HUANYANG_ID 1 // Default ID
// Serial settings
-#define SERIAL_BAUD USART_BAUD_115200
-#define SERIAL_PORT USARTC0
-#define SERIAL_DRE_vect USARTC0_DRE_vect
-#define SERIAL_RXC_vect USARTC0_RXC_vect
+#define SERIAL_BAUD USART_BAUD_115200
+#define SERIAL_PORT USARTC0
+#define SERIAL_DRE_vect USARTC0_DRE_vect
+#define SERIAL_RXC_vect USARTC0_RXC_vect
// Input
-#define INPUT_BUFFER_LEN 255 // text buffer size (255 max)
+#define INPUT_BUFFER_LEN 255 // text buffer size (255 max)
// Arc
-#define ARC_RADIUS_ERROR_MAX 1.0 // max mm diff between start and end radius
-#define ARC_RADIUS_ERROR_MIN 0.005 // min mm where 1% rule applies
-#define ARC_RADIUS_TOLERANCE 0.001 // 0.1% radius variance test
+#define ARC_RADIUS_ERROR_MAX 1.0 // max mm diff between start and end radius
+#define ARC_RADIUS_ERROR_MIN 0.005 // min mm where 1% rule applies
+#define ARC_RADIUS_TOLERANCE 0.001 // 0.1% radius variance test
// Planner
#define PLANNER_BUFFER_POOL_SIZE 48
/// Buffers to reserve in planner before processing new input line
-#define PLANNER_BUFFER_HEADROOM 4
+#define PLANNER_BUFFER_HEADROOM 4
/// Minimum number of filled buffers before timeout until execution starts
-#define PLANNER_EXEC_MIN_FILL 4
+#define PLANNER_EXEC_MIN_FILL 4
/// Delay before executing new buffers unless PLANNER_EXEC_MIN_FILL is met
/// This gives the planner a chance to make a good plan before execution starts
-#define PLANNER_EXEC_DELAY 250 // In ms
+#define PLANNER_EXEC_DELAY 250 // In ms
// I2C
-#define I2C_DEV TWIC
-#define I2C_ISR TWIC_TWIS_vect
-#define I2C_ADDR 0x2b
-#define I2C_MAX_DATA 8
+#define I2C_DEV TWIC
+#define I2C_ISR TWIC_TWIS_vect
+#define I2C_ADDR 0x2b
+#define I2C_MAX_DATA 8
// Sanity check
int32_t diff = labs(labs(travel) - steps);
- if (16 < diff) ALARM(STAT_STEP_CHECK_FAILED);
+ if (20 < diff) ALARM(STAT_STEP_CHECK_FAILED);
}
}
static stat_t _exec_calibrate(mp_buffer_t *bf) {
const float time = MIN_SEGMENT_TIME; // In minutes
- const float max_delta_v = JOG_ACCELERATION * time;
+ const float max_delta_v = CAL_ACCELERATION * time;
do {
if (rtc_expired(cal.wait))
else {
float segment_length = ex.segment_velocity * ex.segment_time;
- for (int i = 0; i < AXES; i++)
- target[i] = mp_runtime_get_axis_position(i) + ex.unit[i] * segment_length;
+ for (int axis = 0; axis < AXES; axis++)
+ target[axis] =
+ mp_runtime_get_axis_position(axis) + ex.unit[axis] * segment_length;
}
mp_runtime_set_velocity(ex.segment_velocity);
/// F_2 = ( - 360 * s + 1800.0 )(Vt - Vi) * h^5
/// F_1 = ( 720.0 )(Vt - Vi) * h^5
///
-static float _init_forward_diffs(float Vi, float Vt, float s) {
+float mp_init_forward_diffs(float fdifs[5], float Vi, float Vt, float s) {
const float h = 1 / s;
const float s2 = square(s);
const float Vdxh5 = (Vt - Vi) * h * h * h * h * h;
- ex.forward_diff[4] = (32.5 * s2 - 75.0 * s + 45.375) * Vdxh5;
- ex.forward_diff[3] = (90.0 * s2 - 435.0 * s + 495.0 ) * Vdxh5;
- ex.forward_diff[2] = (60.0 * s2 - 720.0 * s + 1530.0 ) * Vdxh5;
- ex.forward_diff[1] = ( - 360.0 * s + 1800.0 ) * Vdxh5;
- ex.forward_diff[0] = ( 720.0 ) * Vdxh5;
+ fdifs[4] = (32.5 * s2 - 75.0 * s + 45.375) * Vdxh5;
+ fdifs[3] = (90.0 * s2 - 435.0 * s + 495.0 ) * Vdxh5;
+ fdifs[2] = (60.0 * s2 - 720.0 * s + 1530.0 ) * Vdxh5;
+ fdifs[1] = ( - 360.0 * s + 1800.0 ) * Vdxh5;
+ fdifs[0] = ( 720.0 ) * Vdxh5;
// Calculate the initial velocity by calculating:
//
}
+float mp_next_forward_diff(float fdifs[5]) {
+ float delta = fdifs[4];
+
+ fdifs[4] += fdifs[3];
+ fdifs[3] += fdifs[2];
+ fdifs[2] += fdifs[1];
+ fdifs[1] += fdifs[0];
+
+ return delta;
+}
+
+
/// Common code for head and tail sections
static stat_t _exec_aline_section(float length, float vin, float vout) {
if (ex.section_new) {
ex.segment_count = (uint32_t)ex.segments;
if (vin == vout) ex.segment_velocity = vin;
- else ex.segment_velocity = _init_forward_diffs(vin, vout, ex.segments);
+ else ex.segment_velocity =
+ mp_init_forward_diffs(ex.forward_diff, vin, vout, ex.segments);
if (ex.segment_time < MIN_SEGMENT_TIME)
return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position
ex.section_new = false;
} else {
- if (vin != vout) ex.segment_velocity += ex.forward_diff[4];
+ if (vin != vout)
+ ex.segment_velocity += mp_next_forward_diff(ex.forward_diff);
// Subsequent segments
if (_exec_aline_segment() == STAT_OK) return STAT_OK;
-
- if (vin != vout) {
- ex.forward_diff[4] += ex.forward_diff[3];
- ex.forward_diff[3] += ex.forward_diff[2];
- ex.forward_diff[2] += ex.forward_diff[1];
- ex.forward_diff[1] += ex.forward_diff[0];
- }
}
return STAT_EAGAIN;
}
return ex.segment_velocity +
- (ex.section == SECTION_BODY ? 0 : ex.forward_diff[4]);
+ (ex.section == SECTION_BODY ? 0 : mp_next_forward_diff(ex.forward_diff));
}
// 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, bf is OK to use here
- float braking_length = mp_get_target_length(braking_velocity, 0, bf);
+ float braking_length =
+ mp_get_target_length(braking_velocity, 0, bf->recip_jerk);
// Hack to prevent Case 2 moves for perfect-fit decels. Happens when homing.
if (available_length < braking_length && fp_ZERO(bf->exit_velocity))
// Convert return status for stepper.c
switch (status) {
- case STAT_NOOP: return STAT_EAGAIN; // Tell caller to call again
+ case STAT_NOOP:
+ // Tell caller to call again if there is more in the queue
+ return mp_queue_is_empty() ? STAT_NOOP : STAT_EAGAIN;
case STAT_EAGAIN: return STAT_OK; // A move was queued, call again later
default: return status;
}
stat_t mp_exec_move();
stat_t mp_exec_aline(mp_buffer_t *bf);
+float mp_init_forward_diffs(float fdifs[5], float Vi, float Vt, float s);
+float mp_next_forward_diff(float fdifs[5]);
#include "axes.h"
#include "planner.h"
#include "buffer.h"
+#include "line.h"
+#include "exec.h"
#include "runtime.h"
#include "machine.h"
#include "state.h"
typedef struct {
bool writing;
+ float Vi;
+ float Vt;
+ float fdifs[AXES][5];
+ unsigned segments[AXES];
+
+ int sign[AXES];
+ float velocity[AXES];
float next_velocity[AXES];
float target_velocity[AXES];
- float current_velocity[AXES];
} jog_runtime_t;
static stat_t _exec_jog(mp_buffer_t *bf) {
// Load next velocity
+ bool changed = false;
if (!jr.writing)
- for (int axis = 0; axis < AXES; axis++)
- jr.target_velocity[axis] = jr.next_velocity[axis];
+ for (int axis = 0; axis < AXES; axis++) {
+ float Vn = jr.next_velocity[axis] * axes[axis].velocity_max;
+ float Vi = jr.velocity[axis];
+ float Vt = jr.target_velocity[axis];
+
+ if (!fp_ZERO(Vi) && (Vn < 0) != (Vi < 0))
+ Vn = 0; // Go zero on sign change
+
+ if (fabs(Vn) < JOG_MIN_VELOCITY) Vn = 0;
+
+ if (Vt != Vn) {
+ jr.target_velocity[axis] = Vn;
+ if (Vn) jr.sign[axis] = Vn < 0 ? -1 : 1;
+ changed = true;
+ }
+ }
- // Compute next line segment
- float target[AXES];
- const float time = MIN_SEGMENT_TIME; // In minutes
- const float max_delta_v = JOG_ACCELERATION * time;
- float velocity_sqr = 0;
bool done = true;
+ float velocity_sqr = 0;
- // Compute new axis velocities and target
for (int axis = 0; axis < AXES; axis++) {
- float target_v = jr.target_velocity[axis] * axes[axis].velocity_max;
- float delta_v = target_v - jr.current_velocity[axis];
- float sign = delta_v < 0 ? -1 : 1;
+ float Vi = fabs(jr.velocity[axis]);
+ float Vt = fabs(jr.target_velocity[axis]);
- if (max_delta_v < fabs(delta_v))
- jr.current_velocity[axis] += max_delta_v * sign;
- else jr.current_velocity[axis] = target_v;
+ if (changed) {
+ if (fp_EQ(Vi, Vt)) {
+ Vi = Vt;
+ jr.segments[axis] = 0;
- if (!fp_ZERO(jr.current_velocity[axis])) done = false;
+ } else {
+ // Compute axis max jerk
+ float jerk = axes[axis].jerk_max * JERK_MULTIPLIER;
- // Compute target from axis velocity
- target[axis] =
- mp_runtime_get_axis_position(axis) + time * jr.current_velocity[axis];
+ // Compute length to velocity given max jerk
+ float length = mp_get_target_length(Vi, Vt, 1 / (jerk * JOG_JERK_MULT));
- // Accumulate velocities squared
- velocity_sqr += square(jr.current_velocity[axis]);
+ // Compute move time
+ float move_time = 2 * length / (Vi + Vt);
+ if (move_time < MIN_SEGMENT_TIME) {
+ Vi = Vt;
+ jr.segments[axis] = 0;
+
+ } else {
+ jr.segments[axis] = ceil(move_time / NOM_SEGMENT_TIME);
+
+ Vi = mp_init_forward_diffs(jr.fdifs[axis], Vi, Vt, jr.segments[axis]);
+ jr.segments[axis]--;
+ }
+ }
+
+ } else if (jr.segments[axis]) {
+ Vi += mp_next_forward_diff(jr.fdifs[axis]);
+ jr.segments[axis]--;
+
+ } else Vi = Vt;
+
+ if (Vi < JOG_MIN_VELOCITY && Vt < JOG_MIN_VELOCITY) done = false;
+
+ velocity_sqr += square(Vi);
+ jr.velocity[axis] = Vi * jr.sign[axis];
}
// Check if we are done
return STAT_NOOP; // Done, no move executed
}
+ // Compute target from velocity
+ float target[AXES];
+ for (int axis = 0; axis < AXES; axis++)
+ target[axis] = mp_runtime_get_axis_position(axis) +
+ jr.velocity[axis] * NOM_SEGMENT_TIME;
+
+ // Set velocity and target
mp_runtime_set_velocity(sqrt(velocity_sqr));
- stat_t status = mp_runtime_move_to_target(target, time);
+ stat_t status = mp_runtime_move_to_target(target, NOM_SEGMENT_TIME);
if (status != STAT_OK) return status;
return STAT_EAGAIN;
}
-static float _calc_jerk(const float axis_square[], const float unit[]) {
- /* Determine jerk value to use for the block. First find the axis for which
- * the jerk cannot be exceeded - the 'jerk_axis'. This is the axis for which
- * the time to decelerate from the target velocity to zero would be the
- * longest.
- *
- * We can determine the "longest" deceleration in terms of time or distance.
- *
- * The math for time-to-decelerate T from speed S to speed E with constant
- * jerk J is:
- *
- * T = 2 * sqrt((S - E) / J)
- *
- * Since E is always zero in this calculation, we can simplify:
- *
- * T = 2 * sqrt(S / J)
- *
- * The math for distance-to-decelerate l from speed S to speed E with
- * constant jerk J is:
- *
- * l = (S + E) * sqrt((S - E) / J)
- *
- * Since E is always zero in this calculation, we can simplify:
- *
- * l = S * sqrt(S / J)
- *
- * The final value we want to know is which one is *longest*, compared to the
- * others, so we don't need the actual value. This means that the scale of
- * the value doesn't matter, so for T we can remove the "2 *" and for L we can
- * remove the "S *". This leaves both to be simply "sqrt(S / J)". Since we
- * don't need the scale, it doesn't matter what speed we're coming from, so S
- * can be replaced with 1.
- *
- * However, we *do* need to compensate for the fact that each axis contributes
- * differently to the move, so we will scale each contribution C[n] by the
- * proportion of the axis movement length D[n]. Using that, we construct the
- * following, with these definitions:
- *
- * J[n] = max_jerk for axis n
- * D[n] = distance traveled for this move for axis n
- * C[n] = "axis contribution" of axis n
- *
- * For each axis n:
- *
- * C[n] = sqrt(1 / J[n]) * D[n]
- *
- * Keeping in mind that we only need a rank compared to the other axes, we can
- * further optimize the calculations:
- *
- * Square the expression to remove the square root:
- *
- * C[n]^2 = 1 / J[n] * D[n]^2
- *
- * We don't care that C is squared, so we'll use it that way. Also note that
- * we already have 1 / J[n] calculated for each axis.
- */
-
+/* Find the axis for which the jerk cannot be exceeded - the 'jerk_axis'.
+ * This is the axis for which the time to decelerate from the target velocity
+ * to zero would be the longest.
+ *
+ * We can determine the "longest" deceleration in terms of time or distance.
+ *
+ * The math for time-to-decelerate T from speed S to speed E with constant
+ * jerk J is:
+ *
+ * T = 2 * sqrt((S - E) / J)
+ *
+ * Since E is always zero in this calculation, we can simplify:
+ *
+ * T = 2 * sqrt(S / J)
+ *
+ * The math for distance-to-decelerate l from speed S to speed E with
+ * constant jerk J is:
+ *
+ * l = (S + E) * sqrt((S - E) / J)
+ *
+ * Since E is always zero in this calculation, we can simplify:
+ *
+ * l = S * sqrt(S / J)
+ *
+ * The final value we want to know is which one is *longest*, compared to the
+ * others, so we don't need the actual value. This means that the scale of
+ * the value doesn't matter, so for T we can remove the "2 *" and for L we can
+ * remove the "S *". This leaves both to be simply "sqrt(S / J)". Since we
+ * don't need the scale, it doesn't matter what speed we're coming from, so S
+ * can be replaced with 1.
+ *
+ * However, we *do* need to compensate for the fact that each axis contributes
+ * differently to the move, so we will scale each contribution C[n] by the
+ * proportion of the axis movement length D[n]. Using that, we construct the
+ * following, with these definitions:
+ *
+ * J[n] = max_jerk for axis n
+ * D[n] = distance traveled for this move for axis n
+ * C[n] = "axis contribution" of axis n
+ *
+ * For each axis n:
+ *
+ * C[n] = sqrt(1 / J[n]) * D[n]
+ *
+ * Keeping in mind that we only need a rank compared to the other axes, we can
+ * further optimize the calculations:
+ *
+ * Square the expression to remove the square root:
+ *
+ * C[n]^2 = 1 / J[n] * D[n]^2
+ *
+ * We don't care that C is squared, so we'll use it that way. Also note that
+ * we already have 1 / J[n] calculated for each axis.
+ */
+int mp_find_jerk_axis(const float axis_square[]) {
float C;
float maxC = 0;
int jerk_axis = 0;
}
}
+ return jerk_axis;
+}
+
+
+/// Determine jerk value to use for the block.
+static float _calc_jerk(const float axis_square[], const float unit[]) {
+ int jerk_axis = mp_find_jerk_axis(axis_square);
+
// Finally, the selected jerk term needs to be scaled by the
// reciprocal of the absolute value of the jerk_axis's unit
// vector term. This way when the move is finally decomposed into
stat_t mp_aline(const float target[], int32_t line);
+int mp_find_jerk_axis(const float axis_square[]);
// MIN_BODY_LENGTH
bf->body_length = 0;
float minimum_length =
- mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf);
+ mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf->recip_jerk);
// head-only & tail-only cases
if (bf->length <= (minimum_length + MIN_BODY_LENGTH)) {
// Set head and tail lengths for evaluating the next cases
bf->head_length =
- mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf);
+ mp_get_target_length(bf->entry_velocity, bf->cruise_velocity,
+ bf->recip_jerk);
bf->tail_length =
- mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf);
+ mp_get_target_length(bf->exit_velocity, bf->cruise_velocity,
+ bf->recip_jerk);
if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;}
if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;}
// initialize from previous iteration
bf->cruise_velocity = computed_velocity;
bf->head_length =
- mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf);
+ mp_get_target_length(bf->entry_velocity, bf->cruise_velocity,
+ bf->recip_jerk);
bf->tail_length =
- mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf);
+ mp_get_target_length(bf->exit_velocity, bf->cruise_velocity,
+ bf->recip_jerk);
if (bf->head_length > bf->tail_length) {
bf->head_length =
// set velocity and clean up any parts that are too short
bf->cruise_velocity = computed_velocity;
bf->head_length =
- mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf);
+ mp_get_target_length(bf->entry_velocity, bf->cruise_velocity,
+ bf->recip_jerk);
bf->tail_length = bf->length - bf->head_length;
if (bf->head_length < MIN_HEAD_LENGTH) {
* [2] Cannot assume Vf >= Vi due to rounding errors and use of
* PLANNER_VELOCITY_TOLERANCE necessitating the introduction of fabs()
*/
-float mp_get_target_length(float Vi, float Vf, const mp_buffer_t *bf) {
- return fabs(Vi - Vf) * sqrt(fabs(Vi - Vf) * bf->recip_jerk);
+float mp_get_target_length(float Vi, float Vf, float recip_jerk) {
+ return fabs(Vi - Vf) * sqrt(fabs(Vi - Vf) * recip_jerk);
}
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_length(float Vi, float Vf, float recip_jerk);
float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf);
projects / bbctrl-firmware / commitdiff