stepus53/EggbotWireless
1
0
Fork 0
mirror of https://github.com/usatiuk/EggbotWireless.git synced 2025-10-26 16:57:48 +01:00
Code Issues Packages Projects Releases Wiki Activity

proper linear interpolation

Browse Source 
Signed-off-by: Stepan Usatyuk <usaatyuk@ustk.me>
This commit is contained in:
Stepan Usatiuk
2019-05-10 00:43:47 +03:00
parent 8717e6fe66
commit 3e5ac2de8f
6 changed files with 56 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
Firmware/EggbotWireless/include/EggConstants.h
View File

@@ -1,13 +1,6 @@
#ifndef EGG_H #define EGG_DIA 35.0
#define EGG_H #define Y_DEGREES_PER_MM (360 / (PI * EGG_DIA))
#define PI 3.14 #define X_LIMIT 70.0
#define EGG_LENGTH 50.0
#define EGG_DIA 35 #define X_DEGREES_PER_MM (X_LIMIT / EGG_LENGTH)
#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

6
Firmware/EggbotWireless/include/Stepper.h
View File

@@ -16,20 +16,24 @@ class Stepper {
void clockwise(); void clockwise();
void counterClockwise(); void counterClockwise();
void step(int steps); void step(int steps);
int degreesToSteps(float degrees);
float pos = 0; float pos = 0;
int limit; int limit;
public: public:
Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution, int backlashSteps, int limit, float degreesPerMM); Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution, int backlashSteps, int limit, float degreesPerMM);
int getRemainingSteps();
void rotate(float degrees); void rotate(float degrees);
void rotateTo(float degrees); void rotateTo(float degrees);
void moveTo(float dist); void moveTo(float dist);
void setPos(float degrees); void setPos(float degrees);
float getDist(float degrees); float getDist(float degrees);
int degreesToSteps(float degrees);
int mmToSteps(float dist);
void doStep(); void doStep();
bool finished(); bool finished();
float getPos(); float getPos();
float getPosMm();
float getDistMm(float pos);
}; };
#endif #endif

2
Firmware/EggbotWireless/include/StepperTimer.h
View File

@@ -5,7 +5,7 @@
#define STEPPER_TIMER_H #define STEPPER_TIMER_H
class StepperTimer { class StepperTimer {
private: private:
int speedDelay; float speedDelay;
int stepsPerRevolution; int stepsPerRevolution;
Ticker stepperTicker; Ticker stepperTicker;
Stepper *stepper; Stepper *stepper;

12
Firmware/EggbotWireless/src/Stepper.cpp
View File

@@ -197,11 +197,19 @@ void Stepper::rotateTo(float degrees) {
rotate(mod - pos); rotate(mod - pos);
} }
void Stepper::moveTo(float dist) { void Stepper::moveTo(float dist) { rotateTo(dist * degreesPerMM); }
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; } void Stepper::setPos(float degrees) { pos = degrees; }
int Stepper::getRemainingSteps() { return remainingSteps; }
bool Stepper::finished() { return remainingSteps == 0; } bool Stepper::finished() { return remainingSteps == 0; }
float Stepper::getPos() { return pos; } float Stepper::getPos() { return pos; }

8
Firmware/EggbotWireless/src/StepperTimer.cpp
View File

@@ -4,8 +4,8 @@
StepperTimer::StepperTimer(float rpm, int stepsPerRevolution) StepperTimer::StepperTimer(float rpm, int stepsPerRevolution)
: stepsPerRevolution(stepsPerRevolution) { : stepsPerRevolution(stepsPerRevolution) {
stepperTicker.detach(); stepperTicker.detach();
speedDelay = 60 * 1000 / (rpm * stepsPerRevolution); speedDelay = 60 / (rpm * stepsPerRevolution);
stepperTicker.attach_ms(speedDelay, std::bind(&StepperTimer::tick, this)); stepperTicker.attach(speedDelay, std::bind(&StepperTimer::tick, this));
} }
void StepperTimer::setStepper(Stepper *_stepper) { stepper = _stepper; } void StepperTimer::setStepper(Stepper *_stepper) { stepper = _stepper; }
@@ -14,6 +14,6 @@ void StepperTimer::tick() { stepper->doStep(); }
void StepperTimer::setRPM(float rpm) { void StepperTimer::setRPM(float rpm) {
stepperTicker.detach(); stepperTicker.detach();
speedDelay = 60 * 1000 / (rpm * stepsPerRevolution); speedDelay = 60 / (rpm * stepsPerRevolution);
stepperTicker.attach_ms(speedDelay, std::bind(&StepperTimer::tick, this)); stepperTicker.attach(speedDelay, std::bind(&StepperTimer::tick, this));
} }

44
Firmware/EggbotWireless/src/main.cpp
View File

@@ -1,17 +1,19 @@
#include <Arduino.h> #include <Arduino.h>
#include <ESP8266WiFi.h> #include <ESP8266WiFi.h>
#include "EggConstants.h"
#include "GCodeParser.h" #include "GCodeParser.h"
#include "Pen.h" #include "Pen.h"
#include "Stepper.h" #include "Stepper.h"
#include "StepperTimer.h" #include "StepperTimer.h"
#include "EggConstants.h"
#define STEPS_PER_REVOLUTION 4076 #define STEPS_PER_REVOLUTION 4076
int RPM = 4; int RPM = 4;
Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION, 10, 0, Y_DEGREES_PER_MM); Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION, 0, 0,
Stepper servoStepper(D5, D6, D7, D8, -STEPS_PER_REVOLUTION, 10, X_LIMIT, X_DEGREES_PER_MM); 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); Pen pen(D0, 100, 170);
StepperTimer eggStepperTimer(RPM, STEPS_PER_REVOLUTION); StepperTimer eggStepperTimer(RPM, STEPS_PER_REVOLUTION);
@@ -19,17 +21,20 @@ StepperTimer servoStepperTimer(RPM, STEPS_PER_REVOLUTION);
String inString; String inString;
void execCommand(float *command) { void execCommand(float *command) {
if (command[0] == G01) { if (command[0] == G01) {
if (command[X] != -1 && command[Y] != -1) { if (command[X] != -1 && command[Y] != -1) {
float distX = servoStepper.getDist(command[X]); float distX = servoStepper.getDistMm(command[X]);
float distY = eggStepper.getDist(command[Y]); float distY = eggStepper.getDistMm(command[Y]);
if (distX != 0 && distY != 0) { int stepsX = servoStepper.mmToSteps(distX);
if (distX > distY) { int stepsY = eggStepper.mmToSteps(distY);
eggStepperTimer.setRPM(RPM * distY / distX); if (stepsX != 0 && stepsY != 0) {
} else if (distY > distX) { if (stepsX > stepsY) {
servoStepperTimer.setRPM(RPM * distX / distY); 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("Status:");
Serial.println("X: " + String(servoStepper.getPos())); Serial.println("X: " + String(servoStepper.getPos()));
Serial.println("Y: " + String(eggStepper.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("PEN: " + String(pen.getEngaged()));
return; return;
@@ -78,11 +85,22 @@ void loop() {
if (inChar == '\n') { if (inChar == '\n') {
inString.trim(); inString.trim();
execCommand(parseGCode(inString)); execCommand(parseGCode(inString));
while (!eggStepper.finished() || !servoStepper.finished()) { while (!eggStepper.finished() || !servoStepper.finished()) {
delay(100); 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); eggStepperTimer.setRPM(RPM);
servoStepperTimer.setRPM(RPM); servoStepperTimer.setRPM(RPM);