diff --git a/blmc.c b/blmc.c
index 0a3bd82..22aedfc 100644
--- a/blmc.c
+++ b/blmc.c
@@ -34,11 +34,15 @@ void trigger_adc(uint8_t channel)
 	/* Disable Analog Comperator */
 	ACSR &= ~(1<<ACIE);
 
-	/* set channel (Internal reference, 2.56V) */
-	ADMUX = (1<<REFS1) | (1<<REFS0) | channel;
+	/* set channel (external reference, 5V) */
+	ADMUX = (0<<REFS1) | (0<<REFS0) | channel;
 
-	/* turn on ADC with interrupts, start conversion with 1/32 of F_CPU */
-	ADCSRA = (1<<ADEN) | (1<<ADSC) | (1<<ADIE) | (1<<ADIF)| (1<<ADPS2) | (1<<ADPS0);
+	/*
+	 * turn on ADC with interrupts
+	 * start conversion with 1/16 of F_CPU
+	 * 1/500kHz * 25 => 50us conversion time
+	 */
+	ADCSRA = (1<<ADEN) | (1<<ADSC) | (1<<ADIE) | (1<<ADIF)| (1<<ADPS2) | (0<<ADPS0);
 }
 
 void next_phase(void)
@@ -53,62 +57,62 @@ void next_phase(void)
 	TCNT2 = 0x00;
 
 	switch (phase) {
-	case 0: /* A: PWM, B: LOW, C: SENSE */
-		PORTB |= PHASE_A_EN;
-		PORTD = (PORTD & ~PHASE_C_EN) | PHASE_B_EN;
+	case 0: /* A: PWM, B: LOW, C: SENSE => disable EN_C, disable PWM_B, enable EN_B */
+		PORTD &= ~PHASE_C_EN;
 		TCCR0A &= ~PHASE_B_OC;
-		TCCR2A = (TCCR2A & ~PHASE_C_OC) | PHASE_A_OC;
+		PORTD |= PHASE_B_EN;
 
+		/* C: falling edge */
 		ACSR = (1<<ACIS1);
 		next_sense = SENSE_C;
 		break;
 
-	case 1: /* A: SENSE, B: LOW, C: PWM */
+	case 1: /* A: SENSE, B: LOW, C: PWM => disable EN_A, enable PWM_C, enable EN_C */
 		PORTB &= ~PHASE_A_EN;
-		PORTD |= PHASE_B_EN | PHASE_C_EN;
-		TCCR0A &= ~PHASE_B_OC;
-		TCCR2A = (TCCR2A & ~PHASE_A_OC) | PHASE_C_OC;
+		TCCR2A |= PHASE_C_OC;
+		PORTD |= PHASE_C_EN;
 
+		/* A: rising edge */
 		ACSR = (1<<ACIS1) | (1<<ACIS0);
 		next_sense = SENSE_A;
 		break;
 
-	case 2: /* A: LOW, B: SENSE, C: PWM */
+	case 2: /* A: LOW, B: SENSE, C: PWM => disable EN_B, disable PWM_A, enable EN_A */
+		PORTD &= ~PHASE_B_EN;
+		TCCR2A &= ~PHASE_A_OC;
 		PORTB |= PHASE_A_EN;
-		PORTD = (PORTD & ~PHASE_B_EN) | PHASE_C_EN;
-		TCCR0A &= ~PHASE_B_OC;
-		TCCR2A = (TCCR2A & ~PHASE_A_OC) | PHASE_C_OC;
 
+		/* B: falling edge */
 		ACSR = (1<<ACIS1);
 		next_sense = SENSE_B;
 		break;
 
-	case 3: /* A: LOW, B: PWM, C: SENSE */
-		PORTB |= PHASE_A_EN;
-		PORTD = (PORTD & ~PHASE_C_EN) | PHASE_B_EN;
+	case 3: /* A: LOW, B: PWM, C: SENSE => disable EN_C, enable PWM_B, enable EN_B */
+		PORTD &= ~PHASE_C_EN;
 		TCCR0A |= PHASE_B_OC;
-		TCCR2A &= ~(PHASE_A_OC | PHASE_C_OC);
+		PORTD |= PHASE_B_EN;
 
+		/* C: rising edge */
 		ACSR = (1<<ACIS1) | (1<<ACIS0);
 		next_sense = SENSE_C;
 		break;
 
-	case 4: /* A: SENSE, B: PWM, C: LOW */
+	case 4: /* A: SENSE, B: PWM, C: LOW => disable EN_A, disable PWM_C, enable EN_C */
 		PORTB &= ~PHASE_A_EN;
-		PORTD = (PORTD & ~PHASE_B_EN) | PHASE_C_EN;
-		TCCR0A |= PHASE_B_OC;
-		TCCR2A &= ~(PHASE_A_OC | PHASE_C_OC);
+		TCCR2A &= ~PHASE_C_OC;
+		PORTD |= PHASE_C_EN;
 
+		/* A: falling edge */
 		ACSR = (1<<ACIS1);
 		next_sense = SENSE_A;
 		break;
 
-	case 5: /* A: PWM, B: SENSE, C: LOW */
+	case 5: /* A: PWM, B: SENSE, C: LOW => disable EN_B, enable PWM_A, enable EN_A */
+		PORTD &= ~PHASE_B_EN;
+		TCCR2A |= PHASE_A_OC;
 		PORTB |= PHASE_A_EN;
-		PORTD = (PORTD & ~PHASE_B_EN) | PHASE_C_EN;
-		TCCR0A &= ~PHASE_B_OC;
-		TCCR2A = (TCCR2A & ~PHASE_C_OC) | PHASE_A_OC;
 
+		/* B: rising edge */
 		ACSR = (1<<ACIS1) | (1<<ACIS0);
 		next_sense = SENSE_B;
 		break;
@@ -228,6 +232,18 @@ void setpwm(uint8_t pwm)
 	OCR0B = pwm;
 	OCR2A = pwm;
 	OCR2B = pwm;
+
+	/* disable all drivers */
+	if (pwm == 0) {
+		PORTB &= ~PHASE_A_EN;
+		PORTD &= ~(PHASE_B_EN | PHASE_C_EN);
+
+		TCCR0A &= ~PHASE_B_OC;
+		TCCR2A &= ~(PHASE_A_OC | PHASE_C_OC);
+
+		PORTB |= PHASE_A_EN;
+		PORTD |= (PHASE_B_EN | PHASE_C_EN);
+	}
 }
 
 ISR(ANALOG_COMP_vect)