Fixed speed planner slow down procedure.

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.

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

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);
+    }
+}
+
 
 

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 */

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;