start at tick 0

round the fp to fixfp rather than floor

diff --git a/src/libs/StepTicker.cpp b/src/libs/StepTicker.cpp
index 5963e01..4bdc5fc 100644 (file)
--- a/src/libs/StepTicker.cpp
+++ b/src/libs/StepTicker.cpp
@@ -26,6 +26,11 @@ extern GPIO stepticker_debug_pin;
 
 StepTicker *StepTicker::instance;
 
+// handle 2.30 Fixed point
+#define FPSCALE (1<<30)
+#define TOFP(x) ((int32_t)roundf((float)(x)*FPSCALE))
+#define FROMFP(x) ((float)(x)/FPSCALE)
+
 StepTicker::StepTicker()
 {
     instance = this; // setup the Singleton instance of the stepticker
@@ -129,8 +134,6 @@ void StepTicker::step_tick (void)
         copy_block(b);
     }
 
-    current_tick++; // count number of ticks
-
     bool still_moving = false;
 
     // foreach motor, if it is active see if time to issue a step to that motor
@@ -159,14 +162,14 @@ void StepTicker::step_tick (void)
 
         // protect against rounding errors and such
         if(tick_info[m].steps_per_tick <= 0) {
-            tick_info[m].counter = (1<<30); // we complete this step
+            tick_info[m].counter = FPSCALE; // we force completion this step by setting to 1.0
             tick_info[m].steps_per_tick = 0;
         }
 
         tick_info[m].counter += tick_info[m].steps_per_tick;
 
-        if(tick_info[m].counter >= (1<<30)) { // > 1.0 step time
-            tick_info[m].counter -= (1<<30); // -= 1.0F;
+        if(tick_info[m].counter >= FPSCALE) { // > 1.0 step time
+            tick_info[m].counter -= FPSCALE; // -= 1.0F;
             ++tick_info[m].step_count;
 
             // step the motor
@@ -181,6 +184,9 @@ void StepTicker::step_tick (void)
         }
     }
 
+    // do this after so we start at tick 0
+    current_tick++; // count number of ticks
+
     // We may have set a pin on in this tick, now we reset the timer to set it off
     // Note there could be a race here if we run another tick before the unsteps have happened,
     // right now it takes about 3-4us but if the unstep were near 10uS or greater it would be an issue
@@ -211,7 +217,7 @@ void StepTicker::step_tick (void)
             #endif
 
         } else {
-            move_issued = false; // nothing to do as no more blocks
+            move_issued = false; // nothing to do as no more jobs
         }
 
         // all moves finished
@@ -221,6 +227,7 @@ void StepTicker::step_tick (void)
     }
 }
 
+// this takes about 20us, so we may miss two ticks, we can make this up if needed
 // called in ISR if running, else can be called from anything to start
 void StepTicker::copy_block(Block *block)
 {
@@ -239,7 +246,7 @@ void StepTicker::copy_block(Block *block)
         motor[m]->set_direction(block->direction_bits[m]);
 
         float aratio = inv * steps;
-        tick_info[m].steps_per_tick = floorf(((block->initial_rate * aratio) / frequency) * (1<<30)); // steps/sec / tick frequency to get steps per tick in 2.30 fixed point
+        tick_info[m].steps_per_tick = TOFP((block->initial_rate * aratio) / frequency); // steps/sec / tick frequency to get steps per tick in 2.30 fixed point
         tick_info[m].counter = 0; // 2.30 fixed point
         tick_info[m].step_count = 0;
         tick_info[m].next_accel_event = block->total_move_ticks + 1;
@@ -259,9 +266,9 @@ void StepTicker::copy_block(Block *block)
         }
 
         // convert to fixed point after scaling
-        tick_info[m].acceleration_change= floorf((acceleration_change * aratio) * (1<<30));
-        tick_info[m].deceleration_change= -floorf((block->deceleration_per_tick * aratio) * (1<<30));
-        tick_info[m].plateau_rate= floorf(((block->maximum_rate * aratio) / frequency) * (1<<30));
+        tick_info[m].acceleration_change= TOFP(acceleration_change * aratio);
+        tick_info[m].deceleration_change= -TOFP(block->deceleration_per_tick * aratio);
+        tick_info[m].plateau_rate= TOFP((block->maximum_rate * aratio) / frequency);
     }
     move_issued = true;
     stepticker_debug_pin = 0;

diff --git a/src/libs/StepperMotor.h b/src/libs/StepperMotor.h
index 38a315f..8a9f8ac 100644 (file)
--- a/src/libs/StepperMotor.h
+++ b/src/libs/StepperMotor.h
@@ -8,11 +8,6 @@
 #pragma once
 
 #include "Pin.h"
-#include <atomic>
-#include <functional>
-
-class StepTicker;
-class Hook;
 
 class StepperMotor {
     public:

diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp
index c2a4435..01acab5 100644 (file)
--- a/src/modules/robot/Block.cpp
+++ b/src/modules/robot/Block.cpp
@@ -119,7 +119,7 @@ void Block::calculate_trapezoid( float entryspeed, float exitspeed )
     // Now this is the maximum rate we'll achieve this move, either because
     // it's the higher we can achieve, or because it's the higher we are
     // allowed to achieve
-    this->maximum_rate = std::min(maximum_possible_rate, (float)this->nominal_rate);
+    this->maximum_rate = std::min(maximum_possible_rate, this->nominal_rate);
 
     // Now figure out how long it takes to accelerate in seconds
     float time_to_accelerate = ( this->maximum_rate - initial_rate ) / acceleration_per_second;
@@ -172,10 +172,6 @@ void Block::calculate_trapezoid( float entryspeed, float exitspeed )
     float acceleration_in_steps = (acceleration_time > 0.0F ) ? ( this->maximum_rate - initial_rate ) / acceleration_time : 0;
     float deceleration_in_steps =  (deceleration_time > 0.0F ) ? ( this->maximum_rate - final_rate ) / deceleration_time : 0;
 
-    // Note we use this value for acceleration as well as for deceleration, if that doesn't work, we can also as well compute the deceleration value this way :
-    // float deceleration(steps/s²) = ( final_rate(steps/s) - maximum_rate(steps/s) ) / acceleration_time(s);
-    // and store that in the block and use it for deceleration, which -will- yield better results, but may not be useful. If the moves do not end correctly, try computing this value, adding it to the block, and then using it for deceleration in the step generator
-
     // Now figure out the two acceleration ramp change events in ticks
     this->accelerate_until = acceleration_ticks;
     this->decelerate_after = total_move_ticks - deceleration_ticks;