use atmega32 to control stepper motors

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

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

@@ -0,0 +1,22 @@
+#include <Servo.h>
+
+#ifndef PEN_H
+#define PEN_H
+
+class Pen {
+   private:
+    int posEngaged;
+    int posDisengaged;
+    bool engaged = true;
+    Servo servo;
+
+   public:
+    Pen(int pin, int posEngaged, int posDisengaged);
+    void engage();
+    void disengage();
+    void init();
+    bool getEngaged();
+    ~Pen();
+};
+
+#endif

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

@@ -0,0 +1,39 @@
+#ifndef STEPPER_H
+#define STEPPER_H
+
+class Stepper {
+   private:
+    int stepsPerRevolution;
+    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();
+    float pos = 0;
+    int limit;
+
+   public:
+    Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution,
+            int limit, float degreesPerMM);
+    void step(int steps);
+    int getRemainingSteps();
+    void rotate(float degrees);
+    void rotateTo(float degrees);
+    void moveTo(float dist);
+    void setPos(float degrees);
+    float getDist(float degrees);
+    int degreesToSteps(float degrees);
+    int mmToSteps(float dist);
+    void doStep();
+    bool finished();
+    float getPos();
+    float getPosMm();
+    float getDistMm(float pos);
+};
+
+#endif

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

@@ -0,0 +1,40 @@
+#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);
+}
+
+void Pen::engage() {
+    if (!engaged) {
+        for (int i = posDisengaged; i > posEngaged; i--) {
+            servo.write(i);
+            delay(DELAY);
+        }
+    }
+    engaged = true;
+}
+
+void Pen::disengage() {
+    if (engaged) {
+        for (int i = posEngaged; i < posDisengaged; i++) {
+            servo.write(i);
+            delay(DELAY);
+        }
+    }
+    engaged = false;
+}
+
+void Pen::init() {
+    servo.write(posDisengaged);
+    engaged = false;
+}
+
+bool Pen::getEngaged() { return engaged; }
+
+Pen::~Pen() {}

Firmware/MotorControl/src/Stepper.cpp

