diff --git a/firmware/main.c b/firmware/main.c
index 755ff58..8b48ed5 100644
--- a/firmware/main.c
+++ b/firmware/main.c
@@ -292,6 +292,7 @@ void do_mode4(void) {
 ;
 void do_mode5(void) {
 
+	static bool sample;
 	static timer_t mytimer;
     uint16_t led1;
     uint16_t led2;
@@ -299,50 +300,46 @@ void do_mode5(void) {
 
 	if (ModeChanged) { //init after mode change
 		ModeChanged = false;
+		ADMUX = (1<<REFS1) | (1<<REFS0); //use internal 1.1V as reference
+		ADCSRA = (1<<ADPS1) | (1<<ADPS0);// prescaler F_CPU/8
+		ADCSRA |= (1<<ADEN); // ADC enable - turn it on
+		// do one conversion
+		ADCSRA |= (1<<ADSC);
+		while (ADCSRA & (1<<ADSC) ) {} //wait for conversion to end
+		uint16_t __attribute__((unused)) dummy = ADCW; //read once
 		timer_set(&mytimer, 10);
+		sample = false;
 	};
+	if (timer_expired(&mytimer)) {
+		if (sample){
+			//make a measurement
+			  // single measurement
+			  ADMUX = (ADMUX & ~(0x1F)) | 1; // select channel 1
+			  ADCSRA |= (1<<ADSC); // start single conversion
+			  while (ADCSRA & (1<<ADSC) ) {}; // wait for conversion to end
+			  led1 =ADCW; // read result
+			  ADMUX = (ADMUX & ~(0x1F)) | 3; // select channel 3
+			  ADCSRA |= (1<<ADSC); // start single conversion
+			  while (ADCSRA & (1<<ADSC) ) {}; // wait for conversion to end
+			  led2 =ADCW; // read result
+ 			USART0_putc('1');USART0_putc(':');USART0_put_uint16(led1);USART0_crlf();
+		     USART0_putc('2');USART0_putc(':');USART0_put_uint16(led2);USART0_crlf();
+		     sample = false;
+		} else {
+			//charge LED
+			//enable LED channels as output
+			DDRC |= (1 << PORTC0) | (1 << PORTC1) | (1 << PORTC2) | (1 << PORTC3);
+			// charge with reverse polarity (C0, C2 = low  C1, C3 = high)
+			PORTC = (PORTC & 0b11110000) | (1 << PORTC1) | (1 << PORTC3);
+			//set C1 and C3 to input (disable pullups)
+			DDRC &= ~( (1 << PORTC1) | (1 << PORTC3));
+			//pull ups off
+			PORTC &= ~( (1 << PORTC1) | (1 << PORTC3));
 
-
-	//check for Buttons
-	if (btn_state(BTNST_SUP, BTN_LEFT)) {
-
-	};
-	if (btn_state(BTNST_SUP, BTN_RIGHT)) {
-	};
-	//if (timer_expired(&mytimer)) {
-	  //charge LEDs...
-	  //enable LED channels as output
-	  DDRC |= (1 << PORTC0) | (1 << PORTC1) | (1 << PORTC2) | (1 << PORTC3);
-
-	  //init variables
-	  led1 =0;
-	  led2 =0;
-
-	  //discharge completely for test purposes
-	  //PORTC = (PORTC & 0b11110000);
-      //wait a while until fully dischcharged ....
-      //for (int i=0; i<100; i++){};
-
-	  // charge with reverse polarity (C0, C2 = low  C1, C3 = high)
-      PORTC = (PORTC & 0b11110000) | (1 << PORTC1) | (1 << PORTC3);
-      //wait a while until fully charged ....
-      //for (int i=0; i<10; i++){};
-
-      //set C1 and C3 to input (disable pullups)
-	  DDRC &= ~( (1 << PORTC1) | (1 << PORTC3));
-	  //pull ups off
-
-	  PORTC &= ~( (1 << PORTC1) | (1 << PORTC3));
-
-	  while ((PINC & 0b00001000)!=0){
-		  if (PINC & 0b00000010) led1++;
-		  //if (PINC & 0b00001000) led2++;
-	  };
-      USART0_putc('1');USART0_putc(':');USART0_put_uint16(led1);USART0_crlf();
-      USART0_putc('2');USART0_putc(':');USART0_put_uint16(led2);USART0_crlf();
-            //music_setNote(led1+1000,0);
-		//timer_set(&mytimer, 1);
-	//}; //end if timer_expired
+		};
+		sample= true;
+		timer_set(&mytimer, 1);
+	}; //end if timer_expired
 
 }
 ;