fix chekcTakaoBase

Author: mongonta0716

src/Stackchan_Takao_Base.hpp

@@ -11,24 +11,34 @@ enum PowerStatus {
     Battery
 };
 
-uint8_t checkTakaoBasePowerStatus(m5::Power_Class* power, StackchanSERVO* servo ) {
-  bool last_ext_output = power->getExtOutput();
-  if (power->Axp192.getACINVoltage() > 1.0f) {
+PowerStatus checkTakaoBasePowerStatus(m5::Power_Class* power, int16_t battery_threshold = 3200) {
+  if (!power->getExtOutput() && power->getBatteryCurrent() < 0) {
+    // TakaoBase使用時(ExtOutput=falseの場合)、バッテリー駆動の時はtrueに切り替える。 
+    // 切り替えないとサーボが動きません。
     power->setExtOutput(true);
+    power->setLed(0);
+    return PowerStatus::Battery;  
+  }
+
+  if (power->getExtOutput() && (power->getBatteryCurrent() >= 0) {
+    // M5StackのUSB-C端子から給電されている状態。
     power->setLed(80);
-    return PowerStatus::SidePower;  
+    return PowerStatus::SidePower;
   }
-  while (servo->isMoving()) {delay(1);} // サーボが動いている間は待機（そうしないとサーボの動きが乱れる。）
-  power->setExtOutput(false); // 後側のUSB-Cの状態を把握するためにfalseにする必要があります。
-  delay(500);
-  if (power->Axp192.getBatteryDischargeCurrent() > 3.0f) {
-    power->setExtOutput(true);
-    power->setLed(0);
-    return PowerStatus::Battery;
+
+  if (power->getBatteryLevel() < battery_threshold) {
+    // Batteryの電圧が閾値よりも下がったときの処理
+    power->setExtOutput(false); // 後側のUSB-Cの給電状態を把握するためにfalseにする必要があります。
+    if (power->getVBUSVoltage() > 3000) {
+      // 後ろから給電されている状態。
+      power->setLed(80);
+      return PowerStatus::BackPower; 
+    } else {
+      // 給電されていない場合は電源OFF
+      M5.Power.powerOff();
+    }
   }
-  power->setExtOutput(false);
-  power->setLed(80);
-  return PowerStatus::BackPower; 
+  return PowerStatus::BackPower;
 }
 
 #endif // STACKCHAN_TAKAO_BASE_HPP