more advanced backlash compensation

Signed-off-by: Stepan Usatyuk <usaatyuk@ustk.me>

Firmware/EggbotWireless/include/Globals.h

@@ -0,0 +1,25 @@
+#include <Arduino.h>
+#include "Stepper.h"
+#include "Pen.h"
+#include "EggConstants.h"
+#include "StepperTimer.h"
+
+#ifndef GLOBALS_H
+#define GLOBALS_H
+
+#define STEPS_PER_REVOLUTION 4076
+#define DEF_RPM 2
+
+Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION, 10, 0,
+                   Y_DEGREES_PER_MM, typeBoth, false);
+Stepper servoStepper(D5, D6, D7, D8, -STEPS_PER_REVOLUTION, 10, X_LIMIT,
+                     X_DEGREES_PER_MM, typeClockwise, true);
+
+StepperTimer eggStepperTimer(DEF_RPM, STEPS_PER_REVOLUTION);
+StepperTimer servoStepperTimer(DEF_RPM, STEPS_PER_REVOLUTION);
+
+Pen pen(D0, 100, 150);
+
+String inString;
+
+#endif

Firmware/EggbotWireless/include/Pen.h

@@ -16,6 +16,7 @@ class Pen {
     Pen(int pin, int posEngaged, int posDisengaged);
     void engage();
     void disengage();
+    void init();
     bool getEngaged();
     ~Pen();
 };

Firmware/EggbotWireless/include/Stepper.h

@@ -1,6 +1,8 @@
 #ifndef STEPPER_H
 #define STEPPER_H
 
+enum backlashCompType { typeClockwise, typeCounterClockwise, typeBoth };
+
 class Stepper {
    private:
     int stepsPerRevolution;
@@ -18,9 +20,14 @@ class Stepper {
     void step(int steps);
     float pos = 0;
     int limit;
+    bool compAlways;
+
+    backlashCompType backlashComp;
 
    public:
-    Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution, int backlashSteps, int limit, float degreesPerMM);
+    Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution,
+            int backlashSteps, int limit, float degreesPerMM,
+            backlashCompType backlashComp, bool compAlways);
     int getRemainingSteps();
     void rotate(float degrees);
     void rotateTo(float degrees);

Firmware/EggbotWireless/src/Pen.cpp

@@ -26,6 +26,11 @@ void Pen::disengage() {
     engaged = false;
 }
 
+void Pen::init() {
+    servo.write(posDisengaged);
+    engaged = false;
+}
+
 bool Pen::getEngaged() { return engaged; }
 
 Pen::~Pen() {}

Firmware/EggbotWireless/src/Stepper.cpp

@@ -116,7 +116,8 @@ void Stepper::counterClockwise() {
 }
 
 Stepper::Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution,
-                 int backlashSteps, int limit, float degreesPerMM)
+                 int backlashSteps, int limit, float degreesPerMM,
+                 backlashCompType backlashComp, bool compAlways)
     : pin1(pin1),
       pin2(pin2),
       pin3(pin3),
@@ -124,7 +125,9 @@ Stepper::Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution,
       stepsPerRevolution(stepsPerRevolution),
       backlashSteps(backlashSteps),
       limit(limit),
-      degreesPerMM(degreesPerMM) {
+      degreesPerMM(degreesPerMM),
+      backlashComp(backlashComp),
+      compAlways(compAlways) {
     pinMode(pin1, OUTPUT);
     pinMode(pin2, OUTPUT);
     pinMode(pin3, OUTPUT);
@@ -147,13 +150,20 @@ void Stepper::step(int steps) {
         return;
     }
     if (steps > 0) {
-        if (!direction) {
+        if (!direction || compAlways) {
-            remainingSteps += backlashSteps;
+            if (backlashComp == typeClockwise || backlashComp == typeBoth) {
+                remainingSteps += backlashSteps;
+            }
         }
         direction = true;
         remainingSteps += steps;
     } else {
-        if (direction) {
+        if (direction || compAlways) {
+            if (backlashComp == typeCounterClockwise ||
+                backlashComp == typeBoth) {
+                remainingSteps += backlashSteps;
+            }
+
             remainingSteps += backlashSteps;
         }
         direction = false;

Firmware/EggbotWireless/src/main.cpp

@@ -2,24 +2,12 @@
 #include <ESP8266WiFi.h>
 #include "EggConstants.h"
 #include "GCodeParser.h"
+#include "Globals.h"
 #include "Pen.h"
 #include "Stepper.h"
 #include "StepperTimer.h"
 
-#define STEPS_PER_REVOLUTION 4076
+int RPM = DEF_RPM;
-
-int RPM = 2;
-
-Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION, 40, 0,
-                   Y_DEGREES_PER_MM);
-Stepper servoStepper(D5, D6, D7, D8, -STEPS_PER_REVOLUTION, 40, X_LIMIT,
-                     X_DEGREES_PER_MM);
-Pen pen(D0, 100, 150);
-
-StepperTimer eggStepperTimer(RPM, STEPS_PER_REVOLUTION);
-StepperTimer servoStepperTimer(RPM, STEPS_PER_REVOLUTION);
-
-String inString;
 
 void execCommand(float *command) {
     if (command[0] == G01) {
@@ -73,7 +61,7 @@ void setup() {
     Serial.begin(115200);
     eggStepperTimer.setStepper(&eggStepper);
     servoStepperTimer.setStepper(&servoStepper);
-    pen.disengage();
+    pen.init();
     servoStepper.setPos(70);
     WiFi.mode(WIFI_OFF);
 }
@@ -97,6 +85,9 @@ void loop() {
                 } else if (stepsY > stepsX) {
                     float rpm = (float)RPM * (float)stepsX / (float)stepsY;
                     servoStepperTimer.setRPM(rpm);
+                } else {
+                    eggStepperTimer.setRPM(RPM);
+                    servoStepperTimer.setRPM(RPM);
                 }
                 sei();
             }