From 9cfbde76d52c3d099dcbd355e3533bcb7d6274b2 Mon Sep 17 00:00:00 2001 From: Paciente8159 Date: Fri, 29 Mar 2024 11:55:16 +0000 Subject: [PATCH] prevent floating point round errors in speed - prevent floating point round errors in speed calculations when time slices a too small --- makefiles/virtual/mcu_virtual.cpp | 56 +++++++++++++++++-------------- uCNC/src/core/interpolator.c | 16 +++++---- uCNC/src/core/planner.c | 2 +- 3 files changed, 40 insertions(+), 34 deletions(-) diff --git a/makefiles/virtual/mcu_virtual.cpp b/makefiles/virtual/mcu_virtual.cpp index 44f086235..08da8b443 100644 --- a/makefiles/virtual/mcu_virtual.cpp +++ b/makefiles/virtual/mcu_virtual.cpp @@ -342,16 +342,16 @@ extern "C" BOOL fConnected = FALSE; hPipe = CreateNamedPipe( - lpszPipename, // pipe name - PIPE_ACCESS_DUPLEX, // read/write access - PIPE_TYPE_MESSAGE | // message type pipe - PIPE_READMODE_MESSAGE | // message-read mode - PIPE_WAIT, // blocking mode - PIPE_UNLIMITED_INSTANCES, // max. instances - sizeof(VIRTUAL_MAP), // output buffer size - sizeof(VIRTUAL_MAP), // input buffer size - 0, // client time-out - NULL); // no template file + lpszPipename, // pipe name + PIPE_ACCESS_DUPLEX, // read/write access + PIPE_TYPE_MESSAGE | // message type pipe + PIPE_READMODE_MESSAGE | // message-read mode + PIPE_WAIT, // blocking mode + PIPE_UNLIMITED_INSTANCES, // max. instances + sizeof(VIRTUAL_MAP), // output buffer size + sizeof(VIRTUAL_MAP), // input buffer size + 0, // client time-out + NULL); // no template file if (hPipe == INVALID_HANDLE_VALUE) { @@ -376,11 +376,11 @@ extern "C" memcpy(lpvMessage, (void *)&virtualmap, sizeof(VIRTUAL_MAP)); fSuccess = WriteFile( - hPipe, // pipe handle - lpvMessage, // message - cbToWrite, // message length - &cbWritten, // bytes written - NULL); // not overlapped + hPipe, // pipe handle + lpvMessage, // message + cbToWrite, // message length + &cbWritten, // bytes written + NULL); // not overlapped if (!fSuccess) { @@ -391,11 +391,11 @@ extern "C" // Read from the pipe. fSuccess = ReadFile( - hPipe, // pipe handle - lpvMessage, // buffer to receive reply - cbToWrite, // size of buffer - &cbRead, // number of bytes read - NULL); // not overlapped + hPipe, // pipe handle + lpvMessage, // buffer to receive reply + cbToWrite, // size of buffer + &cbRead, // number of bytes read + NULL); // not overlapped if (!fSuccess && GetLastError() != ERROR_MORE_DATA) break; @@ -623,7 +623,7 @@ extern "C" * * **/ - #ifndef ITP_SAMPLE_RATE +#ifndef ITP_SAMPLE_RATE #define ITP_SAMPLE_RATE (F_STEP_MAX * 2) #endif @@ -873,15 +873,15 @@ extern "C" void ticksimul(void) { -// long t = stopCycleCounter(); -// printf("Elapsed %dus\n\r", (int)((double)t / cyclesPerMicrosecond)); + // long t = stopCycleCounter(); + // printf("Elapsed %dus\n\r", (int)((double)t / cyclesPerMicrosecond)); for (int i = 0; i < (int)ceil(20 * ITP_SAMPLE_RATE / 1000); i++) { mcu_gen_step(); } mcu_rtc_cb(mcu_millis()); -// startCycleCounter(); + // startCycleCounter(); } /** @@ -901,6 +901,10 @@ extern "C" mcu_enable_global_isr(); } + void mcu_io_reset(void) + { + } + int main(int argc, char **argv) { cnc_init(); @@ -910,8 +914,8 @@ extern "C" } return 0; } - - uint8_t itp_set_step_mode(uint8_t mode) {return 0;} + + uint8_t itp_set_step_mode(uint8_t mode) { return 0; } uint32_t mcu_free_micros(void) { diff --git a/uCNC/src/core/interpolator.c b/uCNC/src/core/interpolator.c index 42cf03e41..730c5e994 100644 --- a/uCNC/src/core/interpolator.c +++ b/uCNC/src/core/interpolator.c @@ -459,10 +459,11 @@ void itp_run(void) acc_init_speed = current_speed; #endif t *= accel_inv; - // slice up time in an integral number of periods (half with positive jerk and half with negative) - float slices_inv = fast_flt_inv(floorf(INTERPOLATOR_FREQ * t)); - if (slices_inv) + + if (t > INTERPOLATOR_DELTA_T) { + // slice up time in an integral number of periods (half with positive jerk and half with negative) + float slices_inv = fast_flt_inv(floorf(INTERPOLATOR_FREQ * t)); t_acc_integrator = t * slices_inv; #if S_CURVE_ACCELERATION_LEVEL != 0 acc_step = slices_inv; @@ -497,10 +498,11 @@ void itp_run(void) deac_step_acum = 0; #endif t *= accel_inv; - // slice up time in an integral number of periods (half with positive jerk and half with negative) - float slices_inv = fast_flt_inv(floorf(INTERPOLATOR_FREQ * t)); - if (slices_inv) + + if (t > INTERPOLATOR_DELTA_T) { + // slice up time in an integral number of periods (half with positive jerk and half with negative) + float slices_inv = fast_flt_inv(floorf(INTERPOLATOR_FREQ * t)); t_deac_integrator = t * slices_inv; if (t_deac_integrator < 0.00001f) { @@ -1127,7 +1129,7 @@ MCU_CALLBACK void mcu_step_cb(void) else { cnc_clear_exec_state(EXEC_RUN); // this naturally clears the RUN flag. Any other ISR stop does not clear the flag. - itp_stop(); // the buffer is empty. The ISR can stop + itp_stop(); // the buffer is empty. The ISR can stop return; } } diff --git a/uCNC/src/core/planner.c b/uCNC/src/core/planner.c index 31c27c264..28a8e1315 100644 --- a/uCNC/src/core/planner.c +++ b/uCNC/src/core/planner.c @@ -363,7 +363,7 @@ float planner_get_block_top_speed(float exit_speed_sqr) // calculates the difference between the entry speed and the exit speed uint8_t index = planner_data_read; float speed_delta = exit_speed_sqr - planner_data[index].entry_feed_sqr; - // caclculates the speed increase/decrease for the given distance + // calculates the speed increase/decrease for the given distance float junction_speed_sqr = planner_data[index].acceleration * (float)(planner_data[index].steps[planner_data[index].main_stepper]); junction_speed_sqr = fast_flt_mul2(junction_speed_sqr); // if there is enough space to accelerate computes the junction speed