bugFix testModeMenu implement duration print

src/main.cpp

@@ -66,6 +66,8 @@ void callbackControllerConnect(void);
 void callbackControllerDisconnect(void);
 void i2cScanner(void);
 void makeMenu(void);
+void restart(void);
+void disconnectController(void);
 
 
 void setup() {
@@ -139,9 +141,19 @@ void loop() {
     driveManager->loop();
     lcdWrapper->loop();
 
-    DebugTimes mainBatteryTime;
     mainBattery->loop();
-    mainBatteryTime.stopConsol("MainBatteryTime", 5);
+    if (mainBattery->getBatteryPercent() <= 10) {
+        moveController.setDrivingStatus(DrivingStatus::stop);
+        moveController.setSpeed(0);
+        moveController.setRotationSpeed(0);
+
+        lcd->setCursor(0, 0);
+        lcd->printf("Low Battery...");
+        lcd->setCursor(0, 1);
+        lcd->print("Please turn off.");
+        
+        while (1);        
+    }
 
     if (disconnectPs3) {
         DebugTimes disconnectPS3Time;
@@ -249,22 +261,6 @@ void makeMenu() {
         std::cout << "Dummy in Action" <<std::endl;
     };
 
-    auto restart = [&]() {
-        lcdWrapper->clear();
-        lcdWrapper->setCursor(0, 0);
-        lcdWrapper->print("Rebooting ...");
-        ESP.restart();
-    };
-
-    auto disconnectController = [&]() {
-        disconnectPs3 = true;
-        lcdWrapper->clear();
-        lcdWrapper->setCursor(0, 0);
-        lcdWrapper->print("Controller is");
-        lcdWrapper->setCursor(0, 1);
-        lcdWrapper->print("disconnected.");
-    };
-
     // Create Menu
     main_m = new Menu();
     Menu* mode_m = new Menu();
@@ -339,3 +335,19 @@ void makeMenu() {
     pidr_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(1)->GetKi());
     pidr_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(1)->GetKd());
 }
+
+void restart(void) {
+    lcdWrapper->clear();
+    lcdWrapper->setCursor(0, 0);
+    lcdWrapper->print("Rebooting ...");
+    ESP.restart();
+}
+
+void disconnectController(void) {
+    disconnectPs3 = true;
+    lcdWrapper->clear();
+    lcdWrapper->setCursor(0, 0);
+    lcdWrapper->print("Controller is");
+    lcdWrapper->setCursor(0, 1);
+    lcdWrapper->print("disconnected.");
+}