basic gcode parsing

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

Firmware/EggbotWireless/include/GCodeParser.h

@@ -0,0 +1,20 @@
+#include <Arduino.h>
+
+#ifndef PARSER_H
+#define PARSER_H
+
+enum command {
+    unk,
+    G01,
+    M99,
+};
+
+enum bcAxis {
+    X = 1,
+    Y = 2,
+    Z = 3,
+};
+
+int *parseGCode(String gcode);
+
+#endif

Firmware/EggbotWireless/include/Pen.h

@@ -3,16 +3,20 @@
 #ifndef PEN_H
 #define PEN_H
 
+#define DELAY 300
+
 class Pen {
    private:
     int posEngaged;
     int posDisengaged;
+    bool engaged;
     Servo servo;
 
    public:
     Pen(int pin, int posEngaged, int posDisengaged);
     void engage();
     void disengage();
+    bool getEngaged();
     ~Pen();
 };
 

Firmware/EggbotWireless/include/Stepper.h

@@ -22,6 +22,8 @@ class Stepper {
    public:
     Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution, int backlashSteps, int limit);
     void rotate(float degrees);
+    void moveTo(float degrees);
+    float getDist(float degrees);
     void doStep();
     bool finished();
     float getPos();

Firmware/EggbotWireless/include/StepperTimer.h

@@ -3,9 +3,17 @@
 
 #ifndef STEPPER_TIMER_H
 #define STEPPER_TIMER_H
-
-void stepperTimerInit(int rpm, int stepsPerRevolution);
-void stepperTimerSetStepper(int num, Stepper *stepper);
-void stepperTimerTick();
+class StepperTimer {
+   private:
+    int speedDelay;
+    int stepsPerRevolution;
+    Ticker stepperTicker;
+    Stepper *stepper;
+   public:
+    StepperTimer(float rpm, int stepsPerRevolution);
+    void setStepper(Stepper *stepper);
+    void setRPM(float rpm);
+    void tick();
+};
 
 #endif

Firmware/EggbotWireless/src/GCodeParser.cpp

@@ -0,0 +1,74 @@
+#include "GCodeParser.h"
+#include <Arduino.h>
+
+int bytecode[4] = {-1, -1, -1, -1};
+
+int* parseGCode(String gcode) {
+    char commandStringIn[30];
+    char commandString[26];
+
+    gcode.toCharArray(commandStringIn, 30);
+
+    // Convert command to uppercase
+    for (int i = 0; commandStringIn[i] != '\0'; i++) {
+        commandString[i] = toupper(commandStringIn[i]);
+        commandString[i + 1] = '\0';
+    }
+
+    char command[4];
+    strncpy(command, commandString, 3);
+    command[3] = '\0';
+
+    char args[22];
+    strncpy(args, &commandString[4], 22);
+
+    if (strcmp(command, "G01") == 0) {
+        bytecode[0] = G01;
+
+        char split_args[3][22];
+        memset(split_args, 0, sizeof(split_args));
+        char* arg;
+        int argc = 0;
+        arg = strtok(args, " ");
+
+        // Put every command separated by spaces into the buffer and count them
+        for (int i = 0; arg != NULL; i++) {
+            strcpy(split_args[i], arg);
+            arg = strtok(NULL, " ");
+            argc++;
+        }
+
+        // Iterate through arguments
+        for (int i = 0; i < argc; i++) {
+            arg = split_args[i];
+            char axis[2];
+            char value[7];
+
+            strncpy(axis, arg, 1);
+            axis[1] = '\0';
+
+            strncpy(value, &arg[1], 7);
+
+            float floatValue;
+            floatValue = atof(value);
+
+            if (strcmp(axis, "X") == 0) {
+                bytecode[X] = floatValue;
+            } else if (strcmp(axis, "Y") == 0) {
+                bytecode[Y] = floatValue;
+            } else if (strcmp(axis, "Z") == 0) {
+                bytecode[Z] = floatValue;
+            }
+        }
+
+        return bytecode;
+    }
+
+    if (strcmp(command, "M99") == 0) {
+        bytecode[0] = M99;
+        return bytecode;
+    }
+
+    bytecode[0] = unk;
+    return bytecode;
+}

Firmware/EggbotWireless/src/Pen.cpp

@@ -1,13 +1,25 @@
-#include <Servo.h>
 #include "Pen.h"
+#include <Servo.h>
 
 Pen::Pen(int pin, int posEngaged, int posDisengaged)
     : posEngaged(posEngaged), posDisengaged(posDisengaged) {
     servo.attach(pin);
 }
 
-void Pen::engage() { servo.write(posEngaged); }
+void Pen::engage() {
+    servo.write(posEngaged);
+    engaged = true;
+    delay(DELAY);
+}
 
-void Pen::disengage() { servo.write(posDisengaged); }
+void Pen::disengage() {
+    servo.write(posDisengaged);
+    engaged = false;
+    delay(DELAY);
+}
+
+bool Pen::getEngaged() {
+    return engaged;
+}
 
 Pen::~Pen() {}

Firmware/EggbotWireless/src/Stepper.cpp

@@ -186,5 +186,15 @@ void Stepper::rotate(float degrees) {
     }
 }
 
+float Stepper::getDist(float degrees) {
+    float mod = fmod(degrees, 360);
+    return abs(mod - pos);
+}
+
+void Stepper::moveTo(float degrees) {
+    float mod = fmod(degrees, 360);
+    rotate(mod - pos);
+}
+
 bool Stepper::finished() { return remainingSteps == 0; }
 float Stepper::getPos() { return pos; }

Firmware/EggbotWireless/src/StepperTimer.cpp

@@ -1,22 +1,19 @@
 #include "StepperTimer.h"
 #include "Stepper.h"
 
-int speedDelay;
-Ticker stepperTicker;
-Stepper *steppers[2];
-
-void stepperTimerInit(int rpm, int stepsPerRevolution)
-{
+StepperTimer::StepperTimer(float rpm, int stepsPerRevolution)
+    : stepsPerRevolution(stepsPerRevolution) {
+    stepperTicker.detach();
     speedDelay = 60 * 1000 / (rpm * stepsPerRevolution);
-    stepperTicker.attach_ms(speedDelay, stepperTimerTick);
+    stepperTicker.attach_ms(speedDelay, std::bind(&StepperTimer::tick, this));
 }
 
-void stepperTimerSetStepper(int num, Stepper *stepper) {
-    steppers[num] = stepper;
-}
+void StepperTimer::setStepper(Stepper *_stepper) { stepper = _stepper; }
 
-void stepperTimerTick() {
-    for (int i = 0; i < 2; i++) {
-        steppers[i]->doStep();
-    } 
+void StepperTimer::tick() { stepper->doStep(); }
+
+void StepperTimer::setRPM(float rpm) {
+    stepperTicker.detach();
+    speedDelay = 60 * 1000 / (rpm * stepsPerRevolution);
+    stepperTicker.attach_ms(speedDelay, std::bind(&StepperTimer::tick, this));
 }

Firmware/EggbotWireless/src/main.cpp

@@ -1,23 +1,72 @@
 #include <Arduino.h>
+#include <ESP8266WiFi.h>
+#include "GCodeParser.h"
 #include "Pen.h"
 #include "Stepper.h"
 #include "StepperTimer.h"
 
 #define STEPS_PER_REVOLUTION 4076
 #define BACKLASH_STEPS 20
-#define RPM 10
+int RPM = 10;
 
 Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION, BACKLASH_STEPS, 0);
 Stepper servoStepper(D5, D6, D7, D8, STEPS_PER_REVOLUTION, BACKLASH_STEPS, 80);
-Pen pen(D0, 180, 80);
+Pen pen(D0, 80, 170);
+
+StepperTimer eggStepperTimer(RPM, STEPS_PER_REVOLUTION);
+StepperTimer servoStepperTimer(RPM, STEPS_PER_REVOLUTION);
 
 String inString;
 
+
+void execCommand(int *command) {
+    if (command[0] == G01) {
+        if (command[X] != -1 && command[Y] != -1) {
+            float distX = servoStepper.getDist(command[X]);
+            float distY = eggStepper.getDist(command[Y]);
+            if (distX != 0 && distY != 0) {
+                if (distX > distY) {
+                    eggStepperTimer.setRPM(RPM * distY / distX);
+                } else if (distY > distX) {
+                    servoStepperTimer.setRPM(RPM * distX / distY);
+                }
+            }
+        }
+
+        if (command[X] != -1) {
+            servoStepper.moveTo(command[X]);
+        }
+
+        if (command[Y] != -1) {
+            eggStepper.moveTo(command[Y]);
+        }
+
+        if (command[Z] == 1) {
+            pen.engage();
+        }
+        if (command[Z] == 0) {
+            pen.disengage();
+        }
+
+        return;
+    }
+
+    if (command[0] == M99) {
+        Serial.println("Status:");
+        Serial.println("X: " + String(servoStepper.getPos()));
+        Serial.println("Y: " + String(eggStepper.getPos()));
+        Serial.println("PEN: " + String(pen.getEngaged()));
+
+        return;
+    }
+}
+
 void setup() {
     Serial.begin(115200);
-    stepperTimerSetStepper(0, &eggStepper);
-    stepperTimerSetStepper(1, &servoStepper);
-    stepperTimerInit(RPM, STEPS_PER_REVOLUTION);
+    eggStepperTimer.setStepper(&eggStepper);
+    servoStepperTimer.setStepper(&servoStepper);
+    pen.disengage();
+    WiFi.mode(WIFI_OFF);
 }
 
 void loop() {
@@ -28,11 +77,15 @@ void loop() {
 
         if (inChar == '\n') {
             inString.trim();
-            if (inString == "pos") {
-                Serial.println("Pos: " + String(servoStepper.getPos()));
-            } else {
-                servoStepper.rotate(inString.toFloat());
+
+            execCommand(parseGCode(inString));
+
+            while (!eggStepper.finished() || !servoStepper.finished()) {
+                delay(100);
             }
+            eggStepperTimer.setRPM(RPM);
+            servoStepperTimer.setRPM(RPM);
+            Serial.println("OK");
             inString = "";
         }
     }