Add error console printing and file logging

lib/cube/esp_config.py

@@ -234,6 +234,14 @@ class Esp:
         self.cmd_stop()
         return False
     
+    def reset_non_blocking(self):
+        self.cmd_start()
+        self.esp_mode_prev = Esp.MODE_BT
+        self.flush()
+        self.send_reset()
+        self.esp_running = True
+        self.cmd_stop()
+
     def bt(self):
         if self.esp_mode == Esp.MODE_WIFI:
             self.esp_mode = Esp.MODE_BT
@@ -425,8 +433,8 @@ class Esp:
             self.header = struct.unpack('<B', b_ch)[0]
             self.message_idx += 1
             if (self.esp_mode == Esp.MODE_WIFI 
-                and self.header != COM_WIFI_BUTTONS 
-                and self.header != COM_WIFI_SWITCHES):
+                and self.header != Esp.COM_WIFI_BUTTONS 
+                and self.header != Esp.COM_WIFI_SWITCHES):
                 self.flush()
         elif self.message_idx == 1:
             self.length = struct.unpack('<B', b_ch)[0]

lib/robot.py

@@ -62,7 +62,7 @@ class Robot:
                 self.cube_startup = False
         elif pressed[Button.POWER]:
             self.display.fill(0)
-            self.display.text('Turning off', 15, 30, 1)
+            self.display.centered_text('Turning off', 30, 1)
             self.display.show()
             sleep(0.3)
             self.turn_off_pin.value(1)
@@ -174,7 +174,7 @@ class Sensors:
                         self.sound[port] = nxt.SoundSensor(port)
                     elif sensor_type ==  Sensor.EV3_COLOR:
                         self.light[port] = ev3.ColorSensor(port)
-                    elif sensor_type ==  Sensor.EV3_TOUCH:
+                    elif sensor_type ==  Sensor.EV3_TOUCH or sensor_type == Sensor.OC_TOUCH:
                         self.touch[port] = ev3.TouchSensor(port)
                     elif sensor_type ==  Sensor.EV3_INFRARED:
                         self.infrared[port] = ev3.InfraredSensor(port)

lib/robot_consts.py

@@ -24,6 +24,7 @@ class Sensor:
     OC_COLOR: Sensor = const(14)
     OC_ULTRASONIC: Sensor = const(15)
     OC_GYRO: Sensor = const(16)
+    OC_TOUCH: Sensor = const(17)
 
 class Port:
     M1: Port = const(0)

main/main.py

@@ -37,6 +37,8 @@ def main():
     # File and directory constants for browsing user programs
     FILE = 0x8000
     DIRECTORY = 0x4000
+    FILE_SIZE_IDX = 6
+    MAX_ERROR_LOG_SIZE = 10000
     DISPLAY_TITLE_POS = 18
     DISPLAY_PROG_POS = 45
     DISPLAY_ARROW_GAP = 10
@@ -62,7 +64,7 @@ def main():
             robot.display.centered_text(program_list[current_program][0][:-3], DISPLAY_PROG_POS, 1)
             display_fill_arrows(robot)
         else:
-            robot.display.text("cube is empty", 0, 30, 1)
+            robot.display.centered_text("cube is empty", DISPLAY_PROG_POS, 1)
     
     # Fill framebuffer with bt menu
     def display_fill_bt_menu(robot, esp_default_name):
@@ -190,6 +192,19 @@ def main():
             # if error occurs in user program
             sys.print_exception(e)
             display_show_error(robot)
+            # Write error to error log file
+            log_deleted = False
+            if "error_log.txt" in os.listdir(".") and os.stat("error_log.txt")[FILE_SIZE_IDX] > MAX_ERROR_LOG_SIZE:
+                os.remove("error_log.txt")
+                log_deleted = True
+            with open("error_log.txt", "a") as f:
+                if log_deleted:
+                    print("Previous log too long, file deleted")
+                    f.write("Previous log too long, file deleted\n")
+                    f.write("--------------------------------------------------\n")
+                f.write(f"ERROR program: {program_name}, time since reset: {utime.time()}\n")
+                sys.print_exception(e, f)
+                f.write("--------------------------------------------------\n")
             utime.sleep(1.0)
         finally:
             # Deinit all sensors, motors and uart if initialized by user program 
