diff --git a/firmware/Makefile b/firmware/Makefile
index bed7b54..6a7a846 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -1,6 +1,6 @@
 DEFINES += -DF_CPU=16000000
 CFLAGS += -save-temps
-CFLAGS += -std=gnu99 # implements C99, for <util/atomic.h>
+CFLAGS += -std=gnu99 -Wall # implements C99, for <util/atomic.h>
 # this removes dead code and does global linker optimization
 #CFLAGS += -ffunction-sections -Wl,--gc-sections -Wl,--relax 
 
diff --git a/firmware/display.c b/firmware/display.c
index ec56c27..3b9b18e 100644
--- a/firmware/display.c
+++ b/firmware/display.c
@@ -255,10 +255,6 @@ void display_bar(int8_t value, int8_t min, int8_t max) {
 	}
 }
 
-
-
-
-
 void updateTemperature() {
   if(newThermoData==1) {
     newThermoData = 0;
diff --git a/firmware/display.h b/firmware/display.h
index 6447319..f65a79b 100644
--- a/firmware/display.h
+++ b/firmware/display.h
@@ -5,6 +5,7 @@
 #include "lcd/lcd.h"
 #include "main.h"
 
+
 #define LCD_CHAR_HALFBAR 0x01
 #define LCD_CHAR_BAR     0x02
 #define LCD_CHAR_DEGREE  0x03
@@ -33,6 +34,6 @@ extern void display_puthex(uint8_t outbyte);
 extern void display_bar(int8_t value, int8_t min, int8_t max);
 extern void display_putint(int16_t number);
 
-void updateTemperature();
+extern void updateTemperature();
 
 #endif
diff --git a/firmware/main.c b/firmware/main.c
index 4e0dda3..5f8b085 100644
--- a/firmware/main.c
+++ b/firmware/main.c
@@ -1,10 +1,10 @@
 #include "main.h"
 
-#define SOFTTIMERNUMS 4
+volatile uint16_t timer1_acc;
 uint16_t softtimer_last[SOFTTIMERNUMS];
 
 void softtimer_reset(uint8_t timernum){
-    softtimer_last[SOFTTIMERNUMS] = timer1_acc;
+    softtimer_last[timernum] = timer1_acc;
 }
 
 uint8_t softtimer(uint8_t timernum, uint16_t interval){
@@ -16,6 +16,7 @@ uint8_t softtimer(uint8_t timernum, uint16_t interval){
     return(0);
 }
 
+// SOFTTIMER( <YOUR TIMER NUM>, <YOUR INTERVAL>);
 #define SOFTTIMER(n,a) if(softtimer((n),(a*4)))
 
 
@@ -35,6 +36,9 @@ void hardinit() {
 
   OCR1A = 15; // contrast
   OCR1B = 50; // brightness
+
+  // enable softtimer isr
+  TIMSK1 |= _BV(TOIE1);
 	
   // init LCD:
   lcd_init(1);
@@ -51,27 +55,25 @@ void softinit() {
 
 
 int __attribute__((noreturn)) main(void) {
-  int i=0;
   hardinit();
   softinit();
   usbInit();
 
-  display_puts("Hallo, Welt!\n\n");
-  display_update();
-
-  
   for(;;){
     usbPoll();
 
-    SOFTTIMER(1,100){
-        updateTemperature();
+    SOFTTIMER(2,500) {     
+      thermoData[0]=thermoData[0]+5;
+      thermoData[1]=thermoData[1]+15;
+      thermoData[2]=thermoData[2]+7;
+      thermoData[3]=thermoData[3]+18;
+      newThermoData = 1;
+    }
+
+    SOFTTIMER(1,100) {
+      updateTemperature();
     }
 
-    thermoData[0]=thermoData[0]+5;
-    thermoData[1]=thermoData[1]+15;
-    thermoData[2]=thermoData[2]+7;
-    thermoData[3]=thermoData[3]+18;
-    i++; if(i%200 == 0) newThermoData = 1;
   }
 }
 
@@ -82,16 +84,18 @@ int __attribute__((noreturn)) main(void) {
   asm volatile ("out %1, %0\n" : "=r" (sreg_store) : "I" (_SFR_IO_ADDR(SREG)));
   reti();
   }*/
+
 ISR(TIMER1_OVF_vect,ISR_NOBLOCK){
-    uint16_t temp;
-    temp=timer1_acc;
-    temp++;
-    ATOMIC_BLOCK(ATOMIC_FORCEON){ 
-        timer1_acc=temp;
-    }
+    uint16_t tmp;
+    tmp=timer1_acc;
+    tmp++;
+
     /* the ATOMIC is acutally only needed if timer1_acc is never read from an ISR, which
      * is probably the case.
      * ATOMIC_FORCEON: the ISR_NOBLOCK sets sei() a few cycles before.
      */
-}
+    ATOMIC_BLOCK(ATOMIC_FORCEON){ 
+        timer1_acc=tmp;
+    }
 
+}
diff --git a/firmware/main.h b/firmware/main.h
index 213c57f..5008b1c 100644
--- a/firmware/main.h
+++ b/firmware/main.h
@@ -26,11 +26,10 @@
 #include "spi.h"
 #include "usb.h"
 
-
 static uint8_t newThermoData = 1;
 static uint16_t thermoData[] = {1024, 814, 2475, 2243};
-volatile uint16_t timer1_acc;
 
+#define SOFTTIMERNUMS 4
 
 #define LED1PORT PORTC
 #define LED1APIN 0