DC motor max speed control Gcode implemented.

config.h

@@ -272,7 +272,7 @@
     Valid range: 1 to 3000
  **/
 #define MIN_MOTOR_SPEED 80
-#define MAX_MOTOR_SPEED 400
+#define MAX_MOTOR_SPEED 800
 
 /** \def KX_FACTOR
  * Proportional and integral factor for speed controller. 

gcode_parser.c

@@ -9,7 +9,7 @@
 GCODE_PARAM BSS gcode_params[8];
 static volatile uint8_t current_parameter = 0;
 uint8_t option_all_relative = 0;
-int16_t speed_limit = 0;
+int16_t parameter_1 = 0;
 TARGET BSS next_target;
 
 // Parser is implemented as a finite state automata (DFA)
@@ -95,7 +95,7 @@ uint8_t process_command()
                 next_target.F = decfloat_to_int(gcode_params[i].value, gcode_params[i].exponent, gcode_params[i].is_negative, 1);
             break;
             case 'S':
-               speed_limit = decfloat_to_int(gcode_params[i].value, gcode_params[i].exponent, gcode_params[i].is_negative, 1);
+               parameter_1 = decfloat_to_int(gcode_params[i].value, gcode_params[i].exponent, gcode_params[i].is_negative, 1);
             break;
         }
     }
@@ -207,10 +207,15 @@ uint8_t process_command()
                     serial_writestr_P(PSTR("\n"));
                     endstops_off();
                 break;
-                case 202: //set acceleration
+                
+                case 202:
+                    //set acceleration not supported
                 break;
-                case 222: //set speed
-                    desired_speed = speed_limit;
+                case 222:
+                    //? Example: M222 S400
+                    //?
+                    //? Set dc motor max speed
+                    set_dc_motor_speed_margin(parameter_1);
                 break;
                 default:
                     result = STATE_ERROR;

motor.c

@@ -285,10 +285,12 @@ void dda_create(DDA *dda, const TARGET *target) {
     if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
         sersendf_P(PSTR(" [ts:%lu"), dda->total_steps);
 
-    if (dda->total_steps == 0) {
-        dda->nullmove = 1;
-    }
-    else {
+    // null moves should be accepted anyway
+    //if (dda->total_steps == 0) {
+    //    dda->nullmove = 1;
+    //}
+    //else 
+    {
         // get steppers ready to go
         //power_on();
         stepper_enable();
@@ -399,8 +401,8 @@ void dda_create(DDA *dda, const TARGET *target) {
     if (dda->waitfor)
     {
 		// calculate dc motor speed according to distance, is linear interpolation ok here?
-		int16_t dc_motor_speed = MIN_MOTOR_SPEED + ((MAX_MOTOR_SPEED - MIN_MOTOR_SPEED) * ((int32_t)(distance - MAX_JUMP_LENGTH*1000) / (1 - MAX_JUMP_LENGTH))) / 1000;
-		if(dc_motor_speed > MAX_MOTOR_SPEED) dc_motor_speed = MAX_MOTOR_SPEED;
+		int16_t dc_motor_speed = MIN_MOTOR_SPEED + ((margin_max_speed - MIN_MOTOR_SPEED) * ((int32_t)(distance - MAX_JUMP_LENGTH * 1000) / (1 - MAX_JUMP_LENGTH))) / 1000;
+		if(dc_motor_speed > margin_max_speed) dc_motor_speed = margin_max_speed;
 		else if(dc_motor_speed < MIN_MOTOR_SPEED) dc_motor_speed = MIN_MOTOR_SPEED;
 		
 		if(dc_motor_speed > 0)
@@ -440,8 +442,14 @@ void dda_start(DDA *dda) {
 
         // apply dc motor speed
         uint8_t queue_elements = queue_current_size();
-        if(dda->dc_motor_speed == 0 ) 
-			stop_dc_motor();
+        if(dda->dc_motor_speed == 0 )
+        { 
+			// turn off motor for jump move
+			if (dda->waitfor == 0)
+				desired_speed = 0;
+			else
+				stop_dc_motor();
+		}
 		else
 		{
 			// slow down on almost empty buffer

queue.c

@@ -56,7 +56,7 @@ DDA *queue_current_movement() {
 
   ATOMIC_START
     current = &movebuffer[mb_tail];
-
+    
     if ( ! current->live || current->waitfor || current->nullmove)
       current = NULL;
   ATOMIC_END

sensors_control.c

@@ -8,6 +8,7 @@ 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;
 
 // Init INT0 and INT1 interrupts for optical sensors, init pwm and timers for dc motor speed controller
 void sensing_init()
@@ -85,6 +86,19 @@ void set_dc_motor_speed(int16_t value)
 	desired_speed = value;
 }
 
+void set_dc_motor_speed_margin(int16_t value)
+{
+	if (value <= MAX_MOTOR_SPEED)
+	{
+		if (value >= MIN_MOTOR_SPEED)
+			margin_max_speed = value;
+		else
+			margin_max_speed = MIN_MOTOR_SPEED;
+	}
+	else 
+		margin_max_speed = MAX_MOTOR_SPEED;
+}
+
 // activates stop of the the motor on needle interrupt
 void stop_dc_motor()
 {

sensors_control.h

@@ -11,6 +11,7 @@
 #endif
 
 extern int16_t desired_speed;
+extern int16_t margin_max_speed;
 void stop_dc_motor();
 
 #ifdef __cplusplus
@@ -18,6 +19,7 @@ extern "C" {
 #endif
 void sensing_init();
 void update_dc_motor();
+void set_dc_motor_speed_margin(int16_t);
  #ifdef __cplusplus
 }
 #endif