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