mirror of
https://github.com/usatiuk/EggbotWireless.git
synced 2025-10-26 08:47:49 +01:00
feature to set feedrate
This commit is contained in:
@@ -4,7 +4,10 @@
|
|||||||
#include "ConfigManager.h"
|
#include "ConfigManager.h"
|
||||||
|
|
||||||
const std::unordered_map<std::string, std::string> defaults{
|
const std::unordered_map<std::string, std::string> defaults{
|
||||||
{{"wifiMode", "ap"}, {"wifiSSID", defSSID}, {"wifiPass", defPass}}};
|
{{"wifiMode", "ap"},
|
||||||
|
{"wifiSSID", defSSID},
|
||||||
|
{"wifiPass", defPass},
|
||||||
|
{"defRpm", "2"}}};
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Max string length is 25
|
Max string length is 25
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
#include <unordered_map>
|
#include <unordered_map>
|
||||||
|
|
||||||
|
#include "ConfigManager.h"
|
||||||
#include "GCodeParser.h"
|
#include "GCodeParser.h"
|
||||||
|
|
||||||
Command bufcmd;
|
Command bufcmd;
|
||||||
@@ -56,9 +57,10 @@ Command parseGCode(std::string gcode) {
|
|||||||
auto xIter = argsMap.find('X');
|
auto xIter = argsMap.find('X');
|
||||||
auto yIter = argsMap.find('Y');
|
auto yIter = argsMap.find('Y');
|
||||||
auto zIter = argsMap.find('Z');
|
auto zIter = argsMap.find('Z');
|
||||||
|
auto fIter = argsMap.find('F');
|
||||||
auto endIter = argsMap.end();
|
auto endIter = argsMap.end();
|
||||||
|
|
||||||
if(xIter != endIter) {
|
if (xIter != endIter) {
|
||||||
bufcmd.arg1 = xIter->second;
|
bufcmd.arg1 = xIter->second;
|
||||||
}
|
}
|
||||||
if (yIter != endIter) {
|
if (yIter != endIter) {
|
||||||
@@ -67,6 +69,11 @@ Command parseGCode(std::string gcode) {
|
|||||||
if (zIter != endIter) {
|
if (zIter != endIter) {
|
||||||
bufcmd.arg3 = zIter->second;
|
bufcmd.arg3 = zIter->second;
|
||||||
}
|
}
|
||||||
|
if (fIter != endIter) {
|
||||||
|
bufcmd.arg4 = fIter->second;
|
||||||
|
} else {
|
||||||
|
bufcmd.arg4 = atof(configManager.get("defRpm").c_str());
|
||||||
|
}
|
||||||
|
|
||||||
if (strcmp(command, "G00") == 0) {
|
if (strcmp(command, "G00") == 0) {
|
||||||
bufcmd.type = CommandType::G00;
|
bufcmd.type = CommandType::G00;
|
||||||
|
|||||||
@@ -3,4 +3,8 @@
|
|||||||
|
|
||||||
constexpr unsigned int stsUpdDelay{10};
|
constexpr unsigned int stsUpdDelay{10};
|
||||||
|
|
||||||
|
constexpr float defRPM{2}, defEggLength{60}, defEggDia{45}, xLimit{85};
|
||||||
|
constexpr float defYDegPerMM{360 / (PI * defEggDia)},
|
||||||
|
defXDegPerMM{xLimit / defEggLength};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
@@ -1,29 +1,25 @@
|
|||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
#include "Pen.h"
|
#include "Pen.h"
|
||||||
#include "Stepper.h"
|
#include "Stepper.h"
|
||||||
|
#include "Config.h"
|
||||||
|
|
||||||
#ifndef GLOBALS_H
|
#ifndef GLOBALS_H
|
||||||
#define GLOBALS_H
|
#define GLOBALS_H
|
||||||
|
|
||||||
#define EGG_DIA 45.0
|
|
||||||
#define Y_DEGREES_PER_MM (360 / (PI * EGG_DIA))
|
|
||||||
|
|
||||||
#define X_LIMIT 85.0
|
|
||||||
#define EGG_LENGTH 60.0
|
|
||||||
#define X_DEGREES_PER_MM (X_LIMIT / EGG_LENGTH)
|
|
||||||
|
|
||||||
#define STEPS_PER_REVOLUTION (360/1.8) * 32
|
#define STEPS_PER_REVOLUTION (360/1.8) * 32
|
||||||
#define DEF_RPM 2
|
|
||||||
|
|
||||||
Stepper eggStepper(6, 5, STEPS_PER_REVOLUTION, 0, Y_DEGREES_PER_MM);
|
Stepper eggStepper(6, 5, STEPS_PER_REVOLUTION, 0, defYDegPerMM);
|
||||||
Stepper servoStepper(4, 3, STEPS_PER_REVOLUTION, X_LIMIT,
|
Stepper servoStepper(4, 3, STEPS_PER_REVOLUTION, xLimit,
|
||||||
X_DEGREES_PER_MM);
|
defXDegPerMM);
|
||||||
|
|
||||||
Pen pen(7, 120, 180);
|
Pen pen(7, 120, 180);
|
||||||
|
|
||||||
unsigned int eggStepperDelay;
|
unsigned int eggStepperDelay;
|
||||||
unsigned int servoStepperDelay;
|
unsigned int servoStepperDelay;
|
||||||
|
|
||||||
|
unsigned int eggDia;
|
||||||
|
unsigned int eggLength;
|
||||||
|
|
||||||
float eggStepperRPM;
|
float eggStepperRPM;
|
||||||
float servoStepperRPM;
|
float servoStepperRPM;
|
||||||
|
|
||||||
|
|||||||
@@ -7,7 +7,7 @@
|
|||||||
#include "common/Commands.h"
|
#include "common/Commands.h"
|
||||||
#include "common/Status.h"
|
#include "common/Status.h"
|
||||||
|
|
||||||
int curRPM = DEF_RPM;
|
int curRPM = defRPM;
|
||||||
int adjustDelay = 100;
|
int adjustDelay = 100;
|
||||||
bool needAdjust;
|
bool needAdjust;
|
||||||
|
|
||||||
@@ -95,6 +95,10 @@ void execCommand() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (!isnan(command.arg4)) {
|
||||||
|
curRPM = command.arg4;
|
||||||
|
}
|
||||||
|
|
||||||
adjustRPM();
|
adjustRPM();
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -113,7 +117,7 @@ void setup() {
|
|||||||
OCR2A = 250;
|
OCR2A = 250;
|
||||||
TCCR2A |= (1 << WGM20) | (1 << CS22);
|
TCCR2A |= (1 << WGM20) | (1 << CS22);
|
||||||
TIMSK2 |= (1 << OCIE2A);
|
TIMSK2 |= (1 << OCIE2A);
|
||||||
servoStepper.setPos(X_LIMIT);
|
servoStepper.setPos(xLimit);
|
||||||
pen.init();
|
pen.init();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user