diff --git a/firmware/main.c b/firmware/main.c
index 4abefea..e724d3f 100644
--- a/firmware/main.c
+++ b/firmware/main.c
@@ -6,56 +6,47 @@
 
 #include "main.h"
 
-
-void init_pwm(void){
-	
+static inline void init_pwm(void)
+{
 	// TCCR1A xx01xx10
 	TCCR1A &= ~0b00100001;
-	TCCR1A |=  0b00000010;
+	TCCR1A |= 0b00000010;
 
 	// TCCR1B xxx11001
 	TCCR1B &= ~0b00000110;
-	TCCR1B |=  0b00011001;
-	
+	TCCR1B |= 0b00011001;
+
 	//PB1 set to output:
 	DDRB |= 0b10;
+	return;
+}
 
+static void init_leds(void)
+{
+	//enable LED channels as output
+	DDRC |= (1 << PORTC0) | (1 << PORTC2) | (1 << PORTC3) | (1 << PORTC1);
+	PORTC = 1;		//one led is on...
+	return;
+}
+
+inline void setleds(uint8_t state)
+{
+	//set leds according to
+	PORTC |= (state | 0 b00001111);
+	PORTC &= ~(state | 0 b11110000);
+	return;
+}
+
+static void init_motor(void)
+{
+	//vibration motor on B1:
+	DDRB |= (1 << PORTB1);
 
 }
 
-
-int main (void)
+static void stupid_pwmtest(void)
 {
-
-	//enable LED channels as output
-	DDRC |= (1<<PORTC0)|(1<<PORTC2)|(1<<PORTC3)|(1<<PORTC1);
-	PORTC = 1; //one led is on...
-
-	//BUZZER IS OUT:
-	DDRD |= (1<<PORTD6);
-
-	//vibration motor 
-	DDRB |= (1<<PORTB1);
-
-	//reduce power
-	PRR |= (1<<PRTWI)|(1<<PRTIM2)|(1<<PRSPI)|(1<<PRUSART0);
-
-	/*PORTB |= (1 << PORTB1);*/
-	PORTD = 0;
-
-	init_pwm();
-
-
-	/*while(1)*/
-	/*{*/
-		/*_delay_ms(500);*/
-		/*PORTC ^= (1<<PORTC0)|(1<<PORTC2);*/
-		/*PORTB ^= (1<<PORTB1);*/
-		/*PORTD ^= (1<< PORTD6);*/
-
-	/*}*/
-
-	for(;;){
+	for (;;) {
 		OCR1BH = 0;
 		OCR1BL = 0;
 		_delay_ms(1);
@@ -66,3 +57,15 @@ int main (void)
 
 }
 
+int main(void)
+{
+
+	init_leds();
+	init_motor();
+	init_pwm();
+
+	stupid_pwmtest();
+
+	//never get here
+	return;
+}