refactoring

src/Config.h

@@ -11,7 +11,7 @@
 
   const int LOGGING = 0;
 
-  const int INCOMING_CMD_BUF_SIZE = 50;
+  const int INCOMING_CMD_BUF_SIZE = 100;
 
   const char COMM_REPORT_CMD_IDLE[4] = {'R', '0', '0', '\0'};
   const char COMM_REPORT_CMD_START[4] = {'R', '0', '1', '\0'};

src/StepperControlAxis.cpp

@@ -146,6 +146,16 @@ uint16_t StepperControlAxis::getLoad() {
 
 #endif
 
+long StepperControlAxis::getLastPosition()
+{
+  return axisLastPosition;
+}
+
+void StepperControlAxis::setLastPosition(long position)
+{
+  axisLastPosition = position;
+}
+
 unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec)
 {
 

src/StepperControlAxis.h

@@ -52,7 +52,7 @@ public:
   bool isCruising();
   bool isCrawling();
   bool isMotorActive();
-  bool isMoving();
+  //bool isMoving();
 
   bool endStopMin();
   bool endStopMax();
@@ -76,6 +76,9 @@ public:
   void setMovementUp();
   void setMovementDown();
 
+  long getLastPosition();
+  void setLastPosition(long position);
+
   bool movingToHome();
   bool movingUp();
 
@@ -151,6 +154,8 @@ private:
   long coordDestinationPoint;
   bool coordHomeAxis; // homing command
 
+  long axisLastPosition = 0;
+
   // movement handling
   unsigned long movementLength;
   unsigned long maxLenth;

src/StepperControlEncoder.cpp

@@ -248,3 +248,77 @@ void StepperControlEncoder::shiftChannels()
   prvValChannelA = curValChannelA;
   prvValChannelB = curValChannelB;
 }
+
+
+void StepperControlEncoder::setEnable(bool enable)
+{
+  encoderEnabled = enable;
+}
+
+void StepperControlEncoder::setStepDecay(float stepDecay)
+{
+  encoderStepDecay = stepDecay;
+}
+
+void StepperControlEncoder::setMovementDirection(bool up)
+{
+  encoderMovementUp = up;
+}
+
+float StepperControlEncoder::getMissedSteps()
+{
+  return missedSteps;
+}
+
+void StepperControlEncoder::checkMissedSteps()
+{
+  #if !defined(FARMDUINO_EXP_V20)
+    if (encoderEnabled)
+    {
+      bool stepMissed = false;
+
+      // Decrease amount of missed steps if there are no missed step
+      if (missedSteps > 0)
+      {
+        (missedSteps) -= (encoderStepDecay);
+      }
+
+      // Check if the encoder goes in the wrong direction or nothing moved
+      if ((encoderMovementUp && encoderLastPosition > currentPositionRaw()) ||
+        (!encoderMovementUp && encoderLastPosition < currentPositionRaw()))
+      {
+        stepMissed = true;
+      }
+
+      if (stepMissed && missedSteps < 32000)
+      {
+        (missedSteps)++;
+      }
+
+      encoderLastPosition = currentPositionRaw();
+      //axis->setLastPosition(axis->currentPosition());
+   }
+
+  #endif
+
+  #if defined(FARMDUINO_EXP_V20)
+
+    if (encoderEnabled) {
+      if (axis->stallDetected()) {
+        // In case of stall detection, count this as a missed step
+        (*missedSteps)++;
+        axis->setCurrentPosition(*lastPosition);
+      }
+      else {
+        // Decrease amount of missed steps if there are no missed step
+        if (missedSteps > 0)
+        {
+          (missedSteps) -= (encoderStepDecay);
+        }
+        encoder->setPosition(axis->currentPosition());
+        //axis->setLastPosition(axis->currentPosition());
+      }
+    }
+  #endif
+
+}

src/StepperControlEncoder.h

@@ -16,6 +16,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <SPI.h>
+#include "StepperControlAxis.h"
 
 class StepperControlEncoder
 {
@@ -40,6 +41,13 @@ public:
   void shiftChannels();
   void test();
 
+  void setMovementDirection(bool up);
+  void setEnable(bool enable);
+  void setStepDecay(float stepDecay);
+  float getMissedSteps();
+
+  void checkMissedSteps();
+
 private:
   // pin settings
   int pinChannelA;
@@ -64,6 +72,13 @@ private:
   int encoderType;
   int encoderInvert;
 
+
+  float missedSteps;
+  long encoderLastPosition;
+  float encoderStepDecay;
+  bool encoderEnabled;
+  bool encoderMovementUp;
+
   MdlSpiEncoders mdlEncoder = _MDL_X1;
 
 };