Merge pull request #314 from Copper280z/FEAT_velocity_feadforward

added velocity calculation to StepDirListener for use in FF

src/communication/StepDirListener.cpp

@@ -1,4 +1,5 @@
 #include "StepDirListener.h"
+#include "common/time_utils.h"
 
 StepDirListener::StepDirListener(int _pinStep, int _pinDir, float _counter_to_value){
     pin_step = _pinStep;
@@ -10,31 +11,82 @@ void StepDirListener::init(){
     pinMode(pin_dir, INPUT);
     pinMode(pin_step, INPUT_PULLUP);
     count = 0;
+    prev_pulse_time = 0;
+    current_pulse_time = 0;
+    elapsed_time = 0;
 }
 
 void StepDirListener::enableInterrupt(void (*doA)()){
     attachInterrupt(digitalPinToInterrupt(pin_step), doA, polarity);
 }
 
-void StepDirListener::attach(float* variable){
-    attached_variable = variable;
+void StepDirListener::attach(float* pos_var){
+    attached_position = pos_var;
 }
+void StepDirListener::attach(float* pos_var, float* vel_var){
+    attached_position = pos_var;
+    attached_velocity = vel_var;
+}
+
 
 void StepDirListener::handle(){ 
-  // read step status
-  //bool step = digitalRead(pin_step);
-  // update counter only on rising edge 
-  //if(step && step != step_active){
-    if(digitalRead(pin_dir)) 
+  // read dir status
+    dir_state = digitalRead(pin_dir);
+    if(dir_state) {
         count++;
-    else 
+    } else { 
         count--;
-   //}
-   //step_active = step;
+   }
+   current_pulse_time = _micros();
+
    // if attached variable update it
-   if(attached_variable) *attached_variable = getValue();
+   if(attached_position) {
+        *attached_position = getValue();
+        if(attached_velocity) {
+            // can use STM32 input capture peripheral to do this too
+            if(dir_state) {
+                *attached_velocity = getVelocityValue();
+            } else {
+                *attached_velocity = -getVelocityValue();
+            }
+        }
+    }
+   prev_pulse_time = current_pulse_time;
+
 }
 // calculate the position from counter
 float StepDirListener::getValue(){
-    return (float) count * counter_to_value;
-}
+    noInterrupts();
+    float position_value = (float) count * counter_to_value;
+    interrupts();
+    return position_value;
+}
+float StepDirListener::getVelocityValue(){
+
+    noInterrupts();
+    int tmp_pulse_time = current_pulse_time;
+    int tmp_prev_pulse_time = prev_pulse_time;
+    interrupts();
+
+    elapsed_time = tmp_pulse_time - tmp_prev_pulse_time;
+    if (elapsed_time <= 0) {
+        // caps feedforward velocity based on time precision
+        // if zero microseconds have elapsed, assume 1us has elapsed
+        elapsed_time = 1;
+    }
+    // no need to subtract current position from previous position 
+    // that's always 1 unless we missed an IRQ
+    return (float) counter_to_value * 1e6 / elapsed_time; 
+}
+
+void StepDirListener::update(){
+    noInterrupts();
+    int tmp_prev_pulse_time = prev_pulse_time;
+    int tmp_elapsed_time = elapsed_time;
+    interrupts();
+
+    int current_time = _micros();
+    if ((current_time - tmp_prev_pulse_time) > 5 * tmp_elapsed_time) {
+        *attached_velocity = 0;
+    }
+}

src/communication/StepDirListener.h

@@ -48,18 +48,35 @@ class StepDirListener
      * - no need to call getValue function
      */
     void attach(float* variable);
-
+    void attach(float* pos_var, float* vel_var);
+    /**
+     * Get the calulated velocity value by 
+     * finite differencing the time between step pulses
+     * - no need to call this function outside of IRQ
+     **/
+    float getVelocityValue();
+    /**
+     * Deal with zero velocity by setting velocity to zero 
+     * if it's been a long time since the last pulse.
+     * call this in the main loop somewhat often
+     **/
+    void update();
     // variables
     int pin_step; //!< step pin
     int pin_dir; //!< direction pin
     long count; //!< current counter value - should be set to 0 for homing
+    float counter_to_value; //!< step counter to value 
     PinStatus polarity = RISING; //!< polarity of the step pin
 
   private:
-    float* attached_variable = nullptr; //!< pointer to the attached variable 
-    float counter_to_value; //!< step counter to value 
+    volatile float* attached_position = nullptr; //!< pointer to the attached variable 
+    volatile float* attached_velocity = nullptr; //!< pointer to the attached variable 
+    volatile int prev_pulse_time;
+    volatile int current_pulse_time;
+    volatile int elapsed_time;
+    volatile bool dir_state;
     //bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable
 
 };
 
-#endif
+#endif