Fixed bugs and added missing parts

mongonta0716 · mongonta0716 · commit ac0d04a48486 · 2024-08-23T16:56:35.000+09:00
diff --git a/README.md b/README.md
@@ -7,17 +7,12 @@ platformio.iniのlib_depsに下記の記述を追加してください。(その
 
 ```
 lib_deps = 
-  m5stack/M5Unified@0.1.16
-  arminjo/ServoEasing@^2.4.0
-  madhephaestus/ESP32Servo @ 0.13.0
-  bblanchon/ArduinoJson @ ^6
-  tobozo/YAMLDuino
   https://github.com/mongonta0716/stackchan-arduino.git
-  https://github.com/mongonta0716/SCServo
 lib_ldf_mode = deep                                       ; これを忘れるとリンクエラーになります。
 ```
 
 # 設定ファイル
+設定内容は各ファイルに詳しくコメントしています。
 - [SC_BasicConfig.yaml](./data/yaml/SC_BasicConfig.yaml)<br>ｽﾀｯｸﾁｬﾝを起動するために必要な基本的なパラメータを集めています。
 - [SC_SecConfig.yaml](./data/yaml/SC_SecConfig.yaml)<br>個人情報用の設定ファイルです。(WiFiやAPIKey等)
 - [SC_ExConfig.yaml](./data/yaml/SC_ExConfig.yaml)<br>アプリケーション側で、設定を追加したいときに使用します。使い方はexamples/Advanceを参照してください。
diff --git a/data/yaml/SC_BasicConfig.yaml b/data/yaml/SC_BasicConfig.yaml
@@ -3,14 +3,30 @@ servo:
     # ServoPin
     # Core1 PortA X:22,Y:21 PortC X:16,Y:17
     # Core2 PortA X:33,Y:32 PortC X:13,Y:14
+    # CoreS3 PortA X:2, Y:1 PortB X:9, Y:8 PortC X:17, Y:18
     # Stack-chanPCB Core1 X:5,Y:2 Core2 X:19,Y27
-    # When using SCS0009, x:RX, y:TX (not used).(StackchanRT Version:Core1 x16,y17, Core2: x13,y14)
+    # When using SCS0009 or Dynamixel XL330, x:RX, y:TX (not used).(StackchanRT Version:Core1 x16,y17, Core2: x13,y14)
     x: 17
     y: 18
   offset: 
     # Specified by +- from 90 degree during servo initialization
     x: 0
     y: 0
+  center:
+    # サーボの初期位置
+    # SG90: x:90 y:90
+    # SCS0009: x:150, y:150
+    # Dynamixel XL330: x:180, y:270
+    x: 90
+    y: 90
+  lower_limit:
+    # 可動範囲の下限（下限と言っても取り付け方により逆の場合あり, 値の小さい方を指定。）
+    x: 0
+    y: 60
+  upper_limit:
+    # 可動範囲の上限（上限と言っても取り付け方により逆の場合もあり, 値の大きい方を指定。）
+    x: 90
+    y: 100
   speed: 
     normal_mode: 
       interval_min: 3000
@@ -42,8 +58,10 @@ led_lr: 0 # 0:stereo, 1:left_only, 2:right_only
 led_pin: 15 # GoBottom1:15 GoBottom2:25
 takao_base: false # Whether to use takaobase to feed power from the rear connector.(Stack-chan_Takao_Base  https://ssci.to/8905)
 servo_type: "PWM" # "PWM": SG90PWMServo, "SCS": Feetech SCS0009 "DYN_XL330": Dynamixel XL330
-extend_config_filename: "/yaml/SC_ExConfig.yaml"     # Configuration file for the application.
-extend_config_filesize: 2048                         # Buffer size for feature extensions
-secret_config_filename: "/yaml/SC_SecConfig.yaml"    # Configuration file for the File for personal information.
-secret_config_filesize: 2048                         # Buffer size for personal information.
-secret_info_show: true                               # Whether personal information is output to the log or not.
+
+# 下記は拡張用の設定ファイルを使用する場合に設定します。(初期値はありません。)
+extend_config_filename: ""     # "/yaml/SC_ExConfig.yaml"     # Configuration file for the application.
+extend_config_filesize: 0      # 2048                         # Buffer size for feature extensions
+secret_config_filename: ""     # "/yaml/SC_SecConfig.yaml"    # Configuration file for the File for personal information.
+secret_config_filesize: 0      # 2048                         # Buffer size for personal information.
+secret_info_show: true                                        # Whether personal information is output to the log or not.
diff --git a/library.json b/library.json
@@ -37,7 +37,7 @@
         "version": "^6"
       },
       {
-        "owner": "roboies-git",
+        "owner": "robotis-git",
         "name": "Dynamixel2Arduino",
         "version": "^0.7.0"
       },
