Merge pull request #8 from stack-chan/develop

bug fix and turn function for SCS0009

Author: mongonta0716

library.json

@@ -19,7 +19,7 @@
       {
         "owner": "M5Stack",
         "name": "M5Unified",
-        "version": "^0.2.1"
+        "version": "^0.2.5"
       },
       {
         "owner": "madhephaestus",
@@ -34,7 +34,7 @@
       {
         "owner": "bblanchon",
         "name": "ArduinoJson",
-        "version": "^6"
+        "version": "^7"
       },
       {
         "owner": "robotis-git",
@@ -44,14 +44,14 @@
       {
         "owner": "tobozo",
         "name": "YAMLDuino",
-        "version": "1.4.2"
+        "version": "1.5.0"
       },
       {
         "name": "SCServo",
         "version": "https://github.com/mongonta0716/SCServo.git"
       }
     ],
-    "version": "0.0.2",
+    "version": "0.0.3",
     "frameworks": "arduino",
     "platforms": "espressif32"
   }

library.properties

@@ -1,5 +1,5 @@
 name=stackchan-arduino
-version=0.0.2
+version=0.0.3
 author=Takao Akaki
 maintainer=Takao Akaki<takaoakaki@gmail.com>
 sentence=Stackchan library for M5Stack

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->getBatteryCurrent() > 0) {
+      // 後ろから給電されている状態。
+      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

src/Stackchan_servo.cpp

@@ -54,6 +54,7 @@ void StackchanSERVO::attachServos() {
   if (_servo_type == ServoType::SCS) {
     // SCS0009
     Serial2.begin(1000000, SERIAL_8N1, _init_param.servo[AXIS_X].pin, _init_param.servo[AXIS_Y].pin);
+    delay(500);
     _sc.pSerial = &Serial2;
     _sc.WritePos(AXIS_X + 1, convertSCS0009Pos(_init_param.servo[AXIS_X].start_degree + _init_param.servo[AXIS_X].offset), 1000);
     _sc.WritePos(AXIS_Y + 1, convertSCS0009Pos(_init_param.servo[AXIS_Y].start_degree + _init_param.servo[AXIS_Y].offset), 1000);
@@ -312,6 +313,24 @@ void StackchanSERVO::moveXY(servo_param_s servo_param_x, servo_param_s servo_par
   _last_degree_y = servo_param_y.degree;
 }
 
+// @uint32_t speed 0〜1000
+void StackchanSERVO::turnX(uint32_t speed, bool is_cw, uint32_t millis_for_move) {
+    if (speed >= 1000) {
+      speed = 1000;
+    }
+    if (is_cw) {
+      speed += 1000; // 逆回転時は+1000
+    }
+    Serial.printf("speed: %d\n", speed);
+    _sc.PWMMode(1, true); // 回転モード
+    _isMoving = true;
+    _sc.WritePWM(1, speed);
+    vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
+    _isMoving = false;
+    _sc.PWMMode(1, false); // 位置決めモードへ戻す 
+  return;
+}
+
 void StackchanSERVO::motion(Motion motion_number) {
     if (motion_number == nomove) return; 
     moveXY(90, 75, 500);

src/Stackchan_servo.h

@@ -85,6 +85,7 @@ class StackchanSERVO {
         void moveY(servo_param_s servo_param_y);
         void moveXY(servo_param_s servo_param_x, servo_param_s servo_param_y);
         void motion(Motion motion_no);
+        void turnX(uint32_t speed, bool is_cw, uint32_t millis_for_move);
         bool isMoving() { return _isMoving; }
 };
 #endif // _STACKCHAN_SERVO_H_

src/Stackchan_system_config.cpp

@@ -23,8 +23,10 @@ void StackchanSystemConfig::setDefaultParameters() {
             _servo[AXIS_Y].pin = 21;
             break;
         case m5::board_t::board_M5StackCoreS3:
+        case m5::board_t::board_M5StackCoreS3SE:
             _servo[AXIS_X].pin = 1;
             _servo[AXIS_Y].pin = 2;
+            break;
         default:
             M5_LOGI("UnknownBoard:%d\n", M5.getBoard());
             _servo[AXIS_X].pin = 22;
@@ -190,6 +192,7 @@ void StackchanSystemConfig::setSystemConfig(DynamicJsonDocument doc) {
         _servo_type = ServoType::PWM;
     }
     _secret_config_show     = doc["secret_config_show"].as<bool>(); 
+    
 }
 
 void StackchanSystemConfig::setSecretConfig(DynamicJsonDocument doc) {