From 6149d396f7ed9d6fdc6807cb70b0962e4e433b54 Mon Sep 17 00:00:00 2001
From: Martin <markol@10g.pl>
Date: Tue, 23 Oct 2018 04:11:23 +0200
Subject: [PATCH] DC motor max speed control Gcode implemented.

---
 config.h          |  2 +-
 gcode_parser.c    | 15 ++++++++++-----
 motor.c           | 24 ++++++++++++++++--------
 queue.c           |  2 +-
 sensors_control.c | 14 ++++++++++++++
 sensors_control.h |  2 ++
 6 files changed, 44 insertions(+), 15 deletions(-)

diff --git a/config.h b/config.h
index 529b280..6886cce 100644
--- a/config.h
+++ b/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. 
diff --git a/gcode_parser.c b/gcode_parser.c
index 0543a4d..e3c9afc 100644
--- a/gcode_parser.c
+++ b/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;
diff --git a/motor.c b/motor.c
index 7677bd1..1d9fe1c 100644
--- a/motor.c
+++ b/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
diff --git a/queue.c b/queue.c
index df51833..54378de 100644
--- a/queue.c
+++ b/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
diff --git a/sensors_control.c b/sensors_control.c
index 3aeb9dc..40cbfca 100644
--- a/sensors_control.c
+++ b/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()
 {
diff --git a/sensors_control.h b/sensors_control.h
index 7285e49..ae6823f 100644
--- a/sensors_control.h
+++ b/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