Put SW button cube turn off back

ESP/uart/uart.ino

@@ -190,7 +190,7 @@ void esp_cmd_change() {
 void setup() {
   Serial.begin(115200);
   pinMode(CMD_PIN, INPUT);
-  
+
   pinMode(LED_PIN, OUTPUT);
   cmd_pin = digitalRead(CMD_PIN);
   cmd_pin_prev = cmd_pin;
@@ -230,7 +230,7 @@ void loop() {
         if (message_idx == 0) {
           if (check_commands(message_rx[0])) {
             message_idx++;
-          } 
+          }
         } else if (message_idx == 1) {
           message_idx++;
         } else if (message_idx == message_rx[1] + 2) {
@@ -239,8 +239,6 @@ void loop() {
             switch (message_rx[0]) {
               case COM_NAME:
                 send_short_cmd(COM_ACK);
-
-
                 memset(esp_name, '\0', sizeof(esp_name));
                 memcpy(esp_name, &message_rx[2], message_rx[1]);
                 if (bt_on) {
@@ -254,7 +252,7 @@ void loop() {
                 send_short_cmd(COM_ACK);
                 memset(esp_pass, '\0', sizeof(esp_pass));
                 memcpy(esp_pass, &message_rx[2], message_rx[1]);
-                if(!bt_on) {
+                if (!bt_on) {
                   WiFi.softAP(esp_name, esp_pass);
                 }
                 break;
@@ -336,48 +334,45 @@ void loop() {
       client = server.available();
       if (client)
       {
-        
-      
-
-      while (client.connected() && !cmd_pin)
-      {
-
-        if (client.available())
+        while (client.connected() && !cmd_pin)
         {
-          char c = client.read();
-          request += c;
 
-          if (c == '\n')
+          if (client.available())
           {
-            if (request.indexOf("GET /STOP") != -1)
-            {
-              dir = STOP;
-            }
+            char c = client.read();
+            request += c;
 
-            if (request.indexOf("GET /UP") != -1)
-            {
-              dir = UP;
-            }
-            if (request.indexOf("GET /DOWN") != -1)
-            {
-              dir = DOWN;
-            }
-            if (request.indexOf("GET /LEFT") != -1)
-            {
-              dir = LEFT;
-            }
-            if (request.indexOf("GET /RIGHT") != -1)
+            if (c == '\n')
             {
-              dir = RIGHT;
+              if (request.indexOf("GET /STOP") != -1)
+              {
+                dir = STOP;
+              }
+
+              if (request.indexOf("GET /UP") != -1)
+              {
+                dir = UP;
+              }
+              if (request.indexOf("GET /DOWN") != -1)
+              {
+                dir = DOWN;
+              }
+              if (request.indexOf("GET /LEFT") != -1)
+              {
+                dir = LEFT;
+              }
+              if (request.indexOf("GET /RIGHT") != -1)
+              {
+                dir = RIGHT;
+              }
+              html();
+              break;
             }
-            html();
-            break;
           }
         }
-      }
 
-      request = "";
-      client.stop();
+        request = "";
+        client.stop();
       }
     }
   }

main/main.py

@@ -59,9 +59,15 @@ def main():
             robot.display.text('no', 10, 44, 1)
             robot.display.text('ok', 100,44, 1)
 
-    def esp_command(robot):
+    def esp_command_on(robot):
+        robot.esp_command = True
         robot.buttons.set_pin(8, False)
-        utime.sleep(0.05)
+        utime.sleep(0.01)
+
+    def esp_command_off(robot):
+        robot.buttons.set_pin(8, True)
+        utime.sleep(0.01)
+        robot.esp_command = False
 
     # Analyze pressed buttons for menu movement 
     def buttons(robot, button_values, robotState, menu_move, menu_debounce): 
@@ -87,10 +93,10 @@ def main():
         return menu_move, menu_debounce, ok_pressed
     
     def send_esp_req(robot):
-        esp_command(robot)
+        esp_command_on(robot)
         pin = robot.esp.req_bt_pin()
         robot.esp.flush()
-        robot.buttons.set_pin(8, True)
+        esp_command_off(robot)
         print("Pin: ", pin)
         if pin is not None and pin != -1:
             bt_pair = True
@@ -110,7 +116,7 @@ def main():
     rejected_pin = 0
     bt_pair = False
     button_values = robot.buttons.pressed()
-    robot.esp_command = True
+    
     bt_connecting = False
 
     display_show_startup(robot)
@@ -138,15 +144,15 @@ def main():
     while(True):
         if not robot.esp.running() and counter > 0 and counter % 20 == 0:
             robot.buttons.set_pin(14, False)
-            esp_command(robot)
+            esp_command_on(robot)
             reset = robot.esp.reset(100)
             print("Reset: ", reset)
-            robot.buttons.set_pin(14, True)
             if reset:
                 utime.sleep(0.5)
                 robot.esp.flush()
                 print("Name: ", robot.esp.set_name("Open-Cube5",300))
-            robot.buttons.set_pin(8, True)
+            robot.buttons.set_pin(14, True)
+            esp_command_off(robot)
 
         bat_voltage = robot.battery.voltage() # Get current battery voltage
         button_values = robot.buttons.pressed() # Get buttons state
@@ -167,18 +173,18 @@ def main():
         if bt_pair:
             display_show_bt_pin(robot, pin, bt_connecting)
             if button_values[Button.RIGHT]:
-                esp_command(robot)
+                esp_command_on(robot)
                 print("pair", robot.esp.repeat_cmd(robot.esp.COM_BT_PAIR, 300)==robot.esp.ACK)
                 robot.esp.flush()
-                robot.buttons.set_pin(8, True)
+                esp_command_off(robot)
                 
                 bt_connecting = True
             elif button_values[Button.LEFT]:
                 rejected_pin = pin
-                esp_command(robot)
+                esp_command_on(robot)
                 print("cancel", robot.esp.repeat_cmd(robot.esp.COM_BT_CANCEL, 300)==robot.esp.ACK)
                 robot.esp.flush()
-                robot.buttons.set_pin(8, True)
+                esp_command_off(robot)
                 
             robot.display.show()
             counter += 1

programs/examples/esp_wifi.py

@@ -8,8 +8,9 @@ def main():
     run = True
     while run:
         time = utime.ticks_us()
+        robot.esp_command = True
         robot.buttons.set_pin(8, False)
-        utime.sleep(0.02)
+        utime.sleep(0.01)
         wifi = (robot.esp.repeat_cmd(Esp.COM_WIFI, 50) == Esp.ACK)
         print("Wifi: ", wifi)
         utime.sleep(0.5)
@@ -23,6 +24,8 @@ def main():
 
         
         robot.buttons.set_pin(8, True)
+        utime.sleep(0.01)
+        robot.esp_command = False
         while run:
             buttons = robot.buttons.pressed()
             if buttons[Button.LEFT]: