From 8717e6fe66464d967464312501d4411bf0dd11bc Mon Sep 17 00:00:00 2001
From: Stepan Usatiuk <stepanusatiuk@gmail.com>
Date: Thu, 9 May 2019 23:24:24 +0300
Subject: [PATCH] use floats to parse gcode

Signed-off-by: Stepan Usatyuk <usaatyuk@ustk.me>
---
 Firmware/EggbotWireless/include/EggConstants.h | 13 +++++++++++++
 Firmware/EggbotWireless/include/GCodeParser.h  |  2 +-
 Firmware/EggbotWireless/include/Stepper.h      |  6 ++++--
 Firmware/EggbotWireless/src/GCodeParser.cpp    |  6 +++---
 Firmware/EggbotWireless/src/Stepper.cpp        | 13 ++++++++-----
 Firmware/EggbotWireless/src/main.cpp           |  9 +++++----
 6 files changed, 34 insertions(+), 15 deletions(-)
 create mode 100644 Firmware/EggbotWireless/include/EggConstants.h

diff --git a/Firmware/EggbotWireless/include/EggConstants.h b/Firmware/EggbotWireless/include/EggConstants.h
new file mode 100644
index 0000000..569e5b0
--- /dev/null
+++ b/Firmware/EggbotWireless/include/EggConstants.h
@@ -0,0 +1,13 @@
+#ifndef EGG_H
+#define EGG_H
+
+#define PI 3.14
+
+#define EGG_DIA 35
+#define Y_DEGREES_PER_MM 360 / (PI * EGG_DIA)
+
+#define X_LIMIT 70
+#define EGG_LENGTH 50
+#define X_DEGREES_PER_MM X_LIMIT / EGG_LENGTH
+
+#endif
\ No newline at end of file
diff --git a/Firmware/EggbotWireless/include/GCodeParser.h b/Firmware/EggbotWireless/include/GCodeParser.h
index f236c8b..acbc46e 100644
--- a/Firmware/EggbotWireless/include/GCodeParser.h
+++ b/Firmware/EggbotWireless/include/GCodeParser.h
@@ -15,6 +15,6 @@ enum bcAxis {
     Z = 3,
 };
 
-int *parseGCode(String gcode);
+float *parseGCode(String gcode);
 
 #endif
\ No newline at end of file
diff --git a/Firmware/EggbotWireless/include/Stepper.h b/Firmware/EggbotWireless/include/Stepper.h
index 6cfb31d..e7aaacc 100644
--- a/Firmware/EggbotWireless/include/Stepper.h
+++ b/Firmware/EggbotWireless/include/Stepper.h
@@ -11,6 +11,7 @@ class Stepper {
     int curStep = 1;
     int remainingSteps;
     int backlashSteps;
+    float degreesPerMM;
     bool direction;
     void clockwise();
     void counterClockwise();
@@ -20,9 +21,10 @@ class Stepper {
     int limit;
 
    public:
-    Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution, int backlashSteps, int limit);
+    Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution, int backlashSteps, int limit, float degreesPerMM);
     void rotate(float degrees);
-    void moveTo(float degrees);
+    void rotateTo(float degrees);
+    void moveTo(float dist);
     void setPos(float degrees);
     float getDist(float degrees);
     void doStep();
diff --git a/Firmware/EggbotWireless/src/GCodeParser.cpp b/Firmware/EggbotWireless/src/GCodeParser.cpp
index 9fb6948..e0d7130 100644
--- a/Firmware/EggbotWireless/src/GCodeParser.cpp
+++ b/Firmware/EggbotWireless/src/GCodeParser.cpp
@@ -1,9 +1,9 @@
 #include "GCodeParser.h"
 #include <Arduino.h>
 
-int bytecode[4] = {-1, -1, -1, -1};
+float bytecode[4] = {-1, -1, -1, -1};
 
-int* parseGCode(String gcode) {
+float* parseGCode(String gcode) {
     char commandStringIn[50];
     char commandString[50];
 
@@ -22,7 +22,7 @@ int* parseGCode(String gcode) {
     char args[45];
     strncpy(args, &commandString[4], 45);
 
-    if (strcmp(command, "G01") == 0) {
+    if (strcmp(command, "G01") == 0 || strcmp(command, "G00") == 0) {
         bytecode[0] = G01;
 
         char split_args[3][40];
diff --git a/Firmware/EggbotWireless/src/Stepper.cpp b/Firmware/EggbotWireless/src/Stepper.cpp
index 97dce6e..145237b 100644
--- a/Firmware/EggbotWireless/src/Stepper.cpp
+++ b/Firmware/EggbotWireless/src/Stepper.cpp
@@ -116,14 +116,15 @@ void Stepper::counterClockwise() {
 }
 
 Stepper::Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution,
-                 int backlashSteps, int limit)
+                 int backlashSteps, int limit, float degreesPerMM)
     : pin1(pin1),
       pin2(pin2),
       pin3(pin3),
       pin4(pin4),
       stepsPerRevolution(stepsPerRevolution),
       backlashSteps(backlashSteps),
-      limit(limit) {
+      limit(limit),
+      degreesPerMM(degreesPerMM) {
     pinMode(pin1, OUTPUT);
     pinMode(pin2, OUTPUT);
     pinMode(pin3, OUTPUT);
@@ -191,14 +192,16 @@ float Stepper::getDist(float degrees) {
     return abs(mod - pos);
 }
 
-void Stepper::moveTo(float degrees) {
+void Stepper::rotateTo(float degrees) {
     float mod = fmod(degrees, 360);
     rotate(mod - pos);
 }
 
-void Stepper::setPos(float degrees) {
-    pos = degrees;
+void Stepper::moveTo(float dist) {
+    rotateTo(dist * degreesPerMM);
 }
 
+void Stepper::setPos(float degrees) { pos = degrees; }
+
 bool Stepper::finished() { return remainingSteps == 0; }
 float Stepper::getPos() { return pos; }
\ No newline at end of file
diff --git a/Firmware/EggbotWireless/src/main.cpp b/Firmware/EggbotWireless/src/main.cpp
index a8dc880..add8ffb 100644
--- a/Firmware/EggbotWireless/src/main.cpp
+++ b/Firmware/EggbotWireless/src/main.cpp
@@ -4,13 +4,14 @@
 #include "Pen.h"
 #include "Stepper.h"
 #include "StepperTimer.h"
+#include "EggConstants.h"
 
 #define STEPS_PER_REVOLUTION 4076
-#define BACKLASH_STEPS 40
+
 int RPM = 4;
 
-Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION, BACKLASH_STEPS, 0);
-Stepper servoStepper(D5, D6, D7, D8, -STEPS_PER_REVOLUTION, BACKLASH_STEPS, 70);
+Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION, 10, 0, Y_DEGREES_PER_MM);
+Stepper servoStepper(D5, D6, D7, D8, -STEPS_PER_REVOLUTION, 10, X_LIMIT, X_DEGREES_PER_MM);
 Pen pen(D0, 100, 170);
 
 StepperTimer eggStepperTimer(RPM, STEPS_PER_REVOLUTION);
@@ -19,7 +20,7 @@ StepperTimer servoStepperTimer(RPM, STEPS_PER_REVOLUTION);
 String inString;
 
 
-void execCommand(int *command) {
+void execCommand(float *command) {
     if (command[0] == G01) {
         if (command[X] != -1 && command[Y] != -1) {
             float distX = servoStepper.getDist(command[X]);