diff --git a/firmware/main.c b/firmware/main.c
index bf74a3e..402618d 100644
--- a/firmware/main.c
+++ b/firmware/main.c
@@ -137,7 +137,10 @@ void __attribute__ ((noreturn)) main(void)
 	for ever {
 		//main polling loop;
 		button_poll();
-		modeswitch_poll();
+
+		if(OpMode < mode_num) {
+			modeswitch_poll();
+		}
 
 		if (OpMode != NextMode){
 			mode_last_tick = true;