diff --git a/blmc.c b/blmc.c
index 22aedfc..410f678 100644
--- a/blmc.c
+++ b/blmc.c
@@ -31,8 +31,8 @@ static uint8_t next_sense;
 
 void trigger_adc(uint8_t channel)
 {
-	/* Disable Analog Comperator */
-	ACSR &= ~(1<<ACIE);
+	/* Disable Analog Comperator Interrupt */
+	TIMSK1 &= ~(1<<ICIE1);
 
 	/* set channel (external reference, 5V) */
 	ADMUX = (0<<REFS1) | (0<<REFS0) | channel;
@@ -50,8 +50,8 @@ void next_phase(void)
 	static uint8_t phase;
 	static uint8_t phase_adc;
 
-	/* Disable Analog Comperator */
-	ACSR &= ~(1<<ACIE);
+	/* Disable Analog Comperator Interrupt */
+	TIMSK1 &= ~(1<<ICIE1);
 
 	TCNT0 = 0x00;
 	TCNT2 = 0x00;
@@ -63,7 +63,7 @@ void next_phase(void)
 		PORTD |= PHASE_B_EN;
 
 		/* C: falling edge */
-		ACSR = (1<<ACIS1);
+		TCCR1B &= ~(1<<ICES1);
 		next_sense = SENSE_C;
 		break;
 
@@ -73,7 +73,7 @@ void next_phase(void)
 		PORTD |= PHASE_C_EN;
 
 		/* A: rising edge */
-		ACSR = (1<<ACIS1) | (1<<ACIS0);
+		TCCR1B |= (1<<ICES1);
 		next_sense = SENSE_A;
 		break;
 
@@ -83,7 +83,7 @@ void next_phase(void)
 		PORTB |= PHASE_A_EN;
 
 		/* B: falling edge */
-		ACSR = (1<<ACIS1);
+		TCCR1B &= ~(1<<ICES1);
 		next_sense = SENSE_B;
 		break;
 
@@ -93,7 +93,7 @@ void next_phase(void)
 		PORTD |= PHASE_B_EN;
 
 		/* C: rising edge */
-		ACSR = (1<<ACIS1) | (1<<ACIS0);
+		TCCR1B |= (1<<ICES1);
 		next_sense = SENSE_C;
 		break;
 
@@ -103,7 +103,7 @@ void next_phase(void)
 		PORTD |= PHASE_C_EN;
 
 		/* A: falling edge */
-		ACSR = (1<<ACIS1);
+		TCCR1B &= ~(1<<ICES1);
 		next_sense = SENSE_A;
 		break;
 
@@ -113,7 +113,7 @@ void next_phase(void)
 		PORTB |= PHASE_A_EN;
 
 		/* B: rising edge */
-		ACSR = (1<<ACIS1) | (1<<ACIS0);
+		TCCR1B |= (1<<ICES1);
 		next_sense = SENSE_B;
 		break;
 	}
@@ -136,8 +136,10 @@ void next_phase(void)
 		ADMUX = next_sense;
 
 		/* enable Analog Comparator with Interrupts */
-		if (blmc.flags & FLAG_COM_NORMAL)
-			ACSR |= (1<<ACIE) | (1<<ACI);
+		if (blmc.flags & FLAG_COM_NORMAL) {
+			TIFR1 |= (1<<ICF1);
+			TIMSK1 |= (1<<ICIE1);
+		}
 	}
 
 	phase++;
@@ -246,7 +248,7 @@ void setpwm(uint8_t pwm)
 	}
 }
 
-ISR(ANALOG_COMP_vect)
+ISR(TIMER1_CAPT_vect)
 {
 	next_phase();
 }
@@ -266,8 +268,10 @@ ISR(ADC_vect)
 	ADCSRA = 0x00;
 
 	/* enable Analog Comparator with Interrupts */
-	if (blmc.flags & FLAG_COM_NORMAL)
-		ACSR |= (1<<ACIE) | (1<<ACI);
+	if (blmc.flags & FLAG_COM_NORMAL) {
+		TIFR1 |= (1<<ICF1);
+		TIMSK1 |= (1<<ICIE1);
+	}
 
 	if (channel == SENSE_CURRENT) {
 		current_tmp += value;
diff --git a/main.c b/main.c
index a61160d..afea18f 100644
--- a/main.c
+++ b/main.c
@@ -45,13 +45,13 @@ extern struct blmc_ blmc;
 
 static uint8_t led[2];
 
-ISR(TIMER1_OVF_vect)
+ISR(TIMER1_COMPB_vect)
 {
 	static uint8_t rpm_cnt = 0;
 	static uint8_t adc_chan = SENSE_CURRENT;
 
 	/* Come back in 20ms */
-	TCNT1 = (0xFFFF - 20000);
+	OCR1B = TCNT1 + 20000;
 
 	/* commutations during last 20ms */
 	uint16_t diff = blmc.rpm_tmp - blmc.rpm_tmp_old;
@@ -158,10 +158,11 @@ int main(void)
 	TCCR2B = (1<<CS20);
 
 	/* enable Timer1 OVF Interrupt */
-	TIMSK1 = (1<<TOIE1);
+	TIMSK1 = (1<<OCIE1B);
 
 	/* Enable Analog Comparator Multiplexer */
 	ADCSRB |= (1<<ACME);
+	ACSR |= (1<<ACIC);
 
 	/* I2C Init: keep Address from bootloader, Auto ACKs with Interrupts */
 	TWCR = (1<<TWEA) | (1<<TWEN) | (1<<TWIE);