From 8b067426a1f33e2435b876d1df3eaffadae569ad Mon Sep 17 00:00:00 2001
From: Stepan Usatiuk <stepanusatiuk@gmail.com>
Date: Tue, 28 May 2019 18:39:53 +0300
Subject: [PATCH] some adjustments to stepper control mechanism

---
 Firmware/EggbotWireless/platformio.ini  |  4 +++-
 Firmware/EggbotWireless/src/main.cpp    |  6 +++++
 Firmware/MotorControl/include/Globals.h |  6 ++---
 Firmware/MotorControl/src/main.cpp      | 29 ++++++++++++++++---------
 4 files changed, 30 insertions(+), 15 deletions(-)

diff --git a/Firmware/EggbotWireless/platformio.ini b/Firmware/EggbotWireless/platformio.ini
index 45a695d..f46eaf0 100644
--- a/Firmware/EggbotWireless/platformio.ini
+++ b/Firmware/EggbotWireless/platformio.ini
@@ -13,4 +13,6 @@ platform = espressif8266
 board = d1_mini
 framework = arduino
 monitor_speed = 115200
-board_build.f_cpu = 160000000L
\ No newline at end of file
+board_build.f_cpu = 160000000L
+
+upload_port=/dev/ttyUSB0
\ No newline at end of file
diff --git a/Firmware/EggbotWireless/src/main.cpp b/Firmware/EggbotWireless/src/main.cpp
index d29fb33..6380b62 100644
--- a/Firmware/EggbotWireless/src/main.cpp
+++ b/Firmware/EggbotWireless/src/main.cpp
@@ -68,13 +68,19 @@ void loop() {
         if (inChar == '\n') {
             inString.trim();
             sendCommand(parseGCode(inString));
+            unsigned long reqTime = millis();
             while (waitingForNext) {
                 while (!Wire.available()) {
+                    if (millis() - reqTime > 500) {
+                        Wire.requestFrom(8, 1);
+                        reqTime = millis();
+                    }
                 }
                 int response = Wire.read();
                 if (response == WAIT) {
                     delay(1);
                     Wire.requestFrom(8, 1);
+                    reqTime = millis();
                 } else if (response == NEXT) {
                     Serial.println("OK");
                     waitingForNext = false;
diff --git a/Firmware/MotorControl/include/Globals.h b/Firmware/MotorControl/include/Globals.h
index 89df22d..720e119 100644
--- a/Firmware/MotorControl/include/Globals.h
+++ b/Firmware/MotorControl/include/Globals.h
@@ -21,12 +21,10 @@ Stepper servoStepper(28, 29, -STEPS_PER_REVOLUTION, X_LIMIT,
 
 Pen pen(23, 100, 180);
 
-float eggStepperDelay;
-float servoStepperDelay;
+unsigned int eggStepperDelay;
+unsigned int servoStepperDelay;
 
 float eggStepperRPM;
 float servoStepperRPM;
 
-bool needAdjust;
-
 #endif
\ No newline at end of file
diff --git a/Firmware/MotorControl/src/main.cpp b/Firmware/MotorControl/src/main.cpp
index 2907252..ffbd127 100644
--- a/Firmware/MotorControl/src/main.cpp
+++ b/Firmware/MotorControl/src/main.cpp
@@ -5,9 +5,11 @@
 #include "common/Commands.h"
 
 int curRPM = DEF_RPM;
-int adjustDelay = 10;
+int adjustDelay = 100;
+bool needAdjust;
+bool moving;
 
-float calculateDelay(float rpm, int stepsPerRevolution) {
+int calculateDelay(float rpm, int stepsPerRevolution) {
     return ((float)1000 * (float)60) / (rpm * (float)stepsPerRevolution);
 }
 
@@ -78,6 +80,7 @@ void requestEvent() {
 
 void execCommand(float *command) {
     executing = true;
+    moving = false;
     if (command[0] == G01 || command[0] == G00) {
         if (command[0] == G01) {
             needAdjust = true;
@@ -101,9 +104,11 @@ void execCommand(float *command) {
                 pen.disengage();
             }
         }
-        
+
         adjustRPM();
 
+        moving = true;
+
         return;
     }
 }
@@ -117,20 +122,24 @@ void setup() {
     Serial.println("Hello!");
     eggStepperRPM = servoStepperRPM = curRPM;
     OCR2 = 250;
-    TCCR2 |= (1 << WGM12) | (1 << CS22);
+    TCCR2 |= (1 << WGM20) | (1 << CS22);
     TIMSK |= (1 << OCIE2);
     sei();
     servoStepper.setPos(X_LIMIT);
     pen.init();
 }
 
+unsigned int ms = 0;
+
 ISR(TIMER2_COMP_vect) {
-    unsigned long ms = millis();
-    if (fmod(ms, eggStepperDelay) < 1) {
-        eggStepper.doStep();
-    }
-    if (fmod(ms, servoStepperDelay) < 1) {
-        servoStepper.doStep();
+    ms++;
+    if (moving) {
+        if (ms % eggStepperDelay == 0) {
+            eggStepper.doStep();
+        }
+        if (ms % servoStepperDelay == 0) {
+            servoStepper.doStep();
+        }
     }
 }