diff --git a/main.c b/main.c
index def83af..2f716ae 100644
--- a/main.c
+++ b/main.c
@@ -39,12 +39,9 @@ void get_mcusr(void)
 }
 //***********Prologue for fast WDT disable & and save reason of reset/power-up: END
 
-
 //FUNC headers
 static void avr_init(void);
-void modbus_master_init(void);
 void timer0_init(void);
-void timer2_init(void);
 void timer3_init(void);
 void print_network_information(void);
 
@@ -168,6 +165,8 @@ int main()
     modbus_master_init();
 
     printf("moin!\n\r");
+    timer3_init();
+    timer4_init();
 
     ioHelperInitBuffer();
     ioHelperIoConf();
@@ -270,25 +269,6 @@ int main()
     return 0;
 }
 
-// Timer0
-// 1ms IRQ
-// Used for millis() timing
-void timer0_init()
-{
-    TCCR0A = (1<<WGM01); //TIMER0 SET-UP: CTC MODE
-    TCCR0B = (1<<CS01)|(1<<CS00); // PS 1:64
-    OCR0A = 249; // 1ms reach for clear (16mz:64=>250kHz:250-=>1kHz)
-    TIMSK0 |= 1<<OCIE0A;	 //IRQ on TIMER0 output compareA
-}
-
-void timer2_init()
-{
-    TCCR2A = (1<<WGM21); //TIMER0 SET-UP: CTC MODE
-    TCCR2B|=(1<<CS21); //prescaler 8
-    OCR2A = 200; // 1ms reach for clear (16mz/8/200=>10kHz)
-	TIMSK2|=(1<<OCIE2A);
-}
-
 void print_network_information()
 {
 
@@ -311,6 +291,4 @@ void print_network_information()
     printf("DNS Server : %d.%d.%d.%d\n\r",gWIZNETINFO.dns[0],gWIZNETINFO.dns[1],gWIZNETINFO.dns[2],gWIZNETINFO.dns[3]);
 }
 
-ISR(TIMER2_COMPA_vect) { //this ISR is called 9765.625 times per second
-	modbusTickTimer();
-}
+
diff --git a/millis.c b/millis.c
index 0b236e0..71e81cd 100644
--- a/millis.c
+++ b/millis.c
@@ -4,6 +4,17 @@
 
 volatile unsigned long _millis; // for millis tick !! Overflow every ~49.7 days
 
+// Timer0
+// 1ms IRQ
+// Used for millis() timing
+void timer0_init()
+{
+    TCCR0A = (1<<WGM01); //TIMER0 SET-UP: CTC MODE
+    TCCR0B = (1<<CS01)|(1<<CS00); // PS 1:64
+    OCR0A = 249; // 1ms reach for clear (16mz:64=>250kHz:250-=>1kHz)
+    TIMSK0 |= 1<<OCIE0A;	 //IRQ on TIMER0 output compareA
+}
+
 ISR (TIMER0_COMPA_vect)
 {
 	_millis++; // INC millis tick
diff --git a/millis.h b/millis.h
index 21fef16..1ad9607 100644
--- a/millis.h
+++ b/millis.h
@@ -3,5 +3,6 @@
 
 #define TICK_PER_SEC 1000UL
 unsigned long millis(void);
+void timer0_init(void);
 
 #endif
diff --git a/taenzer.c b/taenzer.c
index aaac9de..7c88e32 100644
--- a/taenzer.c
+++ b/taenzer.c
@@ -47,8 +47,12 @@ void timer4_init()
     DDRH |= 1 << 3;
 }
 
-//ISR(TIMER4_OVF_vect) {
-//}
+ISR(TIMER4_OVF_vect) {
+    if(ioHelperReadBit(outStates, MOTOR_TAENZER_DIR))
+        taenzer_state.pos -= 1;
+    else
+        taenzer_state.pos += 1;
+}
 
 //ISR(TIMER4_OVF_vect) {
 //}