diff --git a/Firmware/EggbotWireless/src/main.cpp b/Firmware/EggbotWireless/src/main.cpp
index 185b4de..e52fd17 100644
--- a/Firmware/EggbotWireless/src/main.cpp
+++ b/Firmware/EggbotWireless/src/main.cpp
@@ -19,6 +19,7 @@ void sendCommand(float *command) {
         Wire.endTransmission();
     } else {
         Serial.println("UNK");
+        return;
     }
 
     if (command[0] == G01 || command[0] == G00) {
@@ -29,8 +30,8 @@ void sendCommand(float *command) {
 
     if (command[0] == M99) {
         Wire.requestFrom(8, 5 * sizeof(float));
-        float resp[5];
 
+        float resp[5];
         byte buffer[4];
 
         for (int i = 0; i < 5; i++) {
@@ -63,7 +64,6 @@ void loop() {
     while (Serial.available() > 0) {
         char inChar = Serial.read();
         inString += inChar;
-        Serial.write(inChar);
 
         if (inChar == '\n') {
             inString.trim();
diff --git a/Firmware/MotorControl/include/Globals.h b/Firmware/MotorControl/include/Globals.h
index 555326e..3133d76 100644
--- a/Firmware/MotorControl/include/Globals.h
+++ b/Firmware/MotorControl/include/Globals.h
@@ -27,7 +27,6 @@ float servoStepperDelay;
 float eggStepperRPM;
 float servoStepperRPM;
 
-bool recalculateDelays;
 bool needAdjust;
 
 #endif
\ No newline at end of file
diff --git a/Firmware/MotorControl/include/Pen.h b/Firmware/MotorControl/include/Pen.h
index 7f454d2..7b75032 100644
--- a/Firmware/MotorControl/include/Pen.h
+++ b/Firmware/MotorControl/include/Pen.h
@@ -7,6 +7,7 @@ class Pen {
    private:
     int posEngaged;
     int posDisengaged;
+    int pin;
     bool engaged = true;
     Servo servo;
 
diff --git a/Firmware/MotorControl/src/Pen.cpp b/Firmware/MotorControl/src/Pen.cpp
index 2e7388d..2339bfc 100644
--- a/Firmware/MotorControl/src/Pen.cpp
+++ b/Firmware/MotorControl/src/Pen.cpp
@@ -6,9 +6,7 @@
 #define DELAY 15
 
 Pen::Pen(int pin, int posEngaged, int posDisengaged)
-    : posEngaged(posEngaged), posDisengaged(posDisengaged) {
-    servo.attach(pin);
-}
+    : posEngaged(posEngaged), posDisengaged(posDisengaged), pin(pin) {}
 
 void Pen::engage() {
     if (!engaged) {
@@ -31,6 +29,7 @@ void Pen::disengage() {
 }
 
 void Pen::init() {
+    servo.attach(pin);
     servo.write(posDisengaged);
     engaged = false;
 }
diff --git a/Firmware/MotorControl/src/main.cpp b/Firmware/MotorControl/src/main.cpp
index dd7b163..807d6ae 100644
--- a/Firmware/MotorControl/src/main.cpp
+++ b/Firmware/MotorControl/src/main.cpp
@@ -6,22 +6,29 @@
 
 int curRPM = DEF_RPM;
 int adjustDelay = 10;
+
+float calculateDelay(float rpm, int stepsPerRevolution) {
+    return ((float)1000 * (float)60) / (rpm * (float)stepsPerRevolution);
+}
+
 void adjustRPM() {
-    unsigned int stepsX = servoStepper.getRemainingSteps();
-    unsigned int stepsY = eggStepper.getRemainingSteps();
-    if (stepsX != 0 && stepsY != 0) {
-        if (stepsX > stepsY) {
-            float rpm = (float)curRPM * (float)stepsY / (float)stepsX;
-            eggStepperRPM = rpm;
-        } else if (stepsY > stepsX) {
-            float rpm = (float)curRPM * (float)stepsX / (float)stepsY;
-            servoStepperRPM = rpm;
-        } else {
-            eggStepperRPM = curRPM;
-            servoStepperRPM = curRPM;
+    eggStepperRPM = curRPM;
+    servoStepperRPM = curRPM;
+    if (needAdjust) {
+        unsigned int stepsX = servoStepper.getRemainingSteps();
+        unsigned int stepsY = eggStepper.getRemainingSteps();
+        if (stepsX != 0 && stepsY != 0) {
+            if (stepsX > stepsY) {
+                float rpm = (float)curRPM * (float)stepsY / (float)stepsX;
+                eggStepperRPM = rpm;
+            } else if (stepsY > stepsX) {
+                float rpm = (float)curRPM * (float)stepsX / (float)stepsY;
+                servoStepperRPM = rpm;
+            }
         }
-        recalculateDelays = true;
     }
+    eggStepperDelay = calculateDelay(eggStepperRPM, STEPS_PER_REVOLUTION);
+    servoStepperDelay = calculateDelay(servoStepperRPM, STEPS_PER_REVOLUTION);
 }
 
 int curFloat = 0;
@@ -69,12 +76,6 @@ void requestEvent() {
     }
 }
 
-float calculateDelay(float rpm, int stepsPerRevolution) {
-    return ((float)1000 * (float)60) / (rpm * (float)stepsPerRevolution);
-}
-
-Servo servo;
-
 void execCommand(float *command) {
     if (command[0] == G01 || command[0] == G00) {
         if (command[0] == G01) {
@@ -111,8 +112,7 @@ void setup() {
     Wire.onReceive(receiveEvent);
     Wire.onRequest(requestEvent);
     Serial.println("Hello!");
-    recalculateDelays = true;
-    eggStepperRPM = servoStepperRPM = 4;
+    eggStepperRPM = servoStepperRPM = curRPM;
     OCR2 = 250;
     TCCR2 |= (1 << WGM12) | (1 << CS22);
     TIMSK |= (1 << OCIE2);
@@ -129,25 +129,20 @@ ISR(TIMER2_COMP_vect) {
     if (fmod(ms, servoStepperDelay) < 1) {
         servoStepper.doStep();
     }
-    if (fmod(ms, adjustDelay) < 1) {
-        adjustRPM();
-    }
 }
 
 void loop() {
-    if (eggStepper.getRemainingSteps() == 0 &&
-        servoStepper.getRemainingSteps() == 0) {
-        executing = false;
+    unsigned long ms = millis();
+    if (ms % adjustDelay < 2) {
+        adjustRPM();
     }
     if (newCommand) {
         newCommand = false;
         executing = true;
         execCommand(command);
     }
-    if (recalculateDelays) {
-        eggStepperDelay = calculateDelay(eggStepperRPM, STEPS_PER_REVOLUTION);
-        servoStepperDelay =
-            calculateDelay(servoStepperRPM, STEPS_PER_REVOLUTION);
-        recalculateDelays = false;
+    if (eggStepper.getRemainingSteps() == 0 &&
+        servoStepper.getRemainingSteps() == 0) {
+        executing = false;
     }
 }