From 055ca528a8ac39d1c9f6ed0864c5c643eec423df Mon Sep 17 00:00:00 2001 From: Martin Date: Tue, 23 Oct 2018 18:17:20 +0200 Subject: [PATCH] Fixed speed planner slow down procedure. --- config.h | 25 +++++++++++++++---------- motor.c | 7 ++++--- queue.c | 16 ++++++++++++++++ queue.h | 1 + sensors_control.c | 15 ++++++++------- 5 files changed, 44 insertions(+), 20 deletions(-) diff --git a/config.h b/config.h index 6886cce..36a2e9d 100644 --- a/config.h +++ b/config.h @@ -88,7 +88,7 @@ Units: mm/s^2 Useful range: 1 to 10'000 */ -#define ACCELERATION 2000 +#define ACCELERATION 2500 /** \def ENDSTOP_CLEARANCE @@ -132,6 +132,7 @@ /** \def TRIGGERED_MOVEMENT Define this to start new G1 movement only by external interrupt from additional sensor switch. + Always used in case of embroidery machines. */ #define TRIGGERED_MOVEMENT @@ -140,6 +141,7 @@ Define this to enable look-ahead during *ramping* acceleration to smoothly transition between moves instead of performing a dead stop every move. Enabling look-ahead requires about 3600 bytes of flash memory. + Keep it disabled for embroidery. */ //#define LOOKAHEAD @@ -248,6 +250,13 @@ #define E_INVERT_ENABLE */ +#define PWR_OUT1_PIN PD4 +#define PWR_OUT2_PIN PD5 +#define ANALOG_IN_PIN 33 +#define SWITCH1_PIN 34 +#define SWITCH2_PIN 35 +#define SWITCH3_PIN PD2 + /** \def ENCODER_PIN * ENCODER_PIN_A must be connected to INT1 pin for external interrupt to work! * Motor encoder needs INT0 pin. @@ -256,6 +265,8 @@ #define ENCODER_PIN_B PA4 #define INVERT_DIRECTION_ENCODER +#endif + /** \def PULSES_PER_TURN * Motor encoder pulses per one machine shaft revolution. * If encoder wheel is mounted on motor pulley for better resolution, @@ -273,6 +284,7 @@ **/ #define MIN_MOTOR_SPEED 80 #define MAX_MOTOR_SPEED 800 +#define DEFAULT_MOTOR_SPEED 500 /** \def KX_FACTOR * Proportional and integral factor for speed controller. @@ -303,13 +315,6 @@ **/ #define POINT_SHIFT 100000 -#define PWR_OUT1_PIN PD4 -#define PWR_OUT2_PIN PD5 -#define ANALOG_IN_PIN 33 -#define SWITCH1_PIN 34 -#define SWITCH2_PIN 35 -#define SWITCH3_PIN PD2 - /** \def MAX_JUMP_LENGTH * This parameter tells motor speed planner to use lowest defined speed * for stitches of length close to this value. @@ -328,9 +333,9 @@ Sane values: 50 to 400 Valid range: 1 to 2000 **/ -#define JUMP_MOTOR_SPEED_DIFF_MAX 100 +#define JUMP_MOTOR_SPEED_DIFF_MAX 80 +#define JUMP_MOTOR_SPEED_DIFF_MAX_SLOWDOWN 20 -#endif /** \def MOVEBUFFER_SIZE Move buffer size, in number of moves. diff --git a/motor.c b/motor.c index 1d9fe1c..0c4f350 100644 --- a/motor.c +++ b/motor.c @@ -416,8 +416,8 @@ void dda_create(DDA *dda, const TARGET *target) { else if(dc_motor_speed - prev_dda->dc_motor_speed > JUMP_MOTOR_SPEED_DIFF_MAX ) dc_motor_speed = prev_dda->dc_motor_speed + JUMP_MOTOR_SPEED_DIFF_MAX; // slow down, decrease previous dda speed if needed - else if(prev_dda->dc_motor_speed - dc_motor_speed > JUMP_MOTOR_SPEED_DIFF_MAX ) - prev_dda->dc_motor_speed = dc_motor_speed + JUMP_MOTOR_SPEED_DIFF_MAX; + else if(prev_dda->dc_motor_speed - dc_motor_speed > JUMP_MOTOR_SPEED_DIFF_MAX_SLOWDOWN ) + queue_set_prev_dc_motor(dc_motor_speed); } else // no previous movement, start slowly as well dc_motor_speed = MIN_MOTOR_SPEED; @@ -428,6 +428,7 @@ void dda_create(DDA *dda, const TARGET *target) { else { dda->dc_motor_speed = 0; + queue_set_prev_dc_motor(MIN_MOTOR_SPEED); } // end of motor speed planner @@ -444,7 +445,7 @@ void dda_start(DDA *dda) { uint8_t queue_elements = queue_current_size(); if(dda->dc_motor_speed == 0 ) { - // turn off motor for jump move + // turn off motor now for jump move if (dda->waitfor == 0) desired_speed = 0; else diff --git a/queue.c b/queue.c index 54378de..48f0d0d 100644 --- a/queue.c +++ b/queue.c @@ -179,5 +179,21 @@ void queue_wait() { } } +/// sets dc motor speed in all previous blocks if it is too high +#define MB_PREV(x) ((x) == 0 ? MOVEBUFFER_SIZE -1 : (x) - 1) +void queue_set_prev_dc_motor(int16_t speed) +{ + int8_t i = (mb_head); + int8_t end = MB_PREV(mb_tail); + while (i != end){ + speed += JUMP_MOTOR_SPEED_DIFF_MAX_SLOWDOWN; + if (movebuffer[i].dc_motor_speed <= speed) + break; + //sersendf_P(PSTR("Old/new: %d/%d\n"), movebuffer[i].dc_motor_speed, speed); + movebuffer[i].dc_motor_speed = speed; + i = MB_PREV(i); + } +} + diff --git a/queue.h b/queue.h index d305028..aa47559 100644 --- a/queue.h +++ b/queue.h @@ -42,5 +42,6 @@ inline void enqueue(const TARGET *t) { } void queue_wait(); +void queue_set_prev_dc_motor(int16_t); #endif /* _QUEUE_H */ diff --git a/sensors_control.c b/sensors_control.c index 40cbfca..7dbb1b4 100644 --- a/sensors_control.c +++ b/sensors_control.c @@ -8,9 +8,9 @@ uint16_t motor_pulses = 0; int32_t ki, kp; int16_t error_speed_sum; int16_t desired_speed, error_speed; -int16_t margin_max_speed = MAX_MOTOR_SPEED; +int16_t margin_max_speed = DEFAULT_MOTOR_SPEED; -// Init INT0 and INT1 interrupts for optical sensors, init pwm and timers for dc motor speed controller +/// Init INT0 and INT1 interrupts for optical sensors, init pwm and timers for dc motor speed controller void sensing_init() { PULL_OFF(PD2); @@ -86,6 +86,7 @@ void set_dc_motor_speed(int16_t value) desired_speed = value; } +/// sets machine maximal work speed void set_dc_motor_speed_margin(int16_t value) { if (value <= MAX_MOTOR_SPEED) @@ -99,7 +100,7 @@ void set_dc_motor_speed_margin(int16_t value) margin_max_speed = MAX_MOTOR_SPEED; } -// activates stop of the the motor on needle interrupt +/// activates stop of the the motor on needle interrupt void stop_dc_motor() { if(desired_speed == 0) return; @@ -107,7 +108,7 @@ void stop_dc_motor() stop_motor_flag = 1; } -//PI control update procedure +/// PI control update procedure void update_dc_motor() { int16_t pwm_pulse; @@ -153,7 +154,7 @@ void update_dc_motor() } } -// needle interrupt +/// needle interrupt ISR(INT1_vect) { // check if second sensor is trigged to estimate needle direction @@ -186,13 +187,13 @@ ISR(INT1_vect) } } -// motor rotary encoder interrupt for speed measurement +/// motor rotary encoder interrupt for speed measurement ISR(INT0_vect) { ++motor_pulses; } -// timer tick interrupt +/// timer tick interrupt ISR(TIMER2_OVF_vect) { ++interrupt_ticks;