diff --git a/fw_test/main.c b/fw_test/main.c
index 4bbcd71..ef81292 100644
--- a/fw_test/main.c
+++ b/fw_test/main.c
@@ -17,17 +17,7 @@ void init_leds(void){
 	DDRC |= (1 << PORTC0) | (1 << PORTC1) | (1 << PORTC2) | (1 << PORTC3);
 	// both LEDs off
 	PORTC &= ~((1 << PORTC0) | (1 << PORTC1) | (1 << PORTC2) | (1 << PORTC3));
-
-
-	 //TCCR2A = (1 << WGM21);
-	  //TCCR2B = (1 << CS20)|(1 << CS21);
-	  //OCR2A = 255; /* TOP */
-	  //	TCNT2 = 0;
-	  //	/*enable interrupt*/
-	  //	TIMSK2 |=  (1<<OCIE2A);
-
-
-  	return;
+ 	return;
 };
 
 void inline led_on(int leds){
@@ -69,9 +59,6 @@ void buzzr_inv(){
 	PORTB ^= (1 << PORTB2);
 };
 
-void init_switch(){
-	return;
-};
 
 void init_motor(void)
 {
@@ -88,39 +75,56 @@ void set_motor(int val){
 };
 
 
+void init_switch(){
+	return;
+};
+
+void blinkrattlebeep(){
+	led_off(LED_L|LED_R);
+	_delay_ms(100);	
+	led_on(LED_L);
+	_delay_ms(100);	
+	led_off(LED_L);
+	led_on(LED_R);
+	_delay_ms(100);
+	led_off(LED_R);	
+	set_motor(MOTOR_ON);
+	_delay_ms(200);
+	set_motor(MOTOR_OFF);
+	buzzr_up();
+	for (int i=0; i<100; i++){
+		_delay_ms(2);
+		buzzr_inv();
+	};
+	buzzr_off();
+};
+
+
 
 void __attribute__((noreturn)) 
 main(void)
 {
+
+	/* hardware initialisation: */
 	init_motor();
 	init_buzzr();
 	init_leds();
+	uint8_t mode = 0;
 
 
-	/* hardware initialisation: */
 
 
 	for(;;) /* ever */  {
 		//do something
-	//	led_off(LED_L|LED_R);
-		_delay_ms(100);	
-		led_on(LED_L);
-		_delay_ms(100);	
-		led_off(LED_L);
-		led_on(LED_R);
-		_delay_ms(100);
-		led_off(LED_R);	
-		set_motor(MOTOR_ON);
-		_delay_ms(200);
-		set_motor(MOTOR_OFF);
-	
-		buzzr_up();
-		for (int i=0; i<100; i++){
-			_delay_ms(2);
-			buzzr_inv();
-	
+
+		//TODO: check switches and change mode
+
+		switch(mode){
+			case 1 : led_on(LED_L|LED_R); _delay_ms(500); led_off(LED_L|LED_R); break;
+			case 2 : break;
+			default : blinkrattlebeep();
 		};
-		buzzr_off();
+
 	}
 	/* never  return 0; */
 }