diff --git a/Firmware/EggbotWireless/include/EggConstants.h b/Firmware/EggbotWireless/include/EggConstants.h index 569e5b0..6ad7e81 100644 --- a/Firmware/EggbotWireless/include/EggConstants.h +++ b/Firmware/EggbotWireless/include/EggConstants.h @@ -1,13 +1,6 @@ -#ifndef EGG_H -#define EGG_H +#define EGG_DIA 35.0 +#define Y_DEGREES_PER_MM (360 / (PI * EGG_DIA)) -#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 +#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/Stepper.h b/Firmware/EggbotWireless/include/Stepper.h index e7aaacc..442371a 100644 --- a/Firmware/EggbotWireless/include/Stepper.h +++ b/Firmware/EggbotWireless/include/Stepper.h @@ -16,20 +16,24 @@ class Stepper { void clockwise(); void counterClockwise(); void step(int steps); - int degreesToSteps(float degrees); float pos = 0; int limit; public: Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution, int backlashSteps, int limit, float degreesPerMM); + 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 \ No newline at end of file diff --git a/Firmware/EggbotWireless/include/StepperTimer.h b/Firmware/EggbotWireless/include/StepperTimer.h index b972858..4f61c96 100644 --- a/Firmware/EggbotWireless/include/StepperTimer.h +++ b/Firmware/EggbotWireless/include/StepperTimer.h @@ -5,7 +5,7 @@ #define STEPPER_TIMER_H class StepperTimer { private: - int speedDelay; + float speedDelay; int stepsPerRevolution; Ticker stepperTicker; Stepper *stepper; diff --git a/Firmware/EggbotWireless/src/Stepper.cpp b/Firmware/EggbotWireless/src/Stepper.cpp index 145237b..711cd31 100644 --- a/Firmware/EggbotWireless/src/Stepper.cpp +++ b/Firmware/EggbotWireless/src/Stepper.cpp @@ -197,11 +197,19 @@ void Stepper::rotateTo(float degrees) { rotate(mod - pos); } -void Stepper::moveTo(float dist) { - rotateTo(dist * degreesPerMM); +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; } \ No newline at end of file diff --git a/Firmware/EggbotWireless/src/StepperTimer.cpp b/Firmware/EggbotWireless/src/StepperTimer.cpp index 6c91646..14ec7bb 100644 --- a/Firmware/EggbotWireless/src/StepperTimer.cpp +++ b/Firmware/EggbotWireless/src/StepperTimer.cpp @@ -4,8 +4,8 @@ StepperTimer::StepperTimer(float rpm, int stepsPerRevolution) : stepsPerRevolution(stepsPerRevolution) { stepperTicker.detach(); - speedDelay = 60 * 1000 / (rpm * stepsPerRevolution); - stepperTicker.attach_ms(speedDelay, std::bind(&StepperTimer::tick, this)); + speedDelay = 60 / (rpm * stepsPerRevolution); + stepperTicker.attach(speedDelay, std::bind(&StepperTimer::tick, this)); } void StepperTimer::setStepper(Stepper *_stepper) { stepper = _stepper; } @@ -14,6 +14,6 @@ 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)); + speedDelay = 60 / (rpm * stepsPerRevolution); + stepperTicker.attach(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 add8ffb..6a5b65d 100644 --- a/Firmware/EggbotWireless/src/main.cpp +++ b/Firmware/EggbotWireless/src/main.cpp @@ -1,17 +1,19 @@ #include #include +#include "EggConstants.h" #include "GCodeParser.h" #include "Pen.h" #include "Stepper.h" #include "StepperTimer.h" -#include "EggConstants.h" #define STEPS_PER_REVOLUTION 4076 int RPM = 4; -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); +Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION, 0, 0, + Y_DEGREES_PER_MM); +Stepper servoStepper(D5, D6, D7, D8, -STEPS_PER_REVOLUTION, 0, X_LIMIT, + X_DEGREES_PER_MM); Pen pen(D0, 100, 170); StepperTimer eggStepperTimer(RPM, STEPS_PER_REVOLUTION); @@ -19,17 +21,20 @@ StepperTimer servoStepperTimer(RPM, STEPS_PER_REVOLUTION); String inString; - void execCommand(float *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); + 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); } } } @@ -56,6 +61,8 @@ void execCommand(float *command) { 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())); return; @@ -78,11 +85,22 @@ void loop() { if (inChar == '\n') { inString.trim(); - execCommand(parseGCode(inString)); - while (!eggStepper.finished() || !servoStepper.finished()) { delay(100); + cli(); + int stepsX = servoStepper.getRemainingSteps(); + int stepsY = eggStepper.getRemainingSteps(); + Serial.println(stepsX); + Serial.println(stepsY); + 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); + } + sei(); } eggStepperTimer.setRPM(RPM); servoStepperTimer.setRPM(RPM);