diff --git a/firmware/input.c b/firmware/input.c
index f80c753..dfa9a6f 100644
--- a/firmware/input.c
+++ b/firmware/input.c
@@ -8,30 +8,38 @@ enum Edge {
 uint16_t volatile last_count;
 enum Edge volatile next_edge;
 
+volatile uint8_t signal_ok;
 
 void input_init() {
+    PORTD |= (1 << PD3);
     MCUCR |= (1 << ISC10) | (1 << ISC11);
     GIMSK |= (1 << INT1);
 
+    TIMSK |= (1 << TOIE1);
+    TCNT1 = 0;
+    TCCR1B |= (1 << CS11); //Prescaler 8, enable Timer
+
     next_edge = RISING;
     last_count = 0;
+    signal_ok = 0;
 }
 
 
-
+ISR(TIMER1_OVF_vect) {
+    signal_ok = 0;
+    PIND |= (1 << PD1);
+}
 
 ISR(INT1_vect) {
+    signal_ok = 1;
+
     if(next_edge == RISING) {
         TCNT1 = 0;
-        TCCR1B |= (1 << CS11); //Prescaler 8, enable Timer
-
         MCUCR &= ~(1 << ISC10); //Trigger on falling edge next
         next_edge = FALLING;
     } else if(next_edge == FALLING) {
         last_count = TCNT1;
-        TCCR1B &= ~(1 << CS11); //Prescaler 8, disable Timer
-
-
+        TCNT1 = 0;
         MCUCR |= (1 << ISC10); //Trigger on falling edge next
         next_edge = RISING;
     }
@@ -47,3 +55,7 @@ int16_t input_get_result() {
     }
     return tmp;
 }
+
+uint8_t input_signal_ok() {
+    return signal_ok;
+}
diff --git a/firmware/input.h b/firmware/input.h
index 41f8ffc..c2f6e01 100644
--- a/firmware/input.h
+++ b/firmware/input.h
@@ -7,6 +7,6 @@
 
 void input_init(void);
 int16_t input_get_result(void);
-
+uint8_t input_signal_ok(void); 
 
 #endif
diff --git a/firmware/main.c b/firmware/main.c
index 1c33597..349b633 100644
--- a/firmware/main.c
+++ b/firmware/main.c
@@ -8,8 +8,6 @@
 
 
 int main(void) {
-	_delay_ms(5000);
-
 	hbridge_init();
 	input_init();
 
@@ -19,20 +17,26 @@ int main(void) {
 	DDRD |= (1 << PD1);
 
 	while(1) {
-
-		int16_t speed = input_get_result();
-
-		if(speed > 20) {
-			pwm_cycle_forwards(speed / 2);
-		} else if (speed < -20){
-			pwm_cycle_backwards(-speed / 2);
-		} else {
-			pwm_cycle_coast();
+		while(!input_signal_ok()) {
+			pwm_cycle_break();
 		}
 
-		PIND |= (1 << PD1);
+		while(input_get_result() < -20 || input_get_result() > 20) {
+			pwm_cycle_break();
+		}
 
+
+		while(input_signal_ok()) {
+			int16_t speed = input_get_result();
+
+			if(speed > 20) {
+				pwm_cycle_forwards(speed / 2);
+			} else if (speed < -20){
+				pwm_cycle_backwards(-speed / 2);
+			} else {
+				pwm_cycle_break();
+			}
+		}
 	}
 
-
 }
diff --git a/firmware/pwm.h b/firmware/pwm.h
index 8392ec5..e9ea7d7 100644
--- a/firmware/pwm.h
+++ b/firmware/pwm.h
@@ -43,6 +43,7 @@ const double delay_time = 0.25;
 
 static inline void pwm_delay(uint16_t slices) {
 	for(uint16_t i = 0; i < slices; i++) {
+		//Anything smaller here will lead to messed up timings
 		_delay_us(10);
 	}
 }
@@ -104,7 +105,7 @@ static inline void pwm_cycle_forwards(uint8_t speed) {
 	hbridge_unset_l2();
 }
 
-static inline void pwm_cycle_coast() {
+static inline void pwm_cycle_break() {
 	hbridge_unset_h1();
 	hbridge_unset_h2();
 	hbridge_set_l1();