Merge pull request #5 from stack-chan/develop

mongonta0716 · web-flow · commit 55deb91bf119 · 2024-08-31T23:26:53.000+09:00
Support Dynamixel XL330
diff --git a/.gitignore b/.gitignore
@@ -30,3 +30,8 @@
 *.exe
 *.out
 *.app
+
+lib/
+.vscode/
+.pio/
+build/
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
@@ -41,9 +57,11 @@ balloon:
 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
-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.
+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: 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
@@ -15,8 +15,42 @@
       }
     ],
     "license": "MIT",
-    "dependencies": {
-    },
+    "dependencies": [
+      {
+        "owner": "M5Stack",
+        "name": "M5Unified",
+        "version": ">=0.1.16"
+      },
+      {
+        "owner": "madhephaestus",
+        "name": "ESP32Servo",
+        "version": "0.13.0"
+      },
+      {
+        "owner": "arminjo",
+        "name": "ServoEasing",
+        "version": "3.1.0"
+      },
+      {
+        "owner": "bblanchon",
+        "name": "ArduinoJson",
+        "version": "^6"
+      },
+      {
+        "owner": "robotis-git",
+        "name": "Dynamixel2Arduino",
+        "version": "^0.7.0"
+      },
+      {
+        "owner": "tobozo",
+        "name": "YAMLDuino",
+        "version": "1.4.2"
+      },
+      {
+        "name": "SCServo",
+        "version": "https://github.com/mongonta0716/SCServo.git"
+      }
+    ],
     "version": "0.0.1",
     "frameworks": "arduino",
     "platforms": "espressif32"
diff --git a/platformoio.ini b/platformoio.ini
@@ -0,0 +1,37 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; http://docs.platformio.org/page/projectconf.html
+
+[platformio]
+default_envs = m5stack-core
+
+[env]
+platform = espressif32
+framework = arduino
+monitor_speed = 115200
+build_flags =
+  -Llib/
+
+[env:m5stack-core]
+board = m5stack-core-esp32
+lib_deps = m5stack/M5Unified
+
+[env:m5stack-core-with-aquestalk]
+board = m5stack-core-esp32
+lib_deps = m5stack/M5Unified
+
+[env:m5stack-fire]
+board = m5stack-fire
+lib_deps = 
+    m5stack/M5Unified
+
+[env:m5stack-core2]
+board = m5stack-core2
+lib_deps = 
+    m5stack/M5Unified
diff --git a/src/Stackchan_servo.cpp b/src/Stackchan_servo.cpp
@@ -3,11 +3,19 @@
 
 #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);
 }
 
+static long convertDYNIXELXL330(int16_t degree) {
+  M5_LOGI("Degree: %d\n", degree);
+  
+  long ret =  map(degree, 0, 360, 0, 4095);
+  M5_LOGI("Position: %d\n", ret);
+  return ret;
+}
+
 // シリアルサーボ用のEasing関数
 float quadraticEaseInOut(float p) {
   //return p;
@@ -21,20 +29,45 @@ float quadraticEaseInOut(float p) {
 	}
 }
 