@@ -47,8 +47,8 @@
         "version": "1.4.2"
       },
       {
-        "name": "SCSServo",
-        "version": "https://github.com/mongonta0716/SCSServo.git"
+        "name": "SCServo",
+        "version": "https://github.com/mongonta0716/SCServo.git"
       }
     ],
     "version": "0.0.1",
diff --git a/src/Stackchan_servo.cpp b/src/Stackchan_servo.cpp
@@ -3,12 +3,12 @@
 
 #include <ServoEasing.hpp>
 
-long convertSCS0009Pos(int16_t degree) {
+static long convertSCS0009Pos(int16_t degree) {
   //Serial.printf("Degree: %d\n", degree);
   return map(degree, 0, 300, 1023, 0);
 }
 
-long convertDYNIXELXL330(int16_t degree) {
+static long convertDYNIXELXL330(int16_t degree) {
   M5_LOGI("Degree: %d\n", degree);
   
   long ret =  map(degree, 0, 360, 0, 4095);
@@ -52,13 +52,17 @@ void StackchanSERVO::attachServos() {
     _dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
     _dxl.ping(AXIS_X + 1);
     _dxl.ping(AXIS_Y + 1);
-    _dxl.torqueOn(AXIS_X + 1);
-    _dxl.torqueOn(AXIS_Y + 1);
     _dxl.setOperatingMode(AXIS_X + 1, OP_POSITION);
     _dxl.setOperatingMode(AXIS_Y + 1, OP_POSITION);
-    _dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, 100);
-    _dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, 100);
-    delay(1000);
+    _dxl.writeControlTableItem(DRIVE_MODE, AXIS_X + 1, 4);  // Velocityのパラメータを移動時間(msec)で指定するモードに変更
+    _dxl.writeControlTableItem(DRIVE_MODE, AXIS_Y + 1, 4);  // Velocityのパラメータを移動時間(msec)で指定するモードに変更
+    _dxl.torqueOn(AXIS_X + 1);
+    delay(10); // ここでWaitを入れないと、Y(tilt)サーボが動かない場合がある。
+    _dxl.torqueOn(AXIS_Y + 1);
+    delay(100);
+    _dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, 1000);
+    _dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, 1000);
+    delay(100);
     _dxl.setGoalPosition(AXIS_X + 1, 2048);
     _dxl.setGoalPosition(AXIS_Y + 1, 3073);
     //_dxl.torqueOff(AXIS_X + 1);
@@ -76,7 +80,7 @@ void StackchanSERVO::attachServos() {
                         _init_param.servo[AXIS_Y].start_degree + _init_param.servo[AXIS_Y].offset,
                         DEFAULT_MICROSECONDS_FOR_0_DEGREE,
                         DEFAULT_MICROSECONDS_FOR_180_DEGREE)) {
-      Serial.print("Error attaching servo x");
+      Serial.print("Error attaching servo y");
     }
 
     _servo_x.setEasingType(EASE_QUADRATIC_IN_OUT);
@@ -111,9 +115,10 @@ void StackchanSERVO::moveX(int x, uint32_t millis_for_move) {
     vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
     _isMoving = false;
   } else if (_servo_type == ServoType::DYN_XL330) {
-    _dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, calcVelocity(ServoAxis::AXIS_X, x, millis_for_move));
+    _dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, millis_for_move);
     vTaskDelay(10/portTICK_PERIOD_MS);
     _dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330(x + _init_param.servo[AXIS_X].offset));