@@ -229,19 +244,25 @@ def main():
                 for entry2 in os.ilistdir("./programs/"+entry[0]):
                     if entry2[1] == FILE and entry2[0] == "main.py":
                         program_list.append((entry[0], DIRECTORY))
+        
     except OSError:
          # Create program directory if this is a new installation
         os.mkdir("./programs")
-    menu_list_size[Menu.PROGRAM] = len(program_list)
+        print("Program directory created")
+    if len(program_list) == 0:
+        print("Cube is empty, no programs found")
+        menu_list_size[Menu.PROGRAM] = 1
+    else:
+        menu_list_size[Menu.PROGRAM] = len(program_list)
 
     # Loop that shows display, controls menu and starts user programs
     while(True):
         # Connect to and reset ESP32 to BT mode if it is not running
         if not robot.esp.running() and counter % 20 == 0:
-            reset = robot.esp.reset(100)
-            if reset:
-                robot.esp.set_name(esp_default_name,100)
-                print("ESP BT name: ", esp_default_name)
+            robot.esp.reset_non_blocking()
+        if robot.esp.running() and counter == 1000:
+            robot.esp.set_name(esp_default_name,100)
+            print("ESP BT name: ", esp_default_name)
 
         bat_voltage = robot.battery.voltage() # Get current battery voltage
         button_values = robot.buttons.pressed() # Get buttons state
@@ -303,6 +324,7 @@ def main():
                 program_name = f"Motor M{chosen_motor_port+1}"
                 program_path = "./menu_programs/motor.py"
                 menu_programs.motor_run(robot, chosen_motor_port)
+                robot.deinit_all()
                 robot_state = 0
                 menu_debounce = 1
                 continue
@@ -312,9 +334,11 @@ def main():
             display_fill_sen_test(robot, sensor_test_menu_port, sensor_test_menu, sensor_test_menu_stype)
             if ok_pressed:
                 if sensor_test_menu == Menu.SEN_TEST_PROGRAM:
+                    print("test program")
                     if sensor_test_menu_stype == MENU_PROGRAM_NXT_UTZ_IDX:
                         robot_state = 1
                         menu_programs_functions[sensor_test_menu_stype][1](robot)
+                        robot.deinit_all()
                         robot_state = 0
                         menu_debounce = 1
                         continue
@@ -324,6 +348,7 @@ def main():
                 elif sensor_test_menu == Menu.SEN_TEST_PORT:
                     robot_state = 1
                     menu_programs_functions[sensor_test_menu_stype][1](robot, sensor_test_menu_port)
+                    robot.deinit_all()
                     sensor_test_menu == Menu.SEN_TEST_PROGRAM
                     sensor_test_menu_port = 0
                     robot_state = 0
@@ -340,6 +365,7 @@ def main():
                 elif menu_list[current_menu] == 1:
                     print("Utility tets")
                     menu_programs.cube_utility_run(robot)
+                robot.deinit_all()
                 robot_state = 0
                 menu_debounce = 1
                 continue

menu_programs/nxt_light.py

@@ -27,9 +27,9 @@ def nxt_light_run(robot, chosen_sensor_port):
 
         robot.display.fill(0)
         robot.display.centered_text(f"NXT Light S{chosen_sensor_port+1}", 0, 1)
-        robot.display.text(f"LED state: {state}", 0, 10, 1)
-        robot.display.text(f"Int on:  {value_on}", 0, 18, 1)
-        robot.display.text(f"Int off: {value_off}", 0, 26, 1)
+        robot.display.text(f"LED state: {state}", 0, 16, 1)
+        robot.display.text(f"Int on:  {value_on}", 0, 24, 1)
+        robot.display.text(f"Int off: {value_off}", 0, 32, 1)
         robot.display.text('<', 0, 54, 1)
         robot.display.show()
 

menu_programs/nxt_sound.py

@@ -16,7 +16,7 @@ def nxt_sound_run(robot, chosen_sensor_port):
         
         robot.display.fill(0)
         robot.display.centered_text(f"NXT Sound S{chosen_sensor_port+1}", 0, 1)
-        robot.display.text(f"Intensity: {intensity}", 0, 18, 1)
+        robot.display.text(f"Intensity: {intensity}", 0, 16, 1)
         robot.display.text('<', 0, 54, 1)
         robot.display.show()
         print(f"Sound level: {intensity}")

