keep track of stepper's position

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

Firmware/EggbotWireless/include/Stepper.h

@@ -14,13 +14,17 @@ class Stepper {
     bool direction;
     void clockwise();
     void counterClockwise();
+    void step(int steps);
+    int degreesToSteps(float degrees);
+    float pos = 0;
+    int limit;
 
    public:
-    Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution, int backlashSteps);
+    Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution, int backlashSteps, int limit);
-    void step(int steps);
     void rotate(float degrees);
     void doStep();
     bool finished();
+    float getPos();
 };
 
 #endif

Firmware/EggbotWireless/src/Stepper.cpp

@@ -116,13 +116,14 @@ void Stepper::counterClockwise() {
 }
 
 Stepper::Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution,
-                 int backlashSteps)
+                 int backlashSteps, int limit)
     : pin1(pin1),
       pin2(pin2),
       pin3(pin3),
       pin4(pin4),
       stepsPerRevolution(stepsPerRevolution),
-      backlashSteps(backlashSteps) {
+      backlashSteps(backlashSteps),
+      limit(limit) {
     pinMode(pin1, OUTPUT);
     pinMode(pin2, OUTPUT);
     pinMode(pin3, OUTPUT);
@@ -141,6 +142,9 @@ void Stepper::doStep() {
 }
 
 void Stepper::step(int steps) {
+    if (steps == 0) {
+        return;
+    }
     if (steps > 0) {
         if (!direction) {
             remainingSteps += backlashSteps;
@@ -156,11 +160,31 @@ void Stepper::step(int steps) {
     }
 }
 
-void Stepper::rotate(float degrees) {
+int Stepper::degreesToSteps(float degrees) {
-    int steps = (degrees * stepsPerRevolution) / 360;
+    return (degrees * stepsPerRevolution) / 360;
-    step(steps);
 }
 
-bool Stepper::finished() {
+void Stepper::rotate(float degrees) {
-    return remainingSteps == 0;
+    int steps = degreesToSteps(degrees);
+    if (!limit) {
+        pos = fmod((pos + degrees), 360);
+        if (pos < 0) {
+            pos = 360 + pos;
         }
+        step(steps);
+    } else {
+        if (degrees + pos > limit) {
+            step(degreesToSteps(limit - pos));
+            pos = limit;
+        } else if (degrees + pos < 0) {
+            step(degreesToSteps(-pos));
+            pos = 0;
+        } else {
+            step(steps);
+            pos += degrees;
+        }
+    }
+}
+
+bool Stepper::finished() { return remainingSteps == 0; }
+float Stepper::getPos() { return pos; }

Firmware/EggbotWireless/src/main.cpp

@@ -4,11 +4,11 @@
 #include "StepperTimer.h"
 
 #define STEPS_PER_REVOLUTION 4076
-#define BACKLASH_STEPS 40
+#define BACKLASH_STEPS 20
-#define RPM 6
+#define RPM 10
 
-Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION, BACKLASH_STEPS);
+Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION, BACKLASH_STEPS, 0);
-Stepper servoStepper(D5, D6, D7, D8, STEPS_PER_REVOLUTION, BACKLASH_STEPS);
+Stepper servoStepper(D5, D6, D7, D8, STEPS_PER_REVOLUTION, BACKLASH_STEPS, 80);
 Pen pen(D0, 180, 80);
 
 String inString;
@@ -22,10 +22,17 @@ void setup() {
 
 void loop() {
     while (Serial.available() > 0) {
-        int inChar = Serial.read();
+        char inChar = Serial.read();
         Serial.write(inChar);
+        inString += inChar;
 
         if (inChar == '\n') {
+            inString.trim();
+            if (inString == "pos") {
+                Serial.println("Pos: " + String(servoStepper.getPos()));
+            } else {
+                servoStepper.rotate(inString.toFloat());
+            }
             inString = "";
         }
     }