diff --git a/controller/controller.ino b/controller/controller.ino
index 42735d8..3881abd 100644
--- a/controller/controller.ino
+++ b/controller/controller.ino
@@ -326,6 +326,7 @@ void handleModeChange() {
                   digitalWrite(PIN_RELAISFRONT,HIGH); //simulate hoverboard power button press
                   state_modechange++;
                   state_modechange_time=loopmillis; //set to current time
+                  Serial.println("PIN_RELAISFRONT,HIGH");
                 }
                 break;
               case 1:
@@ -333,6 +334,7 @@ void handleModeChange() {
                   digitalWrite(PIN_RELAISFRONT,LOW); //release simulated button
                   state_modechange++;
                   state_modechange_time=loopmillis; //set to current time
+                  Serial.println("PIN_RELAISFRONT,LOW");
                 }
                 break;
               case 2:
@@ -340,6 +342,7 @@ void handleModeChange() {
                   digitalWrite(PIN_RELAISREAR,HIGH); //simulate hoverboard power button press
                   state_modechange++;
                   state_modechange_time=loopmillis; //set to current time
+                  Serial.println("PIN_RELAISREAR,HIGH");
                 }
                 break;
               case 3:
@@ -347,9 +350,17 @@ void handleModeChange() {
                   digitalWrite(PIN_RELAISREAR,LOW); //release simulated button
                   state_modechange++;
                   state_modechange_time=loopmillis; //set to current time
+                  Serial.println("PIN_RELAISREAR,LOW");
                 }
                 break;
-              case 4: 
+              case 4:
+                if (loopmillis - state_modechange_time > 1000) { //wait some time after turning on/off
+                  state_modechange++;
+                  state_modechange_time=loopmillis; //set to current time
+                  Serial.println("Waiting finished");
+                }
+                break;
+              case 5: 
                 // ### Request On ###
                 if (requestmode==on) {//wait for both boards to send feedback
                   state_modechange++;
@@ -360,6 +371,7 @@ void handleModeChange() {
                   state_modechange++;
                   board1Enabled=false; //assume board is offline
                   board2Enabled=false; //assume board is offline
+                  Serial.println("finished");
                 }else{ //if changed off from error mode
                   state_modechange++;
                 }
@@ -393,7 +405,7 @@ void handleModeChange() {
 void modeloops() {
   if (loopmillis - last_looptime >= LOOPTIME) {
     last_looptime=loopmillis;
-    switch (requestmode) { //mode changes
+    switch (currentmode) { //mode changes
       case booting: 
         //TODO: LED effect
         break;