diff --git a/Firmware/MotorControl/platformio.ini b/Firmware/MotorControl/platformio.ini
index 68f7663..e4257ee 100644
--- a/Firmware/MotorControl/platformio.ini
+++ b/Firmware/MotorControl/platformio.ini
@@ -22,21 +22,24 @@ board_hardware.oscillator = external
 board_hardware.uart = no_bootloader
 ; Brown-out detection
 board_hardware.bod = 2.7v
-
-; specify fuses because platformio burns wrong ones when uploading
-; actually this doesn't help?
-; for whatever reason avrdude gives an error if you don't upload
-; just after flashing fuses
-board_fuse.lfuse = 0xf7
-board_fuse.hfuse = 0xd7
-board_fuse.efuse = 0xfd
+build_unflags = -flto
 
 board_upload.speed = 115200
 
 monitor_speed = 9600
 
-upload_protocol = stk500
-upload_port = COM3
-upload_flags = -PCOM3
+; Custom upload procedure
+upload_protocol = custom
+; Avrdude upload flags
+upload_flags =
+  -C$PROJECT_PACKAGES_DIR/tool-avrdude/avrdude.conf
+  -p$BOARD_MCU
+  -PCOM3
+  -cstk500
+  -e
+; Avrdude upload command
+upload_command = avrdude $UPLOAD_FLAGS -U flash:w:$SOURCE:i
+
+
 
 lib_deps = 883 #Servo
\ No newline at end of file
diff --git a/Firmware/MotorControl/src/main.cpp b/Firmware/MotorControl/src/main.cpp
index 2c975ef..3322307 100644
--- a/Firmware/MotorControl/src/main.cpp
+++ b/Firmware/MotorControl/src/main.cpp
@@ -20,21 +20,21 @@ int calculateDelay(float rpm, int stepsPerRevolution) {
 }
 
 void adjustRPM() {
-    eggStepperRPM = curRPM;
-    servoStepperRPM = curRPM;
+    eggStepperDelay = calculateDelay(curRPM, STEPS_PER_REVOLUTION);
+    servoStepperDelay = calculateDelay(curRPM, STEPS_PER_REVOLUTION);
     if (needAdjust) {
         unsigned int stepsX = servoStepper.getRemainingSteps();
         unsigned int stepsY = eggStepper.getRemainingSteps();
         if (stepsX != 0 && stepsY != 0) {
             if (stepsX > stepsY) {
-                eggStepperRPM = (float)curRPM * (float)stepsY / (float)stepsX;
-            } else if (stepsY > stepsX) {
-                servoStepperRPM = (float)curRPM * (float)stepsX / (float)stepsY;
+                eggStepperDelay =
+                    (float)servoStepperDelay * (float)stepsY / (float)stepsX;
+            } else {
+                servoStepperDelay =
+                    (float)eggStepperDelay * (float)stepsX / (float)stepsY;
             }
         }
     }
-    eggStepperDelay = calculateDelay(eggStepperRPM, STEPS_PER_REVOLUTION);
-    servoStepperDelay = calculateDelay(servoStepperRPM, STEPS_PER_REVOLUTION);
 }
 
 Command command;
@@ -77,8 +77,6 @@ void requestEvent() {
 
     sts.toBytes(txBuffer);
     Wire.write(txBuffer, i2cStsBytes);
-
-    wdt_reset();
 }
 
 void execCommand(Command cmd) {
@@ -137,6 +135,7 @@ void setup() {
 }
 
 volatile unsigned int tick = 0;
+volatile bool newTick = false;
 void steppersRoutine() {
     if (tick % eggStepperDelay == 0) {
         eggStepper.doStep();
@@ -155,11 +154,16 @@ We use our own timer for more precise timings
  */
 ISR(TIMER2_COMPA_vect) {
     tick++;
-    steppersRoutine();
+    newTick = true;
 }
 
 void loop() {
     if (newCommand) {
         execCommand(command);
     }
+    if (newTick) {
+        steppersRoutine();
+        newTick = false;
+    }
+    wdt_reset();
 }