diff --git a/Firmware/EggbotWireless/include/GCodeParser.h b/Firmware/EggbotWireless/include/GCodeParser.h new file mode 100644 index 0000000..f236c8b --- /dev/null +++ b/Firmware/EggbotWireless/include/GCodeParser.h @@ -0,0 +1,20 @@ +#include + +#ifndef PARSER_H +#define PARSER_H + +enum command { + unk, + G01, + M99, +}; + +enum bcAxis { + X = 1, + Y = 2, + Z = 3, +}; + +int *parseGCode(String gcode); + +#endif \ No newline at end of file diff --git a/Firmware/EggbotWireless/include/Pen.h b/Firmware/EggbotWireless/include/Pen.h index dc0a5c2..2735079 100644 --- a/Firmware/EggbotWireless/include/Pen.h +++ b/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(); }; diff --git a/Firmware/EggbotWireless/include/Stepper.h b/Firmware/EggbotWireless/include/Stepper.h index 115de44..3e00d00 100644 --- a/Firmware/EggbotWireless/include/Stepper.h +++ b/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(); diff --git a/Firmware/EggbotWireless/include/StepperTimer.h b/Firmware/EggbotWireless/include/StepperTimer.h index ee8e10d..b972858 100644 --- a/Firmware/EggbotWireless/include/StepperTimer.h +++ b/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 diff --git a/Firmware/EggbotWireless/src/GCodeParser.cpp b/Firmware/EggbotWireless/src/GCodeParser.cpp new file mode 100644 index 0000000..62ccfac --- /dev/null +++ b/Firmware/EggbotWireless/src/GCodeParser.cpp @@ -0,0 +1,74 @@ +#include "GCodeParser.h" +#include + +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; +} \ No newline at end of file diff --git a/Firmware/EggbotWireless/src/Pen.cpp b/Firmware/EggbotWireless/src/Pen.cpp index efc0eae..337ce37 100644 --- a/Firmware/EggbotWireless/src/Pen.cpp +++ b/Firmware/EggbotWireless/src/Pen.cpp @@ -1,13 +1,25 @@ -#include #include "Pen.h" +#include 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() {} diff --git a/Firmware/EggbotWireless/src/Stepper.cpp b/Firmware/EggbotWireless/src/Stepper.cpp index 228935f..2470603 100644 --- a/Firmware/EggbotWireless/src/Stepper.cpp +++ b/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; } \ No newline at end of file diff --git a/Firmware/EggbotWireless/src/StepperTimer.cpp b/Firmware/EggbotWireless/src/StepperTimer.cpp index 0c59e81..6c91646 100644 --- a/Firmware/EggbotWireless/src/StepperTimer.cpp +++ b/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)); } \ No newline at end of file diff --git a/Firmware/EggbotWireless/src/main.cpp b/Firmware/EggbotWireless/src/main.cpp index b3a38b6..63d70a0 100644 --- a/Firmware/EggbotWireless/src/main.cpp +++ b/Firmware/EggbotWireless/src/main.cpp @@ -1,23 +1,72 @@ #include +#include +#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 = ""; } }