From 1d6cdd58d7ff184a8b91a0ca6c68579dda75bc80 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 9 Apr 2017 21:38:10 -0700 Subject: [PATCH] Roll back to variable step clock period to fix velocity discontinuity. --- avr/src/config.h | 2 +- avr/src/motor.c | 12 +++++------- avr/src/motor.h | 2 +- avr/src/plan/calibrate.c | 2 +- avr/src/plan/exec.c | 12 +++++++----- avr/src/plan/jog.c | 2 +- avr/src/plan/planner.c | 28 +++++++++++++--------------- avr/src/plan/runtime.c | 8 ++++---- avr/src/plan/runtime.h | 6 +++--- avr/src/stepper.c | 16 ++++++++++++---- avr/src/stepper.h | 2 +- avr/test/Makefile | 11 ++++++++++- avr/test/hal.c | 26 +++++++------------------- avr/test/planner-test.c | 2 +- avr/test/test.gc | 11 +++++++++++ 15 files changed, 78 insertions(+), 64 deletions(-) create mode 100644 avr/test/test.gc diff --git a/avr/src/config.h b/avr/src/config.h index eccc5d7..5dc6094 100644 --- a/avr/src/config.h +++ b/avr/src/config.h @@ -152,7 +152,7 @@ enum { #define STEP_TIMER_ENABLE TC_CLKSEL_DIV4_gc #define STEP_TIMER_DIV 4 #define STEP_TIMER_FREQ (F_CPU / STEP_TIMER_DIV) -#define STEP_TIMER_POLL (STEP_TIMER_FREQ * 0.001) +#define STEP_TIMER_POLL ((uint16_t)(STEP_TIMER_FREQ * 0.001)) #define STEP_TIMER_WGMODE TC_WGMODE_NORMAL_gc // count to TOP & rollover #define STEP_TIMER_ISR TCC0_OVF_vect #define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc diff --git a/avr/src/motor.c b/avr/src/motor.c index 9916eaf..818be43 100644 --- a/avr/src/motor.c +++ b/avr/src/motor.c @@ -403,7 +403,7 @@ void motor_load_move(int motor) { } -void motor_prep_move(int motor, int32_t target) { +void motor_prep_move(int motor, float time, int32_t target) { motor_t *m = &motors[motor]; // Validate input @@ -437,8 +437,8 @@ void motor_prep_move(int motor, int32_t target) { // Find the fastest clock rate that will fit the required number of steps. // Note, clock toggles step line so we need two clocks per step. - uint24_t ticks_per_step = SEGMENT_CLOCKS / half_steps; - if (SEGMENT_CLOCKS % half_steps) ticks_per_step++; // Round up + uint24_t seg_clocks = time * F_CPU * 60; + uint24_t ticks_per_step = seg_clocks / half_steps + 1; // Round up if (ticks_per_step < 0xffff) m->timer_clock = TC_CLKSEL_DIV1_gc; else if (ticks_per_step < 0x1ffff) m->timer_clock = TC_CLKSEL_DIV2_gc; else if (ticks_per_step < 0x3ffff) m->timer_clock = TC_CLKSEL_DIV4_gc; @@ -447,15 +447,13 @@ void motor_prep_move(int motor, int32_t target) { // Note, we rely on the fact that TC_CLKSEL_DIV1_gc through TC_CLKSEL_DIV8_gc // equal 1, 2, 3 & 4 respectively. - m->timer_period = ticks_per_step >> (m->timer_clock - 1); - if (ticks_per_step & ((1 << (m->timer_clock - 1)) - 1)) - m->timer_period++; // Round up + m->timer_period = (ticks_per_step >> (m->timer_clock - 1)) + 1; // Round up if (!m->timer_period || !half_steps) m->timer_clock = 0; // Compute power from axis max velocity const float max_step_vel = m->steps_per_unit * axis_get_velocity_max(m->axis); - const float power = half_steps / (max_step_vel * SEGMENT_TIME * 2); + const float power = half_steps / (max_step_vel * time * 2); drv8711_set_power(motor, power); // Power motor diff --git a/avr/src/motor.h b/avr/src/motor.h index 54cb7c4..5f33c91 100644 --- a/avr/src/motor.h +++ b/avr/src/motor.h @@ -84,4 +84,4 @@ void motor_error_callback(int motor, motor_flags_t errors); void motor_end_move(int motor); void motor_load_move(int motor); -void motor_prep_move(int motor, int32_t target); +void motor_prep_move(int motor, float time, int32_t target); diff --git a/avr/src/plan/calibrate.c b/avr/src/plan/calibrate.c index e075d90..1f7c6e8 100644 --- a/avr/src/plan/calibrate.c +++ b/avr/src/plan/calibrate.c @@ -137,7 +137,7 @@ static stat_t _exec_calibrate(mp_buffer_t *bf) { mp_kinematics(travel, steps); // Queue segment - st_prep_line(steps); + st_prep_line(time, steps); return STAT_EAGAIN; } diff --git a/avr/src/plan/exec.c b/avr/src/plan/exec.c index 136dc96..29d13fc 100644 --- a/avr/src/plan/exec.c +++ b/avr/src/plan/exec.c @@ -60,6 +60,7 @@ typedef struct { uint24_t segment_count; // count of running segments uint24_t segment; // current segment + float segment_time; float segment_velocity; // computed velocity for segment float segment_start[AXES]; float segment_delta; @@ -85,9 +86,10 @@ static stat_t _exec_aline_section(float length, float Vi, float Vt) { ASSERT(Vi || Vt); // len / avg. velocity - float move_time = 2 * length / (Vi + Vt); // in mins - float segments = ceil(move_time / SEGMENT_TIME); - ex.segment_count = round(segments); + const float move_time = 2 * length / (Vi + Vt); // in mins + const float segments = round(move_time / SEGMENT_TIME); + ex.segment_count = segments; + ex.segment_time = move_time / segments; // in mins ex.segment = 0; ex.segment_dist = 0; @@ -119,7 +121,7 @@ static stat_t _exec_aline_section(float length, float Vi, float Vt) { // Compute quintic Bezier curve ex.segment_velocity = velocity_curve(Vi, Vt, ex.segment * ex.segment_delta); - ex.segment_dist += ex.segment_velocity * SEGMENT_TIME; + ex.segment_dist += ex.segment_velocity * ex.segment_time; } // Avoid overshoot @@ -130,7 +132,7 @@ static stat_t _exec_aline_section(float length, float Vi, float Vt) { } mp_runtime_set_velocity(ex.segment_velocity); - RITORNO(mp_runtime_move_to_target(target)); + RITORNO(mp_runtime_move_to_target(ex.segment_time, target)); // Return EAGAIN to continue or OK if this segment is done return ex.segment < ex.segment_count ? STAT_EAGAIN : STAT_OK; diff --git a/avr/src/plan/jog.c b/avr/src/plan/jog.c index 930126a..af5a8af 100644 --- a/avr/src/plan/jog.c +++ b/avr/src/plan/jog.c @@ -152,7 +152,7 @@ static stat_t _exec_jog(mp_buffer_t *bf) { // Set velocity and target mp_runtime_set_velocity(sqrt(velocity_sqr)); - stat_t status = mp_runtime_move_to_target(target); + stat_t status = mp_runtime_move_to_target(SEGMENT_TIME, target); if (status != STAT_OK) return status; return STAT_EAGAIN; diff --git a/avr/src/plan/planner.c b/avr/src/plan/planner.c index 4d9b1a6..130959e 100644 --- a/avr/src/plan/planner.c +++ b/avr/src/plan/planner.c @@ -172,16 +172,15 @@ void mp_kinematics(const float travel[], float steps[]) { * Classes of moves: * * Requested-Fit - The move has sufficient length to achieve the target - * velocity (cruise velocity). I.e it will accommodate the acceleration / - * deceleration profile in the given length. + * velocity. I.e it will accommodate the acceleration / deceleration + * profile in the given length. * * Rate-Limited-Fit - The move does not have sufficient length to achieve - * target velocity. In this case the cruise velocity will be set lower than - * the requested velocity (incoming bf->cruise_velocity). The entry and - * exit velocities are satisfied. + * target velocity. In this case, the cruise velocity will be lowered. + * The entry and exit velocities are satisfied. * * Degraded-Fit - The move does not have sufficient length to transition from - * the entry velocity to the exit velocity in the available length. These + * the entry velocity to the exit velocity in the available length. These * velocities are not negotiable, so a degraded solution is found. * * In worst cases, the move cannot be executed as the required execution @@ -209,21 +208,21 @@ void mp_kinematics(const float travel[], float steps[]) { * * Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot: * - * HT (Ve=Vx)Vx Ve is degraded (velocity step). Vx is met * B" line is very short but drawable; is treated as a - * body only + * body only. * F force fit: This block is slowed down until it can - * be executed + * be executed. * * Note: The order of the cases/tests in the code is important. Start with * the shortest cases first and work up. Not only does this simplify the order @@ -234,10 +233,9 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { 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 + // Force block into a single segment body with limited velocities. // Accept the entry velocity, limit the cruise, and go for the best exit // velocity you can get given the delta_vmax (maximum velocity slew). - float naive_move_time = 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average diff --git a/avr/src/plan/runtime.c b/avr/src/plan/runtime.c index d9d3e60..b191af0 100644 --- a/avr/src/plan/runtime.c +++ b/avr/src/plan/runtime.c @@ -123,7 +123,7 @@ void mp_runtime_set_steps_from_position() { /// Set runtime position for a single axis -void mp_runtime_set_axis_position(uint8_t axis, const float position) { +void mp_runtime_set_axis_position(uint8_t axis, float position) { rt.position[axis] = position; report_request(); } @@ -134,7 +134,7 @@ 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[]) { +void mp_runtime_set_position(const float position[]) { copy_vector(rt.position, position); report_request(); } @@ -153,7 +153,7 @@ void mp_runtime_set_work_offsets(float offset[]) { /// Segment runner -stat_t mp_runtime_move_to_target(float target[]) { +stat_t mp_runtime_move_to_target(float time, const float target[]) { ASSERT(isfinite(target[0]) && isfinite(target[1]) && isfinite(target[2]) && isfinite(target[3])); @@ -162,7 +162,7 @@ stat_t mp_runtime_move_to_target(float target[]) { mp_kinematics(target, steps); // Call the stepper prep function - RITORNO(st_prep_line(steps)); + RITORNO(st_prep_line(time, steps)); // Update positions mp_runtime_set_position(target); diff --git a/avr/src/plan/runtime.h b/avr/src/plan/runtime.h index ca835a9..a5c0fc2 100644 --- a/avr/src/plan/runtime.h +++ b/avr/src/plan/runtime.h @@ -46,13 +46,13 @@ void mp_runtime_set_velocity(float velocity); void mp_runtime_set_steps_from_position(); -void mp_runtime_set_axis_position(uint8_t axis, const float position); +void mp_runtime_set_axis_position(uint8_t axis, float position); float mp_runtime_get_axis_position(uint8_t axis); float *mp_runtime_get_position(); -void mp_runtime_set_position(float position[]); +void mp_runtime_set_position(const 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[]); +stat_t mp_runtime_move_to_target(float time, const float target[]); diff --git a/avr/src/stepper.c b/avr/src/stepper.c index d176d0b..2c226a4 100644 --- a/avr/src/stepper.c +++ b/avr/src/stepper.c @@ -61,6 +61,7 @@ typedef struct { bool move_queued; // prepped move queued move_type_t move_type; float prep_dwell; + uint16_t clock_period; } stepper_t; @@ -71,8 +72,10 @@ void stepper_init() { // Setup step timer TIMER_STEP.CTRLB = STEP_TIMER_WGMODE; // waveform mode TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // interrupt mode - TIMER_STEP.PER = SEGMENT_PERIOD; // timer rate + TIMER_STEP.PER = STEP_TIMER_POLL; // timer rate TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // start step timer + + st.clock_period = STEP_TIMER_POLL; } @@ -141,9 +144,12 @@ static void _load_move() { return; } + // New clock period + TIMER_STEP.PER = st.clock_period; + // Dwell if (0 < st.dwell) { - st.dwell -= SEGMENT_SEC; + st.dwell -= 0.001; // 1ms return; } else st.dwell = 0; @@ -172,6 +178,7 @@ static void _load_move() { st.move_type = MOVE_TYPE_NULL; st.prep_dwell = 0; // clear dwell st.move_ready = false; // flip the flag back + st.clock_period = STEP_TIMER_POLL; // Request next move if not currently in a dwell. Requesting the next move // may power up motors and the motors should not be powered up during a dwell. @@ -197,17 +204,18 @@ ISR(STEP_TIMER_ISR) { * Steps are fractional. Their sign indicates direction. Motors not in the * move have 0 steps. */ -stat_t st_prep_line(const float target[]) { +stat_t st_prep_line(float time, const float target[]) { // Trap conditions that would prevent queueing the line ASSERT(!st.move_ready); // Setup segment parameters st.move_type = MOVE_TYPE_LINE; + st.clock_period = round(time * STEP_TIMER_FREQ * 60); // Prepare motor moves for (int motor = 0; motor < MOTORS; motor++) { ASSERT(isfinite(target[motor])); - motor_prep_move(motor, round(target[motor])); + motor_prep_move(motor, time, round(target[motor])); } st.move_queued = true; // signal prep buffer ready (do this last) diff --git a/avr/src/stepper.h b/avr/src/stepper.h index 0f0a0ff..ab5023c 100644 --- a/avr/src/stepper.h +++ b/avr/src/stepper.h @@ -38,5 +38,5 @@ void stepper_init(); void st_shutdown(); bool st_is_busy(); -stat_t st_prep_line(const float target[]); +stat_t st_prep_line(float time, const float target[]); void st_prep_dwell(float seconds); diff --git a/avr/test/Makefile b/avr/test/Makefile index cc89e18..6327058 100644 --- a/avr/test/Makefile +++ b/avr/test/Makefile @@ -15,6 +15,15 @@ all: $(TESTS) planner-test: $(PLANNER_TEST_SRC) gcc -o $@ $(PLANNER_TEST_SRC) $(CFLAGS) $(LDFLAGS) +test: planner-test + ./$< < test.gc + +csv: planner-test + @$(MAKE) -s test | grep -E '^-?[0-9.]+,' + +plot: planner-test + @$(MAKE) -s csv | ./plot_velocity.py + # Clean tidy: rm -f $(shell find -name \*~ -o -name \#\*) @@ -22,7 +31,7 @@ tidy: clean: tidy rm -rf $(TESTS) -.PHONY: tidy clean all +.PHONY: tidy clean all test csv plot # Dependencies -include $(shell mkdir -p .dep) $(wildcard .dep/*) diff --git a/avr/test/hal.c b/avr/test/hal.c index 0b818fe..4f7d90d 100644 --- a/avr/test/hal.c +++ b/avr/test/hal.c @@ -140,8 +140,8 @@ void spindle_set_mode(spindle_mode_t mode) { } -void motor_set_encoder(int motor, float encoder) { - DEBUG_CALL("%d, %f", motor, encoder); +void motor_set_position(int motor, int32_t position) { + DEBUG_CALL("%d, %d", motor, position); } @@ -207,27 +207,15 @@ bool st_is_busy() {return false;} float square(float x) {return x * x;} -stat_t st_prep_line(float time, const float target[], const int32_t error[]) { - DEBUG_CALL("%f, (%f, %f, %f, %f), (%d, %d, %d, %d)", - time, target[0], target[1], target[2], target[3], - error[0], error[1], error[2], error[3]); - - double dist = 0; - - for (int i = 0; i < 4; i++) - dist += - square((target[i] - motor_position[i]) / motor_get_steps_per_unit(i)); - - dist = sqrt(dist); - - double velocity = dist / time; +stat_t st_prep_line(float time, const float target[]) { + DEBUG_CALL("%f, (%f, %f, %f, %f)", + time, target[0], target[1], target[2], target[3]); for (int i = 0; i < MOTORS; i++) motor_position[i] = target[i]; - printf("%0.10f, %0.10f, %0.10f, %0.10f, %0.10f, %0.10f\n", - velocity, dist, time, - motor_position[0], motor_position[1], motor_position[2]); + printf("%0.10f, %0.10f, %0.10f, %0.10f\n", + time, motor_position[0], motor_position[1], motor_position[2]); return STAT_OK; } diff --git a/avr/test/planner-test.c b/avr/test/planner-test.c index 7af39ac..7e9038b 100644 --- a/avr/test/planner-test.c +++ b/avr/test/planner-test.c @@ -71,5 +71,5 @@ int main(int argc, char *argv[]) { printf("STATE: %s\n", mp_get_state_pgmstr(mp_get_state())); - return status; + return 0; } diff --git a/avr/test/test.gc b/avr/test/test.gc new file mode 100644 index 0000000..7cf8315 --- /dev/null +++ b/avr/test/test.gc @@ -0,0 +1,11 @@ +$resume + +$0vm=10000 +$1vm=10000 +$0jm=50 +$1jm=50 + +G0 X500Y500 +G0 X0Y0 +G0 X-500Y0 +G0 X-500Y-500 -- 2.27.0