diff --git a/Firmware/EggbotWireless/include/EggConstants.h b/Firmware/EggbotWireless/include/EggConstants.h deleted file mode 100644 index 6ad7e81..0000000 --- a/Firmware/EggbotWireless/include/EggConstants.h +++ /dev/null @@ -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) diff --git a/Firmware/EggbotWireless/include/GCodeParser.h b/Firmware/EggbotWireless/include/GCodeParser.h index 99acb49..a692701 100644 --- a/Firmware/EggbotWireless/include/GCodeParser.h +++ b/Firmware/EggbotWireless/include/GCodeParser.h @@ -1,21 +1,9 @@ #include +#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 \ No newline at end of file diff --git a/Firmware/EggbotWireless/include/Globals.h b/Firmware/EggbotWireless/include/Globals.h deleted file mode 100644 index 9bba13f..0000000 --- a/Firmware/EggbotWireless/include/Globals.h +++ /dev/null @@ -1,25 +0,0 @@ -#include -#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 \ No newline at end of file diff --git a/Firmware/EggbotWireless/include/StepperTimer.h b/Firmware/EggbotWireless/include/StepperTimer.h deleted file mode 100644 index 4f61c96..0000000 --- a/Firmware/EggbotWireless/include/StepperTimer.h +++ /dev/null @@ -1,19 +0,0 @@ -#include -#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 diff --git a/Firmware/EggbotWireless/include/common b/Firmware/EggbotWireless/include/common new file mode 120000 index 0000000..dc879ab --- /dev/null +++ b/Firmware/EggbotWireless/include/common @@ -0,0 +1 @@ +../../common \ No newline at end of file diff --git a/Firmware/EggbotWireless/src/GCodeParser.cpp b/Firmware/EggbotWireless/src/GCodeParser.cpp index f2a44bd..4f8a48a 100644 --- a/Firmware/EggbotWireless/src/GCodeParser.cpp +++ b/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]; diff --git a/Firmware/EggbotWireless/src/StepperTimer.cpp b/Firmware/EggbotWireless/src/StepperTimer.cpp deleted file mode 100644 index 14ec7bb..0000000 --- a/Firmware/EggbotWireless/src/StepperTimer.cpp +++ /dev/null @@ -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)); -} \ No newline at end of file diff --git a/Firmware/EggbotWireless/src/commonSrc b/Firmware/EggbotWireless/src/commonSrc new file mode 120000 index 0000000..4a6da72 --- /dev/null +++ b/Firmware/EggbotWireless/src/commonSrc @@ -0,0 +1 @@ +../../commonSrc \ No newline at end of file diff --git a/Firmware/EggbotWireless/src/main.cpp b/Firmware/EggbotWireless/src/main.cpp index c0ec93a..185b4de 100644 --- a/Firmware/EggbotWireless/src/main.cpp +++ b/Firmware/EggbotWireless/src/main.cpp @@ -1,116 +1,87 @@ #include #include #include - -#include "EggConstants.h" +#include #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; +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 { + Serial.println("UNK"); + } -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); - } - } - } - } 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(); - } - + 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()) { + } + 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"); + } } - eggStepperTimer.setRPM(RPM); - servoStepperTimer.setRPM(RPM); - Serial.println("OK"); inString = ""; } } diff --git a/Firmware/MotorControl/.clang-format b/Firmware/MotorControl/.clang-format new file mode 100644 index 0000000..635c58f --- /dev/null +++ b/Firmware/MotorControl/.clang-format @@ -0,0 +1,6 @@ +--- +Language: Cpp +BasedOnStyle: Google +IndentWidth: 4 +... + diff --git a/Firmware/MotorControl/.gitignore b/Firmware/MotorControl/.gitignore new file mode 100644 index 0000000..a4976f5 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Firmware/MotorControl/.travis.yml b/Firmware/MotorControl/.travis.yml new file mode 100644 index 0000000..7c486f1 --- /dev/null +++ b/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 diff --git a/Firmware/MotorControl/include/Globals.h b/Firmware/MotorControl/include/Globals.h new file mode 100644 index 0000000..555326e --- /dev/null +++ b/Firmware/MotorControl/include/Globals.h @@ -0,0 +1,33 @@ +#include +#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 \ No newline at end of file diff --git a/Firmware/EggbotWireless/include/Pen.h b/Firmware/MotorControl/include/Pen.h similarity index 94% rename from Firmware/EggbotWireless/include/Pen.h rename to Firmware/MotorControl/include/Pen.h index ea7ce62..7f454d2 100644 --- a/Firmware/EggbotWireless/include/Pen.h +++ b/Firmware/MotorControl/include/Pen.h @@ -3,8 +3,6 @@ #ifndef PEN_H #define PEN_H -#define DELAY 15 - class Pen { private: int posEngaged; diff --git a/Firmware/MotorControl/include/README b/Firmware/MotorControl/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/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 diff --git a/Firmware/EggbotWireless/include/Stepper.h b/Firmware/MotorControl/include/Stepper.h similarity index 65% rename from Firmware/EggbotWireless/include/Stepper.h rename to Firmware/MotorControl/include/Stepper.h index d2a371d..d77ae4e 100644 --- a/Firmware/EggbotWireless/include/Stepper.h +++ b/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); diff --git a/Firmware/MotorControl/include/common b/Firmware/MotorControl/include/common new file mode 120000 index 0000000..dc879ab --- /dev/null +++ b/Firmware/MotorControl/include/common @@ -0,0 +1 @@ +../../common \ No newline at end of file diff --git a/Firmware/MotorControl/lib/README b/Firmware/MotorControl/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/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 +#include + +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 diff --git a/Firmware/MotorControl/platformio.ini b/Firmware/MotorControl/platformio.ini new file mode 100644 index 0000000..91bbce6 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Firmware/EggbotWireless/src/Pen.cpp b/Firmware/MotorControl/src/Pen.cpp similarity index 94% rename from Firmware/EggbotWireless/src/Pen.cpp rename to Firmware/MotorControl/src/Pen.cpp index 584eee5..2e7388d 100644 --- a/Firmware/EggbotWireless/src/Pen.cpp +++ b/Firmware/MotorControl/src/Pen.cpp @@ -1,6 +1,10 @@ -#include "Pen.h" +#include #include +#include "Pen.h" + +#define DELAY 15 + Pen::Pen(int pin, int posEngaged, int posDisengaged) : posEngaged(posEngaged), posDisengaged(posDisengaged) { servo.attach(pin); diff --git a/Firmware/EggbotWireless/src/Stepper.cpp b/Firmware/MotorControl/src/Stepper.cpp similarity index 88% rename from Firmware/EggbotWireless/src/Stepper.cpp rename to Firmware/MotorControl/src/Stepper.cpp index f0c2ec2..9fb3bad 100644 --- a/Firmware/EggbotWireless/src/Stepper.cpp +++ b/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); } diff --git a/Firmware/MotorControl/src/commonSrc b/Firmware/MotorControl/src/commonSrc new file mode 120000 index 0000000..4a6da72 --- /dev/null +++ b/Firmware/MotorControl/src/commonSrc @@ -0,0 +1 @@ +../../commonSrc \ No newline at end of file diff --git a/Firmware/MotorControl/src/main.cpp b/Firmware/MotorControl/src/main.cpp new file mode 100644 index 0000000..dd7b163 --- /dev/null +++ b/Firmware/MotorControl/src/main.cpp @@ -0,0 +1,153 @@ +#include +#include +#include +#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; + } +} diff --git a/Firmware/MotorControl/test/README b/Firmware/MotorControl/test/README new file mode 100644 index 0000000..df5066e --- /dev/null +++ b/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 diff --git a/Firmware/common/Commands.h b/Firmware/common/Commands.h new file mode 100644 index 0000000..1b7bda4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Firmware/commonSrc/Commands.cpp b/Firmware/commonSrc/Commands.cpp new file mode 100644 index 0000000..6a36771 --- /dev/null +++ b/Firmware/commonSrc/Commands.cpp @@ -0,0 +1,11 @@ +#include + +void bytesToFloat(float *target, byte *val) { + memcpy(target, val, 4); + return; +} + +void floatToBytes(byte *target, float val) { + memcpy(target, &val, 4); + return; +} \ No newline at end of file