use atmega32 to control stepper motors

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

Firmware/EggbotWireless/include/EggConstants.h

@@ -1,6 +0,0 @@
-#define EGG_DIA 35.0
-#define Y_DEGREES_PER_MM (360 / (PI * EGG_DIA))
-
-#define X_LIMIT 70.0
-#define EGG_LENGTH 50.0
-#define X_DEGREES_PER_MM (X_LIMIT / EGG_LENGTH)

Firmware/EggbotWireless/include/GCodeParser.h

@@ -1,21 +1,9 @@
 #include <Arduino.h>
+#include "common/Commands.h"
 
 #ifndef PARSER_H
 #define PARSER_H
 
-enum command {
-    unk,
-    G00,
-    G01,
-    M99,
-};
-
-enum bcAxis {
-    X = 1,
-    Y = 2,
-    Z = 3,
-};
-
 float *parseGCode(String gcode);
 
 #endif

Firmware/EggbotWireless/include/Globals.h

@@ -1,25 +0,0 @@
-#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, 0, X_LIMIT,
-                     X_DEGREES_PER_MM, typeClockwise, false);
-
-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/StepperTimer.h

@@ -1,19 +0,0 @@
-#include <Ticker.h>
-#include "Stepper.h"
-
-#ifndef STEPPER_TIMER_H
-#define STEPPER_TIMER_H
-class StepperTimer {
-   private:
-    float 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/include/common

@@ -0,0 +1 @@
+../../common

Firmware/EggbotWireless/src/GCodeParser.cpp

@@ -4,6 +4,10 @@
 float bytecode[4] = {-1, -1, -1, -1};
 
 float* parseGCode(String gcode) {
+    for (int i = 0; i < 4; i++) {
+        bytecode[i] = -1.0F;
+    }
+
     char commandStringIn[50];
     char commandString[50];
 

Firmware/EggbotWireless/src/StepperTimer.cpp

@@ -1,19 +0,0 @@
-#include "StepperTimer.h"
-#include "Stepper.h"
-
-StepperTimer::StepperTimer(float rpm, int stepsPerRevolution)
-    : stepsPerRevolution(stepsPerRevolution) {
-    stepperTicker.detach();
-    speedDelay = 60 / (rpm * stepsPerRevolution);
-    stepperTicker.attach(speedDelay, std::bind(&StepperTimer::tick, this));
-}
-
-void StepperTimer::setStepper(Stepper *_stepper) { stepper = _stepper; }
-
-void StepperTimer::tick() { stepper->doStep(); }
-
-void StepperTimer::setRPM(float rpm) {
-    stepperTicker.detach();
-    speedDelay = 60 / (rpm * stepsPerRevolution);
-    stepperTicker.attach(speedDelay, std::bind(&StepperTimer::tick, this));
-}

Firmware/EggbotWireless/src/commonSrc

@@ -0,0 +1 @@
+../../commonSrc

Firmware/EggbotWireless/src/main.cpp

@@ -1,116 +1,87 @@
 #include <Arduino.h>
 #include <ESP8266WiFi.h>
 #include <Ticker.h>
-
-#include "EggConstants.h"
+#include <Wire.h>
 #include "GCodeParser.h"
-#include "Globals.h"
-#include "Pen.h"
-#include "Stepper.h"
-#include "StepperTimer.h"
+#include "common/Commands.h"
 
-int RPM = DEF_RPM;
+String inString;
+bool waitingForNext;
 
-bool needAdjust = false;
-
-Ticker adjustTicker;
-
-void execCommand(float *command) {
-    if (command[0] == G01 || command[0] == G00) {
-        if (command[0] == G01) {
-            needAdjust = true;
-            if (command[X] != -1 && command[Y] != -1) {
-                float distX = servoStepper.getDistMm(command[X]);
-                float distY = eggStepper.getDistMm(command[Y]);
-                int stepsX = servoStepper.mmToSteps(distX);
-                int stepsY = eggStepper.mmToSteps(distY);
-                if (stepsX != 0 && stepsY != 0) {
-                    if (stepsX > stepsY) {
-                        float rpm = (float)RPM * (float)stepsY / (float)stepsX;
-                        eggStepperTimer.setRPM(rpm);
-                    } else if (stepsY > stepsX) {
-                        float rpm = (float)RPM * (float)stepsX / (float)stepsY;
-                        servoStepperTimer.setRPM(rpm);
-                    }
-                }
+void sendCommand(float *command) {
+    if (command[0] != unk) {
+        Wire.beginTransmission(8);
+        byte txBuffer[4];
+        for (int i = 0; i < 4; i++) {
+            floatToBytes(txBuffer, command[i]);
+            Wire.write(txBuffer, 4);
         }
+        Wire.endTransmission();
     } else {
-            needAdjust = false;
-        }
-
-        if (command[X] != -1) {
-            servoStepper.moveTo(command[X]);
-        }
-
-        if (command[Y] != -1) {
-            eggStepper.moveTo(command[Y]);
-        }
-
-        if (command[Z] < 0) {
-            pen.engage();
-        }
-        if (command[Z] >= 0) {
-            pen.disengage();
+        Serial.println("UNK");
     }
 
+    if (command[0] == G01 || command[0] == G00) {
+        Wire.requestFrom(8, 1);
+        waitingForNext = true;
         return;
     }
 
     if (command[0] == M99) {
+        Wire.requestFrom(8, 5 * sizeof(float));
+        float resp[5];
+
+        byte buffer[4];
+
+        for (int i = 0; i < 5; i++) {
+            for (int j = 0; j < 4; j++) {
+                while (!Wire.available()) {
+                }
+                char read = Wire.read();
+                buffer[j] = read;
+            }
+            bytesToFloat(&resp[i], buffer);
+        }
+
         Serial.println("Status:");
-        Serial.println("X: " + String(servoStepper.getPos()));
-        Serial.println("Y: " + String(eggStepper.getPos()));
-        Serial.println("Xmm: " + String(servoStepper.getPosMm()));
-        Serial.println("Ymm: " + String(eggStepper.getPosMm()));
-        Serial.println("PEN: " + String(pen.getEngaged()));
+        Serial.println("X: " + String(resp[servoRot]));
+        Serial.println("Y: " + String(resp[eggRot]));
+        Serial.println("Xmm: " + String(resp[servoPos]));
+        Serial.println("Ymm: " + String(resp[eggPos]));
+        Serial.println("PEN: " + String(resp[penPos]));
 
         return;
     }
 }
 
-void adjustRPM() {
-    if (needAdjust) {
-        cli();
-        int stepsX = servoStepper.getRemainingSteps();
-        int stepsY = eggStepper.getRemainingSteps();
-        if (stepsX > stepsY) {
-            float rpm = (float)RPM * (float)stepsY / (float)stepsX;
-            eggStepperTimer.setRPM(rpm);
-        } else if (stepsY > stepsX) {
-            float rpm = (float)RPM * (float)stepsX / (float)stepsY;
-            servoStepperTimer.setRPM(rpm);
-        } else {
-            eggStepperTimer.setRPM(RPM);
-            servoStepperTimer.setRPM(RPM);
-        }
-        sei();
-    }
-}
-
 void setup() {
-    WiFi.mode(WIFI_OFF);
     Serial.begin(115200);
-    eggStepperTimer.setStepper(&eggStepper);
-    servoStepperTimer.setStepper(&servoStepper);
-    pen.init();
-    servoStepper.setPos(70);
-    adjustTicker.attach_ms(50, adjustRPM);
+    Wire.begin(D1, D2);
 }
 
 void loop() {
     while (Serial.available() > 0) {
         char inChar = Serial.read();
         inString += inChar;
+        Serial.write(inChar);
 
         if (inChar == '\n') {
             inString.trim();
-            execCommand(parseGCode(inString));
-            while (!eggStepper.finished() || !servoStepper.finished()) {
-                delay(50);
+            sendCommand(parseGCode(inString));
+            while (waitingForNext) {
+                while (!Wire.available()) {
                 }
-            eggStepperTimer.setRPM(RPM);
-            servoStepperTimer.setRPM(RPM);
+                int response = Wire.read();
+                if (response == WAIT) {
+                    delay(1);
+                    Wire.requestFrom(8, 1);
+                } else if (response == NEXT) {
                     Serial.println("OK");
+                    waitingForNext = false;
+                } else {
+                    Serial.println("Error");
+                }
+            }
             inString = "";
         }
     }

Firmware/MotorControl/.clang-format

@@ -0,0 +1,6 @@
+---
+Language:        Cpp
+BasedOnStyle:  Google
+IndentWidth: 4
+...
+

Firmware/MotorControl/.gitignore

@@ -0,0 +1,9 @@
+.pio
+.pioenvs
+.piolibdeps
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.history
+.vscode
+.directory

Firmware/MotorControl/.travis.yml

@@ -0,0 +1,67 @@
+# Continuous Integration (CI) is the practice, in software
+# engineering, of merging all developer working copies with a shared mainline
+# several times a day < https://docs.platformio.org/page/ci/index.html >
+#
+# Documentation:
+#
+# * Travis CI Embedded Builds with PlatformIO
+#   < https://docs.travis-ci.com/user/integration/platformio/ >
+#
+# * PlatformIO integration with Travis CI
+#   < https://docs.platformio.org/page/ci/travis.html >
+#
+# * User Guide for `platformio ci` command
+#   < https://docs.platformio.org/page/userguide/cmd_ci.html >
+#
+#
+# Please choose one of the following templates (proposed below) and uncomment
+# it (remove "# " before each line) or use own configuration according to the
+# Travis CI documentation (see above).
+#
+
+
+#
+# Template #1: General project. Test it using existing `platformio.ini`.
+#
+
+# language: python
+# python:
+#     - "2.7"
+#
+# sudo: false
+# cache:
+#     directories:
+#         - "~/.platformio"
+#
+# install:
+#     - pip install -U platformio
+#     - platformio update
+#
+# script:
+#     - platformio run
+
+
+#
+# Template #2: The project is intended to be used as a library with examples.
+#
+
+# language: python
+# python:
+#     - "2.7"
+#
+# sudo: false
+# cache:
+#     directories:
+#         - "~/.platformio"
+#
+# env:
+#     - PLATFORMIO_CI_SRC=path/to/test/file.c
+#     - PLATFORMIO_CI_SRC=examples/file.ino
+#     - PLATFORMIO_CI_SRC=path/to/test/directory
+#
+# install:
+#     - pip install -U platformio
+#     - platformio update
+#
+# script:
+#     - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N

Firmware/MotorControl/include/Globals.h

@@ -0,0 +1,33 @@
+#include <Arduino.h>
+#include "Pen.h"
+#include "Stepper.h"
+
+#ifndef GLOBALS_H
+#define GLOBALS_H
+
+#define EGG_DIA 35.0
+#define Y_DEGREES_PER_MM (360 / (PI * EGG_DIA))
+
+#define X_LIMIT 70.0
+#define EGG_LENGTH 50.0
+#define X_DEGREES_PER_MM (X_LIMIT / EGG_LENGTH)
+
+#define STEPS_PER_REVOLUTION 4076
+#define DEF_RPM 2
+
+Stepper eggStepper(28, 29, 30, 31, STEPS_PER_REVOLUTION, 0, Y_DEGREES_PER_MM);
+Stepper servoStepper(24, 25, 26, 27, -STEPS_PER_REVOLUTION, X_LIMIT,
+                     X_DEGREES_PER_MM);
+
+Pen pen(23, 100, 150);
+
+float eggStepperDelay;
+float servoStepperDelay;
+
+float eggStepperRPM;
+float servoStepperRPM;
+
+bool recalculateDelays;
+bool needAdjust;
+
+#endif

Firmware/MotorControl/include/Pen.h

@@ -3,8 +3,6 @@
 #ifndef PEN_H
 #define PEN_H
 
-#define DELAY 15
-
 class Pen {
    private:
     int posEngaged;

Firmware/MotorControl/include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

Firmware/MotorControl/include/Stepper.h

@@ -1,33 +1,26 @@
 #ifndef STEPPER_H
 #define STEPPER_H
 
-enum backlashCompType { typeClockwise, typeCounterClockwise, typeBoth };
-
 class Stepper {
    private:
     int stepsPerRevolution;
-    int pin1;
-    int pin2;
-    int pin3;
-    int pin4;
-    int curStep = 1;
-    int remainingSteps;
-    int backlashSteps;
+    unsigned int pin1;
+    unsigned int pin2;
+    unsigned int pin3;
+    unsigned int pin4;
+    unsigned int curStep = 1;
+    unsigned int remainingSteps;
     float degreesPerMM;
     bool direction;
     void clockwise();
     void counterClockwise();
-    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,
-            backlashCompType backlashComp, bool compAlways);
+            int limit, float degreesPerMM);
+    void step(int steps);
     int getRemainingSteps();
     void rotate(float degrees);
     void rotateTo(float degrees);

Firmware/MotorControl/include/common

@@ -0,0 +1 @@
+../../common

Firmware/MotorControl/lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in a an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

Firmware/MotorControl/platformio.ini

@@ -0,0 +1,27 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:motorcontrol]
+platform = atmelavr
+framework = arduino
+
+board = mightycore32
+board_build.mcu = atmega32
+board_build.f_cpu = 16000000L
+
+build_flags = 
+
+board_upload.speed = 115200
+
+monitor_speed = 9600
+
+upload_protocol = stk500
+upload_flags =
+    -P/dev/ttyUSB1

Firmware/MotorControl/src/Pen.cpp

@@ -1,6 +1,10 @@
-#include "Pen.h"
+#include <Arduino.h>
 #include <Servo.h>
 
+#include "Pen.h"
+
+#define DELAY 15
+
 Pen::Pen(int pin, int posEngaged, int posDisengaged)
     : posEngaged(posEngaged), posDisengaged(posDisengaged) {
     servo.attach(pin);

Firmware/MotorControl/src/Stepper.cpp

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

Firmware/MotorControl/src/commonSrc

@@ -0,0 +1 @@
+../../commonSrc

Firmware/MotorControl/src/main.cpp

@@ -0,0 +1,153 @@
+#include <Arduino.h>
+#include <Servo.h>
+#include <Wire.h>
+#include "Globals.h"
+#include "common/Commands.h"
+
+int curRPM = DEF_RPM;
+int adjustDelay = 10;
+void adjustRPM() {
+    unsigned int stepsX = servoStepper.getRemainingSteps();
+    unsigned int stepsY = eggStepper.getRemainingSteps();
+    if (stepsX != 0 && stepsY != 0) {
+        if (stepsX > stepsY) {
+            float rpm = (float)curRPM * (float)stepsY / (float)stepsX;
+            eggStepperRPM = rpm;
+        } else if (stepsY > stepsX) {
+            float rpm = (float)curRPM * (float)stepsX / (float)stepsY;
+            servoStepperRPM = rpm;
+        } else {
+            eggStepperRPM = curRPM;
+            servoStepperRPM = curRPM;
+        }
+        recalculateDelays = true;
+    }
+}
+
+int curFloat = 0;
+float command[4];
+
+bool newCommand = false;
+bool executing = false;
+
+int curByte = 0;
+byte rxBuffer[4];
+void receiveEvent(int howMany) {
+    while (Wire.available() > 0) {
+        if (!newCommand) {
+            char c = Wire.read();
+            rxBuffer[curByte] = c;
+            curByte++;
+            if (curByte == 4) {
+                curByte = 0;
+                bytesToFloat(&command[curFloat], rxBuffer);
+                curFloat++;
+            }
+            if (curFloat == 4) {
+                curFloat = 0;
+                newCommand = true;
+            }
+        }
+    }
+}
+
+byte txBuffer[5 * sizeof(float)];
+void requestEvent() {
+    if (command[0] == M99) {
+        floatToBytes(&txBuffer[0], servoStepper.getPos());
+        floatToBytes(&txBuffer[sizeof(float)], eggStepper.getPos());
+
+        floatToBytes(&txBuffer[sizeof(float) * 2], servoStepper.getPosMm());
+        floatToBytes(&txBuffer[sizeof(float) * 3], eggStepper.getPosMm());
+
+        floatToBytes(&txBuffer[sizeof(float) * 4], (float)pen.getEngaged());
+        Wire.write(txBuffer, 5 * sizeof(float));
+    } else if (executing || newCommand) {
+        Wire.write(WAIT);
+    } else {
+        Wire.write(NEXT);
+    }
+}
+
+float calculateDelay(float rpm, int stepsPerRevolution) {
+    return ((float)1000 * (float)60) / (rpm * (float)stepsPerRevolution);
+}
+
+Servo servo;
+
+void execCommand(float *command) {
+    if (command[0] == G01 || command[0] == G00) {
+        if (command[0] == G01) {
+            needAdjust = true;
+        } else {
+            needAdjust = false;
+        }
+
+        if (command[X] != -1) {
+            servoStepper.moveTo(command[X]);
+        }
+
+        if (command[Y] != -1) {
+            eggStepper.moveTo(command[Y]);
+        }
+
+        if (command[Z] < 0) {
+            pen.engage();
+        }
+        if (command[Z] >= 0) {
+            pen.disengage();
+        }
+
+        adjustRPM();
+
+        return;
+    }
+}
+
+void setup() {
+    cli();
+    Serial.begin(115200);
+    Wire.begin(8);
+    Wire.onReceive(receiveEvent);
+    Wire.onRequest(requestEvent);
+    Serial.println("Hello!");
+    recalculateDelays = true;
+    eggStepperRPM = servoStepperRPM = 4;
+    OCR2 = 250;
+    TCCR2 |= (1 << WGM12) | (1 << CS22);
+    TIMSK |= (1 << OCIE2);
+    sei();
+    servoStepper.setPos(X_LIMIT);
+    pen.init();
+}
+
+ISR(TIMER2_COMP_vect) {
+    unsigned long ms = millis();
+    if (fmod(ms, eggStepperDelay) < 1) {
+        eggStepper.doStep();
+    }
+    if (fmod(ms, servoStepperDelay) < 1) {
+        servoStepper.doStep();
+    }
+    if (fmod(ms, adjustDelay) < 1) {
+        adjustRPM();
+    }
+}
+
+void loop() {
+    if (eggStepper.getRemainingSteps() == 0 &&
+        servoStepper.getRemainingSteps() == 0) {
+        executing = false;
+    }
+    if (newCommand) {
+        newCommand = false;
+        executing = true;
+        execCommand(command);
+    }
+    if (recalculateDelays) {
+        eggStepperDelay = calculateDelay(eggStepperRPM, STEPS_PER_REVOLUTION);
+        servoStepperDelay =
+            calculateDelay(servoStepperRPM, STEPS_PER_REVOLUTION);
+        recalculateDelays = false;
+    }
+}

Firmware/MotorControl/test/README

@@ -0,0 +1,11 @@
+
+This directory is intended for PIO Unit Testing and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PIO Unit Testing:
+- https://docs.platformio.org/page/plus/unit-testing.html

Firmware/common/Commands.h

@@ -0,0 +1,33 @@
+#ifndef COMMANDS_H
+#define COMMANDS_H
+
+enum command{
+    unk,
+    G00,
+    G01,
+    M99,
+};
+
+enum bcAxis{
+    X = 1,
+    Y = 2,
+    Z = 3,
+};
+
+enum I2CMessage {
+    WAIT,
+    NEXT,
+};
+
+enum StatusMSG {
+    servoRot,
+    eggRot,
+    servoPos,
+    eggPos,
+    penPos,
+};
+
+void bytesToFloat(float *target, byte *val);
+void floatToBytes(byte *target, float val);
+
+#endif

Firmware/commonSrc/Commands.cpp

@@ -0,0 +1,11 @@
+#include <Arduino.h>
+
+void bytesToFloat(float *target, byte *val) {
+    memcpy(target, val, 4);
+    return;
+}
+
+void floatToBytes(byte *target, float val) {
+    memcpy(target, &val, 4);
+    return;
+}