+
 StackchanSERVO::StackchanSERVO() {}
 
 StackchanSERVO::~StackchanSERVO() {}
 
 
 void StackchanSERVO::attachServos() {
-  if (_servo_type == SCS) {
+  if (_servo_type == ServoType::SCS) {
     // SCS0009
     Serial2.begin(1000000, SERIAL_8N1, _init_param.servo[AXIS_X].pin, _init_param.servo[AXIS_Y].pin);
     _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);
     vTaskDelay(1000/portTICK_PERIOD_MS);
 
+  } else if (_servo_type == ServoType::DYN_XL330) {
+    M5_LOGI("DYN_XL330");
+    Serial2.begin(1000000, SERIAL_8N1, _init_param.servo[AXIS_X].pin, _init_param.servo[AXIS_Y].pin);
+    _dxl = Dynamixel2Arduino(Serial2);
+    _dxl.begin(1000000);
+    _dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
+    _dxl.ping(AXIS_X + 1);
+    _dxl.ping(AXIS_Y + 1);
+    _dxl.setOperatingMode(AXIS_X + 1, OP_POSITION);
+    _dxl.setOperatingMode(AXIS_Y + 1, OP_POSITION);
+    _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);
+    //_dxl.torqueOff(AXIS_Y + 1);
+    
   } else {
     // SG90 PWM
     if (_servo_x.attach(_init_param.servo[AXIS_X].pin, 
@@ -47,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);
@@ -81,6 +114,14 @@ void StackchanSERVO::moveX(int x, uint32_t millis_for_move) {
     _isMoving = true;
     vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
     _isMoving = false;
+  } else if (_servo_type == ServoType::DYN_XL330) {
+    _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;
   } else {
     if (millis_for_move == 0) {
       _servo_x.easeTo(x + _init_param.servo[AXIS_X].offset);
@@ -100,11 +141,19 @@ void StackchanSERVO::moveX(servo_param_s servo_param_x) {
 }
 
 void StackchanSERVO::moveY(int y, uint32_t millis_for_move) {
-  if (_servo_type == SCS) {
+  if (_servo_type == ServoType::SCS) {
     _sc.WritePos(AXIS_Y + 1, convertSCS0009Pos(y + _init_param.servo[AXIS_Y].offset), millis_for_move);
     _isMoving = true;
     vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
     _isMoving = false;
+  } else if (_servo_type == ServoType::DYN_XL330) {
+    _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);
+    _isMoving = true;
+    vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
+    _isMoving = false;
   } else {
     if (millis_for_move == 0) {
       _servo_y.easeTo(y + _init_param.servo[AXIS_Y].offset);
@@ -123,7 +172,7 @@ void StackchanSERVO::moveY(servo_param_s servo_param_y) {
   moveY(servo_param_y.degree, servo_param_y.millis_for_move);
 }
 void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) {
-  if (_servo_type == SCS) {
+  if (_servo_type == ServoType::SCS) {
     int increase_degree_x = x - _last_degree_x;
     int increase_degree_y = y - _last_degree_y;
     uint32_t division_time = millis_for_move / SERIAL_EASE_DIVISION;
@@ -137,6 +186,11 @@ void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) {
       //vTaskDelay(division_time);
     }
     _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);
@@ -150,12 +204,17 @@ void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) {
 }
 
 void StackchanSERVO::moveXY(servo_param_s servo_param_x, servo_param_s servo_param_y) {
-  if (_servo_type == SCS) {
+  if (_servo_type == ServoType::SCS) {
     _sc.WritePos(AXIS_X + 1, convertSCS0009Pos(servo_param_x.degree + servo_param_x.offset), servo_param_x.millis_for_move);
     _sc.WritePos(AXIS_Y + 1, convertSCS0009Pos(servo_param_y.degree + servo_param_y.offset), servo_param_y.millis_for_move);
     _isMoving = true;
     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);
@@ -211,3 +270,4 @@ void StackchanSERVO::motion(Motion motion_number) {
     delay(1000);
     moveXY(_init_param.servo[AXIS_X].start_degree, _init_param.servo[AXIS_Y].degree, 1000);
 }
+
diff --git a/src/Stackchan_servo.h b/src/Stackchan_servo.h
@@ -11,6 +11,10 @@
 #include <ServoEasing.h>
 #include <SCServo.h>
 #include <M5Unified.h>
+#include <Dynamixel2Arduino.h>
+
+using namespace ControlTableItem;
+
 #define SERIAL_EASE_DIVISION  5      // シリアルサーボのEasing分割数
 
 enum Motion {
@@ -28,8 +32,9 @@ enum ServoAxis {
 };
 
 enum ServoType {
-    PWM,
-    SCS
+    PWM,             // SG90 PWM
+    SCS,             // Feetech SCS0009
+    DYN_XL330        // Dynamixel XL330
 };
 
 typedef struct ServoParam {
@@ -47,10 +52,14 @@ typedef struct  StackchanServo{
     servo_param_s servo[2];
 } stackchan_servo_initial_param_s;
 
+
+const float DXL_PROTOCOL_VERSION = 2.0f;
+
 class StackchanSERVO {
     protected:
         ServoType _servo_type;
         SCSCL _sc;
+        Dynamixel2Arduino _dxl;
         ServoEasing _servo_x;
         ServoEasing _servo_y;
         void attachServos();
@@ -74,5 +83,4 @@ class StackchanSERVO {
         void motion(Motion motion_no);
         bool isMoving() { return _isMoving; }
 };
-
 #endif // _STACKCHAN_SERVO_H_
diff --git a/src/Stackchan_system_config.cpp b/src/Stackchan_system_config.cpp

-Original file line number
+Diff line change
 *.exe
 *.out
 *.app
++
 +lib/
 +.vscode/
 +.pio/
 +build/