@@ -0,0 +1,208 @@
+#include "Stepper.h"
+#include <Arduino.h>
+
+void Stepper::clockwise() {
+    switch (curStep) {
+        case 1:
+            digitalWrite(pin4, HIGH);
+            digitalWrite(pin3, LOW);
+            digitalWrite(pin2, LOW);
+            digitalWrite(pin1, LOW);
+            break;
+        case 2:
+            digitalWrite(pin4, HIGH);
+            digitalWrite(pin3, HIGH);
+            digitalWrite(pin2, LOW);
+            digitalWrite(pin1, LOW);
+            break;
+        case 3:
+            digitalWrite(pin4, LOW);
+            digitalWrite(pin3, HIGH);
+            digitalWrite(pin2, LOW);
+            digitalWrite(pin1, LOW);
+            break;
+        case 4:
+            digitalWrite(pin4, LOW);
+            digitalWrite(pin3, HIGH);
+            digitalWrite(pin2, HIGH);
+            digitalWrite(pin1, LOW);
+            break;
+        case 5:
+            digitalWrite(pin4, LOW);
+            digitalWrite(pin3, LOW);
+            digitalWrite(pin2, HIGH);
+            digitalWrite(pin1, LOW);
+            break;
+        case 6:
+            digitalWrite(pin4, LOW);
+            digitalWrite(pin3, LOW);
+            digitalWrite(pin2, HIGH);
+            digitalWrite(pin1, HIGH);
+            break;
+        case 7:
+            digitalWrite(pin4, LOW);
+            digitalWrite(pin3, LOW);
+            digitalWrite(pin2, LOW);
+            digitalWrite(pin1, HIGH);
+            break;
+        case 8:
+            digitalWrite(pin4, HIGH);
+            digitalWrite(pin3, LOW);
+            digitalWrite(pin2, LOW);
+            digitalWrite(pin1, HIGH);
+    }
+    if (curStep == 8) {
+        curStep = 1;
+    } else {
+        curStep++;
+    }
+}
+
+void Stepper::counterClockwise() {
+    switch (curStep) {
+        case 1:
+            digitalWrite(pin1, HIGH);
+            digitalWrite(pin2, LOW);
+            digitalWrite(pin3, LOW);
+            digitalWrite(pin4, LOW);
+            break;
+        case 2:
+            digitalWrite(pin1, HIGH);
+            digitalWrite(pin2, HIGH);
+            digitalWrite(pin3, LOW);
+            digitalWrite(pin4, LOW);
+            break;
+        case 3:
+            digitalWrite(pin1, LOW);
+            digitalWrite(pin2, HIGH);
+            digitalWrite(pin3, LOW);
+            digitalWrite(pin4, LOW);
+            break;
+        case 4:
+            digitalWrite(pin1, LOW);
+            digitalWrite(pin2, HIGH);
+            digitalWrite(pin3, HIGH);
+            digitalWrite(pin4, LOW);
+            break;
+        case 5:
+            digitalWrite(pin1, LOW);
+            digitalWrite(pin2, LOW);
+            digitalWrite(pin3, HIGH);
+            digitalWrite(pin4, LOW);
+            break;
+        case 6:
+            digitalWrite(pin1, LOW);
+            digitalWrite(pin2, LOW);
+            digitalWrite(pin3, HIGH);
+            digitalWrite(pin4, HIGH);
+            break;
+        case 7:
+            digitalWrite(pin1, LOW);
+            digitalWrite(pin2, LOW);
+            digitalWrite(pin3, LOW);
+            digitalWrite(pin4, HIGH);
+            break;
+        case 8:
+            digitalWrite(pin1, HIGH);
+            digitalWrite(pin2, LOW);
+            digitalWrite(pin3, LOW);
+            digitalWrite(pin4, HIGH);
+    }
+    if (curStep == 8) {
+        curStep = 1;
+    } else {
+        curStep++;
+    }
+}
+
+Stepper::Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution,
+                 int limit, float degreesPerMM)
+    : pin1(pin1),
+      pin2(pin2),
+      pin3(pin3),
+      pin4(pin4),
+      stepsPerRevolution(stepsPerRevolution),
+      limit(limit),
+      degreesPerMM(degreesPerMM) {
+    pinMode(pin1, OUTPUT);
+    pinMode(pin2, OUTPUT);
+    pinMode(pin3, OUTPUT);
+    pinMode(pin4, OUTPUT);
+}
+
+void Stepper::doStep() {
+    if (remainingSteps > 0) {
+        if (direction) {
+            clockwise();
+        } else {
+            counterClockwise();
+        }
+        remainingSteps--;
+    }
+}
+
+void Stepper::step(int steps) {
+    if (steps == 0) {
+        return;
+    }
+    if (steps > 0) {
+        direction = true;
+        remainingSteps += steps;
+    } else {
+        direction = false;
+        remainingSteps += abs(steps);
+    }
+}
+
+int Stepper::degreesToSteps(float degrees) {
+    return (degrees * stepsPerRevolution) / 360;
+}
+
+void Stepper::rotate(float degrees) {
+    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;
+        }
+    }
+}
+
+float Stepper::getDist(float degrees) {
+    float mod = fmod(degrees, 360);
+    return abs(mod - pos);
+}
+
+void Stepper::rotateTo(float degrees) {
+    float mod = fmod(degrees, 360);
+    rotate(mod - pos);
+}
+
+void Stepper::moveTo(float dist) { rotateTo(dist * degreesPerMM); }
+
+int Stepper::mmToSteps(float dist) {
+    return abs(degreesToSteps(dist * degreesPerMM));
+}
+
+float Stepper::getPosMm() { return pos / degreesPerMM; }
+
+float Stepper::getDistMm(float pos) { return abs(getPosMm() - pos); }
+
+void Stepper::setPos(float degrees) { pos = degrees; }
+
+int Stepper::getRemainingSteps() { return remainingSteps; }
+
+bool Stepper::finished() { return remainingSteps == 0; }
+float Stepper::getPos() { return pos; }

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