From: Joseph Coffland Date: Mon, 19 Sep 2016 03:57:22 +0000 (-0700) Subject: S-curve jogging X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=e1de8e888ed3dc67211acb2022ea57bce37ed6df;p=bbctrl-firmware S-curve jogging --- diff --git a/src/config.h b/src/config.h index e81f842..1e9b3ea 100644 --- a/src/config.h +++ b/src/config.h @@ -94,10 +94,10 @@ enum { // 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 @@ -113,11 +113,11 @@ typedef enum { } 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 @@ -156,14 +156,16 @@ typedef enum { // 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 @@ -193,7 +195,7 @@ typedef enum { #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 @@ -262,8 +264,8 @@ typedef enum { // 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 @@ -291,24 +293,24 @@ typedef enum { */ // 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 @@ -353,34 +355,34 @@ typedef enum { #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 @@ -391,18 +393,18 @@ typedef enum { #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 diff --git a/src/motor.c b/src/motor.c index 6d9d2ed..a54ecfd 100644 --- a/src/motor.c +++ b/src/motor.c @@ -433,7 +433,7 @@ void motor_prep_move(int motor, int32_t seg_clocks, float target) { // 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); } } diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c index 03ab1b4..389560a 100644 --- a/src/plan/calibrate.c +++ b/src/plan/calibrate.c @@ -78,7 +78,7 @@ static calibrate_t cal = {0}; 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)) diff --git a/src/plan/exec.c b/src/plan/exec.c index 533b31a..4126eda 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -82,8 +82,9 @@ static stat_t _exec_aline_segment() { 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); @@ -218,16 +219,16 @@ static stat_t _exec_aline_segment() { /// 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: // @@ -242,6 +243,18 @@ static float _init_forward_diffs(float Vi, float Vt, float s) { } +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) { @@ -254,7 +267,8 @@ static stat_t _exec_aline_section(float length, float vin, float vout) { 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 @@ -268,17 +282,11 @@ static stat_t _exec_aline_section(float length, float vin, float vout) { 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; @@ -339,7 +347,7 @@ static float _compute_next_segment_velocity() { } 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)); } @@ -364,7 +372,8 @@ static void _plan_hold() { // 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)) @@ -583,7 +592,9 @@ stat_t mp_exec_move() { // 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; } diff --git a/src/plan/exec.h b/src/plan/exec.h index 00972b2..7db35f7 100644 --- a/src/plan/exec.h +++ b/src/plan/exec.h @@ -32,3 +32,5 @@ 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]); diff --git a/src/plan/jog.c b/src/plan/jog.c index d78f78a..ea07518 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -30,6 +30,8 @@ #include "axes.h" #include "planner.h" #include "buffer.h" +#include "line.h" +#include "exec.h" #include "runtime.h" #include "machine.h" #include "state.h" @@ -44,9 +46,15 @@ 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; @@ -55,35 +63,68 @@ static jog_runtime_t jr; 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 @@ -97,8 +138,15 @@ static stat_t _exec_jog(mp_buffer_t *bf) { 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; diff --git a/src/plan/line.c b/src/plan/line.c index a236e9f..c2ca3d0 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -143,63 +143,61 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { } -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; @@ -215,6 +213,14 @@ static float _calc_jerk(const float axis_square[], const float unit[]) { } } + 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 diff --git a/src/plan/line.h b/src/plan/line.h index 1661db0..44930ed 100644 --- a/src/plan/line.h +++ b/src/plan/line.h @@ -31,3 +31,4 @@ stat_t mp_aline(const float target[], int32_t line); +int mp_find_jerk_axis(const float axis_square[]); diff --git a/src/plan/planner.c b/src/plan/planner.c index 5552e23..0219b2b 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -306,7 +306,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { // 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)) { @@ -341,9 +341,11 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { // 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;} @@ -381,9 +383,11 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { // 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 = @@ -404,7 +408,8 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { // 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) { @@ -674,8 +679,8 @@ void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line) { * [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); } diff --git a/src/plan/planner.h b/src/plan/planner.h index f657829..ffed535 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -93,5 +93,5 @@ void mp_replan_all(); 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);