diff --git a/heater.c b/heater.c
index 62d5981..d6c1282 100644
--- a/heater.c
+++ b/heater.c
@@ -93,6 +93,61 @@ EE_factor EEMEM EE_factors[NUM_HEATERS];
 /// Set directions, initialise PWM timers, read PID factors from eeprom, etc
 void heater_init() {
 	heater_t i;
+
+	// setup PWM timers: fast PWM
+	// Warning 2012-01-11: these are not consistent across all AVRs
+	TCCR0A = MASK(WGM01) | MASK(WGM00);
+	// PWM frequencies in TCCR0B, see page 108 of the ATmega644 reference.
+	TCCR0B = MASK(CS00); // F_CPU / 256 (about 78(62.5) kHz on a 20(16) MHz chip)
+	#ifndef FAST_PWM
+		TCCR0B = MASK(CS00) | MASK(CS02); // F_CPU / 256 / 1024  (about 76(61) Hz)
+	#endif
+	TIMSK0 = 0;
+	OCR0A = 0;
+	OCR0B = 0;
+
+	// timer 1 is used for stepping
+
+	TCCR2A = MASK(WGM21) | MASK(WGM20);
+	// PWM frequencies in TCCR2B, see page 156 of the ATmega644 reference.
+	TCCR2B = MASK(CS20); // F_CPU / 256  (about 78(62.5) kHz on a 20(16) MHz chip)
+	#ifndef FAST_PWM
+		TCCR2B = MASK(CS20) | MASK(CS21) | MASK(CS22); // F_CPU / 256 / 1024
+	#endif
+	TIMSK2 = 0;
+	OCR2A = 0;
+	OCR2B = 0;
+
+	#ifdef	TCCR3A
+		TCCR3A = MASK(WGM30);
+		TCCR3B = MASK(WGM32) | MASK(CS30);
+		TIMSK3 = 0;
+		OCR3A = 0;
+		OCR3B = 0;
+	#endif
+
+	#ifdef	TCCR4A
+		#ifdef TIMER4_IS_10_BIT
+			// ATmega16/32U4 fourth timer is a 10 special bit timer
+			TCCR4D = MASK(WGM40);
+			TCCR4B = MASK(CS40);
+		#else
+			TCCR4A = MASK(WGM40);
+			TCCR4B = MASK(WGM42) | MASK(CS40);
+		#endif
+		TIMSK4 = 0;
+		OCR4A = 0;
+		OCR4B = 0;
+	#endif
+
+	#ifdef	TCCR5A
+		TCCR5A = MASK(WGM50);
+		TCCR5B = MASK(WGM52) | MASK(CS50);
+		TIMSK5 = 0;
+		OCR5A = 0;
+		OCR5B = 0;
+	#endif
+
 	// setup pins
 	for (i = 0; i < NUM_HEATERS; i++) {
 		if (heaters[i].heater_pwm) {
@@ -180,6 +235,14 @@ void heater_init() {
 			}
 		#endif /* BANG_BANG */
 	}
+
+	// set all heater pins to output
+	do {
+		#undef	DEFINE_HEATER
+		#define	DEFINE_HEATER(name, pin) WRITE(pin, 0); SET_OUTPUT(pin);
+			#include "config.h"
+		#undef DEFINE_HEATER
+	} while (0);
 }
 
 /// \brief Write PID factors to eeprom
diff --git a/mendel.c b/mendel.c
index 3e66890..bbf6be6 100644
--- a/mendel.c
+++ b/mendel.c
@@ -184,78 +184,10 @@ void io_init(void) {
 		SET_OUTPUT(E_ENABLE_PIN);
 	#endif
 
-	// setup PWM timers: fast PWM
-	// Warning 2012-01-11: these are not consistent across all AVRs
-	TCCR0A = MASK(WGM01) | MASK(WGM00);
-	// PWM frequencies in TCCR0B, see page 108 of the ATmega644 reference.
-	TCCR0B = MASK(CS00); // F_CPU / 256 (about 78(62.5) kHz on a 20(16) MHz chip)
-	//TCCR0B = MASK(CS01);              // F_CPU / 256 / 8  (about 9.8(7.8) kHz)
-	//TCCR0B = MASK(CS00) | MASK(CS01); // F_CPU / 256 / 64  (about 1220(977) Hz)
-	//TCCR0B = MASK(CS02);              // F_CPU / 256 / 256  (about 305(244) Hz)
-	#ifndef FAST_PWM
-		TCCR0B = MASK(CS00) | MASK(CS02); // F_CPU / 256 / 1024  (about 76(61) Hz)
-	#endif
-	TIMSK0 = 0;
-	OCR0A = 0;
-	OCR0B = 0;
-
-	TCCR2A = MASK(WGM21) | MASK(WGM20);
-	// PWM frequencies in TCCR2B, see page 156 of the ATmega644 reference.
-	TCCR2B = MASK(CS20); // F_CPU / 256  (about 78(62.5) kHz on a 20(16) MHz chip)
-	//TCCR2B = MASK(CS21);              // F_CPU / 256 / 8  (about 9.8(7.8) kHz)
-	//TCCR2B = MASK(CS20) | MASK(CS21); // F_CPU / 256 / 32  (about 2.4(2.0) kHz)
-	//TCCR2B = MASK(CS22);              // F_CPU / 256 / 64  (about 1220(977) Hz)
-	//TCCR2B = MASK(CS20) | MASK(CS22); // F_CPU / 256 / 128  (about 610(488) Hz)
-	//TCCR2B = MASK(CS21) | MASK(CS22); // F_CPU / 256 / 256  (about 305(244) Hz)
-	#ifndef FAST_PWM
-		TCCR2B = MASK(CS20) | MASK(CS21) | MASK(CS22); // F_CPU / 256 / 1024
-	#endif
-	TIMSK2 = 0;
-	OCR2A = 0;
-	OCR2B = 0;
-
-	#ifdef	TCCR3A
-		TCCR3A = MASK(WGM30);
-		TCCR3B = MASK(WGM32) | MASK(CS30);
-		TIMSK3 = 0;
-		OCR3A = 0;
-		OCR3B = 0;
-	#endif
-
-	#ifdef	TCCR4A
-		#ifdef TIMER4_IS_10_BIT
-			// ATmega16/32U4 fourth timer is a 10 special bit timer
-			TCCR4D = MASK(WGM40);
-			TCCR4B = MASK(CS40);
-		#else
-			TCCR4A = MASK(WGM40);
-			TCCR4B = MASK(WGM42) | MASK(CS40);
-		#endif
-		TIMSK4 = 0;
-		OCR4A = 0;
-		OCR4B = 0;
-	#endif
-
-	#ifdef	TCCR5A
-		TCCR5A = MASK(WGM50);
-		TCCR5B = MASK(WGM52) | MASK(CS50);
-		TIMSK5 = 0;
-		OCR5A = 0;
-		OCR5B = 0;
-	#endif
-
 	#ifdef	STEPPER_ENABLE_PIN
 		power_off();
 	#endif
 
-	// set all heater pins to output
-	do {
-		#undef	DEFINE_HEATER
-		#define	DEFINE_HEATER(name, pin) WRITE(pin, 0); SET_OUTPUT(pin);
-			#include "config.h"
-		#undef DEFINE_HEATER
-	} while (0);
-
 	#ifdef	TEMP_MAX6675
 		// setup SPI
 		WRITE(SCK, 0);				SET_OUTPUT(SCK);