menu_programs/nxt_touch.py

@@ -15,7 +15,7 @@ def nxt_touch_run(robot, chosen_sensor_port):
         
         robot.display.fill(0)
         robot.display.centered_text(f"NXT Touch S{chosen_sensor_port+1}", 0, 1)
-        robot.display.text(f"Pressed: {touch_pressed}", 0, 18, 1)
+        robot.display.text(f"Pressed: {touch_pressed}", 0, 16, 1)
         robot.display.text('<', 0, 54, 1)
         robot.display.show()
         print(f"Presed: {"True" if touch_pressed else "False"}")

menu_programs/oc_color.py

@@ -25,9 +25,11 @@ def oc_color_run(robot, chosen_sensor_port):
     try:
         robot.sensors.light[chosen_sensor_port].reflection()
     except SensorNotReadyError as e:
+        print(f"OC Color S{chosen_sensor_port+1} sensor not connected")
         display_sensor_not_connected(robot)
         return
     except SensorMismatchError as e:
+        print(f"OC Color S{chosen_sensor_port+1} incorrect sensor connected")
         display_incorrect_sensor(robot)
         return
     

menu_programs/oc_gyro.py

@@ -16,7 +16,7 @@ def oc_gyro_run(robot, chosen_sensor_port):
     robot.init_sensor(Sensor.OC_GYRO, chosen_sensor_port)
 
     robot.display.fill(0)
-    robot.display.centered_text(f"OC Gyro S{chosen_sensor_port+1}", 0, 1)
+    robot.display.centered_text(f"OC AHRS S{chosen_sensor_port+1}", 0, 1)
     robot.display.centered_text("Connecting...", 16, 1)
     robot.display.show()
 
@@ -26,9 +26,11 @@ def oc_gyro_run(robot, chosen_sensor_port):
         #print("Error:", e)
         #print(e.args)
         #print(type(e))
+        print(f"OC AHRS S{chosen_sensor_port+1} sensor not connected")
         display_sensor_not_connected(robot)
         return
     except SensorMismatchError as e:
+        print(f"OC AHRS S{chosen_sensor_port+1} incorrect sensor connected")
         display_incorrect_sensor(robot)
         return
         
@@ -37,7 +39,7 @@ def oc_gyro_run(robot, chosen_sensor_port):
     debounce = False
     while True:
         robot.display.fill(0)
-        robot.display.centered_text(f"OC Gyro S{chosen_sensor_port+1}", 0, 1)
+        robot.display.centered_text(f"OC AHRS S{chosen_sensor_port+1}", 0, 1)
         robot.display.text(mode_str[mode], 0, 16, 1)
 
         if mode == 0:

menu_programs/oc_laser.py

@@ -24,9 +24,11 @@ def oc_laser_run(robot, chosen_sensor_port):
     try:
         robot.sensors.laser[chosen_sensor_port].distance()
     except SensorNotReadyError as e:
+        print(f"OC Laser S{chosen_sensor_port+1} sensor not connected")
         display_sensor_not_connected(robot)
         return
     except SensorMismatchError as e:
+        print(f"OC Laser S{chosen_sensor_port+1} incorrect sensor connected")
         display_incorrect_sensor(robot)
         return
     
@@ -54,7 +56,7 @@ def oc_laser_run(robot, chosen_sensor_port):
         pressed = robot.buttons.pressed()
         # Exit program if left cube button is pressed
         if pressed[Button.LEFT]:
-            break;
+            break
         if not debounce:
             if pressed[Button.DOWN]:
                 mode += 1

menu_programs/oc_ultra.py

@@ -21,9 +21,11 @@ def oc_ultra_run(robot, chosen_sensor_port):
     try:
         robot.sensors.ultrasonic[chosen_sensor_port].distance()
     except SensorNotReadyError as e:
+        print(f"OC Ultrasonic S{chosen_sensor_port+1} sensor not connected")
         display_sensor_not_connected(robot)
         return
     except SensorMismatchError as e:
+        print(f"OC Ultrasonic S{chosen_sensor_port+1} incorrect sensor connected")
         display_incorrect_sensor(robot)
         return