From 40436f4e08c9ff943dbc1fdc76cc42d730c3047e Mon Sep 17 00:00:00 2001
From: Stepan Usatiuk <stepanusatiuk@gmail.com>
Date: Thu, 9 May 2019 14:09:57 +0300
Subject: [PATCH] stepper backlash compensation

Signed-off-by: Stepan Usatyuk <usaatyuk@ustk.me>
---
 Firmware/EggbotWireless/include/Stepper.h |  4 +++-
 Firmware/EggbotWireless/src/Stepper.cpp   | 24 +++++++++++++++++------
 Firmware/EggbotWireless/src/main.cpp      | 19 ++++++++++++++----
 3 files changed, 36 insertions(+), 11 deletions(-)

diff --git a/Firmware/EggbotWireless/include/Stepper.h b/Firmware/EggbotWireless/include/Stepper.h
index c1c5962..b613413 100644
--- a/Firmware/EggbotWireless/include/Stepper.h
+++ b/Firmware/EggbotWireless/include/Stepper.h
@@ -10,15 +10,17 @@ class Stepper {
     int pin4;
     int curStep = 1;
     int remainingSteps;
+    int backlashSteps;
     bool direction;
     void clockwise();
     void counterClockwise();
 
    public:
-    Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution);
+    Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution, int backlashSteps);
     void step(int steps);
     void rotate(float degrees);
     void doStep();
+    bool finished();
 };
 
 #endif
\ No newline at end of file
diff --git a/Firmware/EggbotWireless/src/Stepper.cpp b/Firmware/EggbotWireless/src/Stepper.cpp
index eea3178..09c195a 100644
--- a/Firmware/EggbotWireless/src/Stepper.cpp
+++ b/Firmware/EggbotWireless/src/Stepper.cpp
@@ -115,12 +115,14 @@ void Stepper::counterClockwise() {
     }
 }
 
-Stepper::Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution)
+Stepper::Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution,
+                 int backlashSteps)
     : pin1(pin1),
       pin2(pin2),
       pin3(pin3),
       pin4(pin4),
-      stepsPerRevolution(stepsPerRevolution) {
+      stepsPerRevolution(stepsPerRevolution),
+      backlashSteps(backlashSteps) {
     pinMode(pin1, OUTPUT);
     pinMode(pin2, OUTPUT);
     pinMode(pin3, OUTPUT);
@@ -128,8 +130,8 @@ Stepper::Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution)
 }
 
 void Stepper::doStep() {
-    if(remainingSteps > 0) {
-        if(direction){
+    if (remainingSteps > 0) {
+        if (direction) {
             clockwise();
         } else {
             counterClockwise();
@@ -140,15 +142,25 @@ void Stepper::doStep() {
 
 void Stepper::step(int steps) {
     if (steps > 0) {
+        if (!direction) {
+            remainingSteps += backlashSteps;
+        }
         direction = true;
-        remainingSteps = steps;
+        remainingSteps += steps;
     } else {
+        if (direction) {
+            remainingSteps += backlashSteps;
+        }
         direction = false;
-        remainingSteps = abs(steps);
+        remainingSteps += abs(steps);
     }
 }
 
 void Stepper::rotate(float degrees) {
     int steps = (degrees * stepsPerRevolution) / 360;
     step(steps);
+}
+
+bool Stepper::finished() {
+    return remainingSteps == 0;
 }
\ No newline at end of file
diff --git a/Firmware/EggbotWireless/src/main.cpp b/Firmware/EggbotWireless/src/main.cpp
index 606cf4d..f54a6b6 100644
--- a/Firmware/EggbotWireless/src/main.cpp
+++ b/Firmware/EggbotWireless/src/main.cpp
@@ -4,18 +4,29 @@
 #include "StepperTimer.h"
 
 #define STEPS_PER_REVOLUTION 4076
+#define BACKLASH_STEPS 40
+#define RPM 6
 
-Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION);
-Stepper servoStepper(D5, D6, D7, D8, STEPS_PER_REVOLUTION);
-
+Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION, BACKLASH_STEPS);
+Stepper servoStepper(D5, D6, D7, D8, STEPS_PER_REVOLUTION, BACKLASH_STEPS);
 Pen pen(D0, 180, 80);
 
+String inString;
+
 void setup() {
     Serial.begin(115200);
     stepperTimerSetStepper(0, &eggStepper);
     stepperTimerSetStepper(1, &servoStepper);
-    stepperTimerInit(6, STEPS_PER_REVOLUTION);
+    stepperTimerInit(RPM, STEPS_PER_REVOLUTION);
 }
 
 void loop() {
+    while (Serial.available() > 0) {
+        int inChar = Serial.read();
+        Serial.write(inChar);
+        
+        if (inChar == '\n') {
+            inString = "";
+        }
+    }
 }
\ No newline at end of file