diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 63f4f298e..67601fb1a 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -6998,13 +6998,16 @@ if((eSoundMode==e_SOUND_MODE_LOUD)||(eSoundMode==e_SOUND_MODE_ONCE))
 		if (mmu_enabled)
 		{
 			tmp_extruder = choose_menu_P(_T(MSG_CHOOSE_FILAMENT), _T(MSG_FILAMENT));
-			if ((tmp_extruder == mmu_extruder) && mmu_fil_loaded) {
-				printf_P(PSTR("Duplicit T-code ignored.\n"));
-				return; //dont execute the same T-code twice in a row
+			if ((tmp_extruder == mmu_extruder) && mmu_fil_loaded) //dont execute the same T-code twice in a row
+			{
+				printf_P(PSTR("Duplicate T-code ignored.\n"));
+			}
+			else
+			{
+				st_synchronize();
+				mmu_command(MmuCmd::T0 + tmp_extruder);
+				manage_response(true, true, MMU_TCODE_MOVE);
 			}
-			st_synchronize();
-			mmu_command(MmuCmd::T0 + tmp_extruder);
-			manage_response(true, true, MMU_TCODE_MOVE);
 		}
 	  }
 	  else if (*(strchr_pointer + index) == 'c') { //load to from bondtech gears to nozzle (nozzle should be preheated)
@@ -7040,20 +7043,23 @@ if((eSoundMode==e_SOUND_MODE_LOUD)||(eSoundMode==e_SOUND_MODE_ONCE))
 
           if (mmu_enabled)
           {
-              if ((tmp_extruder == mmu_extruder) && mmu_fil_loaded) {
-                  printf_P(PSTR("Duplicit T-code ignored.\n"));
-                  return; //dont execute the same T-code twice in a row
-              }
-              mmu_command(MmuCmd::T0 + tmp_extruder);
-
-			  manage_response(true, true, MMU_TCODE_MOVE);
-			  mmu_continue_loading();
-			  mmu_extruder = tmp_extruder; //filament change is finished
-
-              if (load_to_nozzle)// for single material usage with mmu
+              if ((tmp_extruder == mmu_extruder) && mmu_fil_loaded) //dont execute the same T-code twice in a row
               {
-                  mmu_load_to_nozzle();
+                  printf_P(PSTR("Duplicate T-code ignored.\n"));
               }
+			  else
+			  {
+				  mmu_command(MmuCmd::T0 + tmp_extruder);
+
+				  manage_response(true, true, MMU_TCODE_MOVE);
+				  mmu_continue_loading();
+				  mmu_extruder = tmp_extruder; //filament change is finished
+
+				  if (load_to_nozzle)// for single material usage with mmu
+				  {
+					  mmu_load_to_nozzle();
+				  }
+			  }
           }
           else
           {
diff --git a/Firmware/mmu.cpp b/Firmware/mmu.cpp
index 5cf650c98..dad00f8ee 100644
--- a/Firmware/mmu.cpp
+++ b/Firmware/mmu.cpp
@@ -58,9 +58,9 @@ static S mmu_state = S::Disabled;
 MmuCmd mmu_cmd = MmuCmd::None;
 
 //idler ir sensor
-uint8_t mmu_idl_sens = 0;
+static uint8_t mmu_idl_sens = 0;
 bool ir_sensor_detected = false; 
-bool mmu_loading_flag = false; //when set to true, we assume that mmu2 unload was finished and loading phase is now performed; printer can send 'A' to mmu2 to abort loading process
+static bool mmu_loading_flag = false; //when set to true, we assume that mmu2 unload was finished and loading phase is now performed; printer can send 'A' to mmu2 to abort loading process
 
 uint8_t mmu_extruder = MMU_FILAMENT_UNKNOWN;
 
diff --git a/Firmware/mmu.h b/Firmware/mmu.h
index 9c233d05f..41e0ba1f4 100644
--- a/Firmware/mmu.h
+++ b/Firmware/mmu.h
@@ -15,7 +15,6 @@ extern uint8_t tmp_extruder;
 
 extern int8_t mmu_finda;
 extern bool ir_sensor_detected;
-extern bool mmu_loading_flag;
 
 extern int16_t mmu_version;
 extern int16_t mmu_buildnr;
diff --git a/Firmware/timer02.c b/Firmware/timer02.c
index 8309f9401..113c72463 100644
--- a/Firmware/timer02.c
+++ b/Firmware/timer02.c
@@ -11,19 +11,11 @@
 
 uint8_t timer02_pwm0 = 0;
 
+
 void timer02_set_pwm0(uint8_t pwm0)
 {
-	if (timer02_pwm0 == pwm0) return;
-	if (pwm0)
-	{
-		TCCR0A |= (2 << COM0B0);
-		OCR0B = pwm0 - 1;
-	}
-	else
-	{
-		TCCR0A &= ~(2 << COM0B0);
-		OCR0B = 0;
-	}
+	TCCR0A |= (2 << COM0B0); //switch OC0B to OCR mode
+	OCR0B = (uint16_t)OCR0A * pwm0 / 255;
 	timer02_pwm0 = pwm0;
 }
 
@@ -39,13 +31,12 @@ void timer02_init(void)
 	TIMSK0 &= ~(1<<OCIE0B);
 	//setup timer0
 	TCCR0A = 0x00; //COM_A-B=00, WGM_0-1=00
-	TCCR0B = (1 << CS00); //WGM_2=0, CS_0-2=011
-	//switch timer0 to fast pwm mode
-	TCCR0A |= (3 << WGM00); //WGM_0-1=11
-	//set OCR0B register to zero
-	OCR0B = 0;
-	//disable OCR0B output (will be enabled in timer02_set_pwm0)
-	TCCR0A &= ~(2 << COM0B0);
+	OCR0A = 200; //max PWM value (freq = 40kHz)
+	OCR0B = 0; //current PWM value
+	//switch timer0 to mode 5 (Phase Correct PWM)
+	TCCR0A |= (1 << WGM00); //WGM_0-1=01
+	TCCR0B = (1 << CS00) | (1 << WGM02); //WGM_2=1, CS_0-2=001 (no prescaling)
+	TCCR0A |= (2 << COM0B0); //switch OC0B to OCR mode
 	//setup timer2
 	TCCR2A = 0x00; //COM_A-B=00, WGM_0-1=00
 	TCCR2B = (4 << CS20); //WGM_2=0, CS_0-2=011