Fixed acceleration algorithm. Added deceleration. Added configuration setting for acceleration in configuration.h

1 files changed, 30 insertions, 10 deletions

diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde
index a0d48b4..3014d4b 100644
--- a/Tonokip_Firmware/Tonokip_Firmware.pde
+++ b/Tonokip_Firmware/Tonokip_Firmware.pde
@@ -51,6 +51,7 @@
 bool direction_x, direction_y, direction_z, direction_e;
 unsigned long previous_micros=0, previous_micros_x=0, previous_micros_y=0, previous_micros_z=0, previous_micros_e=0, previous_millis_heater;
 unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take;
+unsigned long long_full_velocity_units = full_velocity_units * 100;
 float destination_x =0.0, destination_y = 0.0, destination_z = 0.0, destination_e = 0.0;
 float current_x = 0.0, current_y = 0.0, current_z = 0.0, current_e = 0.0;
 long x_interval, y_interval, z_interval, e_interval; // for speed delay
@@ -744,38 +745,53 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin
   int error_x;
   int error_y;
   int error_z;
-  long full_velocity_units = 30;
+  unsigned long virtual_full_velocity_steps;
   unsigned long full_velocity_steps;
+  unsigned long steps_remaining;
+  unsigned long steps_to_take;
   
   if(steep_y) {
    error_x = delta_y / 2;
    previous_micros_y=micros();
    interval = y_interval;
-   full_velocity_steps = full_velocity_units * y_steps_per_unit /100;
-   if (full_velocity_steps > y_steps_remaining) full_velocity_steps = y_steps_remaining;
+   virtual_full_velocity_steps = long_full_velocity_units * y_steps_per_unit /100;
+   full_velocity_steps = min(virtual_full_velocity_steps, delta_y / 2);
+   steps_remaining = delta_y;
+   steps_to_take = delta_y;
   } else if (steep_x) {
    error_y = delta_x / 2;
    previous_micros_x=micros();
    interval = x_interval;
-   full_velocity_steps = full_velocity_units * x_steps_per_unit /100;
-   if (full_velocity_steps > x_steps_remaining) full_velocity_steps = x_steps_remaining;
+   virtual_full_velocity_steps = long_full_velocity_units * x_steps_per_unit /100;
+   full_velocity_steps = min(virtual_full_velocity_steps, delta_x / 2);
+   steps_remaining = delta_x;
+   steps_to_take = delta_x;
   }
+  if(full_velocity_steps == 0) full_velocity_steps++;
   long full_interval = interval;
   unsigned long steps_done = 0;
-  unsigned int steps_acceleration_check = 100;
+  unsigned int steps_acceleration_check = 1;
   
   // move until no more steps remain 
   while(x_steps_remaining + y_steps_remaining + z_steps_remaining + e_steps_remaining > 0) { 
     if (steps_done < full_velocity_steps && steps_done / full_velocity_steps < 1 && (steps_done % steps_acceleration_check == 0)) {
       if(steps_done == 0) {
-        interval = full_interval * steps_acceleration_check / full_velocity_steps;
+        interval = full_interval * virtual_full_velocity_steps;
       } else {
-        interval = full_interval * steps_done / full_velocity_steps;
+        interval = full_interval * virtual_full_velocity_steps / steps_done;
       }
-    } else if (steps_done - full_velocity_steps >= 1) {
+    } else if (steps_remaining < full_velocity_steps) {
+      if(steps_remaining == 0) {
+        interval = full_interval * virtual_full_velocity_steps;
+      } else {
+        interval = full_interval * virtual_full_velocity_steps / steps_remaining;
+      }
+    } else if (steps_done - full_velocity_steps >= 1){
       interval = full_interval;
     }
-    steps_done++;
+      
+    
+    
       
     if(x_steps_remaining || y_steps_remaining) {
       if(X_MIN_PIN > -1) if(!direction_x) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) x_steps_remaining=0;
@@ -785,6 +801,8 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin
       if(steep_y) {
         timediff = micros() - previous_micros_y;
         while(timediff >= interval && y_steps_remaining>0) {
+          steps_done++;
+          steps_remaining--;
           y_steps_remaining--; timediff-=interval;
           error_x = error_x - delta_x;
           do_y_step();
@@ -796,6 +814,8 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin
       } else if (steep_x) {
         timediff=micros() - previous_micros_x;
         while(timediff >= interval && x_steps_remaining>0) {
+          steps_done++;
+          steps_remaining--;
           x_steps_remaining--; timediff-=interval;
           error_y = error_y - delta_y;
           do_x_step();