basic gcode sender thing

Firmware/MotorControl/include/Globals.h

@@ -8,19 +8,16 @@
 
 #define STEPS_PER_REVOLUTION (360/1.8) * 32
 
-Stepper eggStepper(6, 5, STEPS_PER_REVOLUTION, 0, defYDegPerMM);
-Stepper servoStepper(4, 3, STEPS_PER_REVOLUTION, xLimit,
-                     defXDegPerMM);
+extern Stepper eggStepper;
+extern Stepper servoStepper;
 
-Pen pen(7, 120, 180);
+extern unsigned int eggStepperDelay;
+extern unsigned int servoStepperDelay;
 
-unsigned int eggStepperDelay;
-unsigned int servoStepperDelay;
+extern unsigned int eggDia;
+extern unsigned int eggLength;
 
-unsigned int eggDia;
-unsigned int eggLength;
-
-float eggStepperRPM;
-float servoStepperRPM;
+extern float eggStepperRPM;
+extern float servoStepperRPM;
 
 #endif

Firmware/MotorControl/include/Pen.h

@@ -17,7 +17,8 @@ class Pen {
     void disengage();
     void init();
     bool getEngaged();
-    ~Pen();
 };
 
+extern Pen pen;
+
 #endif

Firmware/MotorControl/src/Globals.cpp

@@ -0,0 +1,13 @@
+#include "Globals.h"
+
+Stepper eggStepper(6, 5, STEPS_PER_REVOLUTION, 0, defYDegPerMM);
+Stepper servoStepper(4, 3, STEPS_PER_REVOLUTION, xLimit, defXDegPerMM);
+
+unsigned int eggStepperDelay;
+unsigned int servoStepperDelay;
+
+unsigned int eggDia;
+unsigned int eggLength;
+
+float eggStepperRPM;
+float servoStepperRPM;

Firmware/MotorControl/src/Pen.cpp

@@ -3,27 +3,19 @@
 
 #include "Pen.h"
 
-#define DELAY 15
-
 Pen::Pen(int pin, int posEngaged, int posDisengaged)
     : posEngaged(posEngaged), posDisengaged(posDisengaged), pin(pin) {}
 
 void Pen::engage() {
     if (!engaged) {
-        for (int i = posDisengaged; i > posEngaged; i--) {
-            servo.write(i);
-            delay(DELAY);
-        }
+        servo.write(posEngaged);
     }
     engaged = true;
 }
 
 void Pen::disengage() {
     if (engaged) {
-        for (int i = posEngaged; i < posDisengaged; i++) {
-            servo.write(i);
-            delay(DELAY);
-        }
+        servo.write(posDisengaged);
     }
     engaged = false;
 }
@@ -36,4 +28,4 @@ void Pen::init() {
 
 bool Pen::getEngaged() { return engaged; }
 
-Pen::~Pen() {}
+Pen pen(7, 120, 170);

Firmware/MotorControl/src/main.cpp

@@ -35,10 +35,8 @@ void adjustRPM() {
         calculateDelay(eggStepperRPM, STEPS_PER_REVOLUTION);
     int newServoStepperDelay =
         calculateDelay(servoStepperRPM, STEPS_PER_REVOLUTION);
-    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-        eggStepperDelay = newEggStepperDelay;
-        servoStepperDelay = newServoStepperDelay;
-    }
+    eggStepperDelay = newEggStepperDelay;
+    servoStepperDelay = newServoStepperDelay;
 }
 
 Command command;
@@ -67,36 +65,36 @@ byte txBuffer[i2cStsBytes];
 Status sts;
 void requestEvent() { Wire.write(txBuffer, i2cStsBytes); }
 
-void execCommand() {
+void execCommand(Command cmd) {
     executing = true;
+    newCommand = false;
 
-    if (command.type == CommandType::G01 || command.type == CommandType::G00) {
-        newCommand = false;
-        if (command.type == CommandType::G01) {
+    if (cmd.type == CommandType::G01 || cmd.type == CommandType::G00) {
+        if (cmd.type == CommandType::G01) {
             needAdjust = true;
         } else {
             needAdjust = false;
         }
 
-        if (!isnan(command.arg1)) {
-            servoStepper.moveTo(command.arg1);
+        if (!isnan(cmd.arg1)) {
+            servoStepper.moveTo(cmd.arg1);
         }
 
-        if (!isnan(command.arg2)) {
-            eggStepper.moveTo(command.arg2);
+        if (!isnan(cmd.arg2)) {
+            eggStepper.moveTo(cmd.arg2);
         }
 
-        if (!isnan(command.arg3)) {
-            if (command.arg3 < 0) {
+        if (!isnan(cmd.arg3)) {
+            if (cmd.arg3 < 0) {
                 pen.engage();
             }
-            if (command.arg3 >= 0) {
+            if (cmd.arg3 >= 0) {
                 pen.disengage();
             }
         }
 
-        if (!isnan(command.arg4)) {
-            curRPM = command.arg4;
+        if (!isnan(cmd.arg4)) {
+            curRPM = cmd.arg4;
         }
 
         adjustRPM();
@@ -157,6 +155,8 @@ void steppersRoutine() {
             sts.mmS = servoStepper.getPosMm();
             sts.mmE = eggStepper.getPosMm();
 
+            sts.feedrate = curRPM;
+
             sts.pEng = (float)pen.getEngaged();
 
             ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { sts.toBytes(txBuffer); }
@@ -171,7 +171,7 @@ void steppersRoutine() {
 
 void loop() {
     if (newCommand) {
-        execCommand();
+        execCommand(command);
     }
     steppersRoutine();
 }