+    vTaskDelay(10/portTICK_PERIOD_MS);
     _isMoving = true;
     vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
     _isMoving = false;
@@ -142,9 +147,10 @@ void StackchanSERVO::moveY(int y, uint32_t millis_for_move) {
     vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
     _isMoving = false;
   } else if (_servo_type == ServoType::DYN_XL330) {
-    _dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, calcVelocity(ServoAxis::AXIS_Y, y, millis_for_move));
+    _dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, millis_for_move);
+    vTaskDelay(10/portTICK_PERIOD_MS);
+    _dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330(y + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
     vTaskDelay(10/portTICK_PERIOD_MS);
-    _dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330(y + 90 + _init_param.servo[AXIS_Y].offset));
     _isMoving = true;
     vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
     _isMoving = false;
@@ -181,6 +187,10 @@ void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) {
     }
     _isMoving = false;
   } else if (_servo_type == ServoType::DYN_XL330) {
+    _isMoving = true;
+    _dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330(x + _init_param.servo[AXIS_X].offset)); 
+    _dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330(y + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
+    _isMoving = false;
   } else {
     _servo_x.setEaseToD(x + _init_param.servo[AXIS_X].offset, millis_for_move);
     _servo_y.setEaseToD(y + _init_param.servo[AXIS_Y].offset, millis_for_move);
@@ -201,6 +211,10 @@ void StackchanSERVO::moveXY(servo_param_s servo_param_x, servo_param_s servo_par
     vTaskDelay(max(servo_param_x.millis_for_move, servo_param_y.millis_for_move)/portTICK_PERIOD_MS);
     _isMoving = false;
   } else if (_servo_type == ServoType::DYN_XL330) {
+    _isMoving = true;
+    _dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330(servo_param_x.degree + _init_param.servo[AXIS_X].offset)); 
+    _dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330(servo_param_y.degree + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
+    _isMoving = false;
   } else {
     if (servo_param_x.degree != 0) {
       _servo_x.setEaseToD(servo_param_x.degree + servo_param_x.offset, servo_param_x.millis_for_move);
@@ -257,18 +271,3 @@ void StackchanSERVO::motion(Motion motion_number) {
     moveXY(_init_param.servo[AXIS_X].start_degree, _init_param.servo[AXIS_Y].degree, 1000);
 }
 
-// Dynamixel XL330では移動時間の指定はできないので変換する。
-long StackchanSERVO::calcVelocity(ServoAxis axis_id, int16_t degree, uint32_t millis_for_move) {
-  int16_t move_degree = 0;
-  if (axis_id == ServoAxis::AXIS_X) {
-    move_degree = abs(_last_degree_x - degree);
-  } else {
-    move_degree = abs(_last_degree_y - degree);
-  }
-
-  long rpm = (move_degree / 360) / (60 * (millis_for_move / 1000));
-  M5_LOGI("RPM: %d\n", rpm);
-  //return rpm;
-
-  return 300;
-}
diff --git a/src/Stackchan_servo.h b/src/Stackchan_servo.h
@@ -82,6 +82,5 @@ class StackchanSERVO {
         void moveXY(servo_param_s servo_param_x, servo_param_s servo_param_y);
         void motion(Motion motion_no);
         bool isMoving() { return _isMoving; }
-        long calcVelocity(ServoAxis axis_id, int16_t degree, uint32_t millis_for_move);
 };
 #endif // _STACKCHAN_SERVO_H_
diff --git a/src/Stackchan_system_config.cpp b/src/Stackchan_system_config.cpp
@@ -11,6 +11,8 @@ StackchanSystemConfig::~StackchanSystemConfig() {
 }
 
 void StackchanSystemConfig::setDefaultParameters() {
+    // 設定ファイルが存在しないときはデフォルトパラメータを使用します。
+    // PWM サーボでPort.Aを想定しています。
     switch(M5.getBoard()) {
         case m5::board_t::board_M5StackCore2:
             _servo[AXIS_X].pin = 33;
@@ -92,7 +94,7 @@ void StackchanSystemConfig::loadConfig(fs::FS& fs, const char *app_yaml_filename
     if (secret_yaml_filesize > 0) {
         loadSecretConfig(fs, secret_yaml_filename, secret_yaml_filesize);
     }
-    if (app_yaml_filename > 0) {
+    if (app_yaml_filesize > 0) {
         loadExtendConfig(fs, app_yaml_filename, app_yaml_filesize);
     }
     printAllParameters();
@@ -138,6 +140,9 @@ void StackchanSystemConfig::setSystemConfig(DynamicJsonDocument doc) {
     _servo[AXIS_X].offset = servo["offset"]["x"];
     _servo[AXIS_Y].offset = servo["offset"]["y"];
 
+    _servo[AXIS_X].start_degree = servo["center"]["x"];
+    _servo[AXIS_Y].start_degree = servo["center"]["y"];
+
     _servo[AXIS_X].lower_limit = servo["lower_limit"]["x"];
     _servo[AXIS_X].upper_limit = servo["upper_limit"]["x"];
     _servo[AXIS_Y].lower_limit = servo["lower_limit"]["y"];
@@ -174,17 +179,12 @@ void StackchanSystemConfig::setSystemConfig(DynamicJsonDocument doc) {
     if (_servo_type_str.indexOf("SCS") != -1) {
         // SCS0009
         _servo_type = ServoType::SCS;
-        _servo[AXIS_X].start_degree = 150;
-        _servo[AXIS_Y].start_degree = 150;
     } else if (_servo_type_str.indexOf("DYN_XL330") != -1) {
         // Dynamixel XL330
         _servo_type = ServoType::DYN_XL330;
-        _servo[AXIS_X].start_degree = 180;
-        _servo[AXIS_Y].start_degree = 180;
     } else  {
-        _servo_type = ServoType::PWM; // PWMサーボ
-        _servo[AXIS_X].start_degree = 90;
-        _servo[AXIS_Y].start_degree = 90;
+        // PWMサーボ
+        _servo_type = ServoType::PWM;
     }
     _secret_config_show     = doc["secret_config_show"].as<bool>(); 
 }
@@ -216,6 +216,8 @@ void StackchanSystemConfig::printAllParameters() {
     M5_LOGI("servo:pin_y:%d", _servo[AXIS_Y].pin);
     M5_LOGI("servo:offset_x:%d", _servo[AXIS_X].offset);
     M5_LOGI("servo:offset_y:%d", _servo[AXIS_Y].offset);
+    M5_LOGI("servo.start_degree_x:%d", _servo[AXIS_X].start_degree);
+    M5_LOGI("servo.start_degree_y:%d", _servo[AXIS_Y].start_degree);
     M5_LOGI("servo.lower_limit_x:%d", _servo[AXIS_X].lower_limit);
     M5_LOGI("servo.lower_limit_y:%d", _servo[AXIS_Y].lower_limit);
     M5_LOGI("servo.upper_limit_x:%d", _servo[AXIS_X].upper_limit);

Original file line number	Diff line number	Diff line change
`@@ -37,7 +37,7 @@`
`37`	`37`	`"version": "^6"`
`38`	`38`	`},`
`39`	`39`	`{`
`40`		`- "owner": "roboies-git",`
	`40`	`+ "owner": "robotis-git",`
`41`	`41`	`"name": "Dynamixel2Arduino",`
`42`	`42`	`"version": "^0.7.0"`
`43`	`43`	`},`
`@@ -47,8 +47,8 @@`
`47`	`47`	`"version": "1.4.2"`
`48`	`48`	`},`
`49`	`49`	`{`
`50`		`- "name": "SCSServo",`
`51`		`- "version": "https://github.com/mongonta0716/SCSServo.git"`
	`50`	`+ "name": "SCServo",`
	`51`	`+ "version": "https://github.com/mongonta0716/SCServo.git"`
`52`	`52`	`}`
`53`	`53`	`],`
`54`	`54`	`"version": "0.0.1",`