From aa95e64995f70271eda69718514c159cfc162f11 Mon Sep 17 00:00:00 2001 From: starf555 Date: Sun, 10 Nov 2024 22:57:11 +0900 Subject: [PATCH 1/2] Add RTversion Dynamixel --- src/Stackchan_servo.cpp | 79 ++++++++++++++++++++++++++++++++- src/Stackchan_servo.h | 6 ++- src/Stackchan_system_config.cpp | 3 ++ 3 files changed, 86 insertions(+), 2 deletions(-) diff --git a/src/Stackchan_servo.cpp b/src/Stackchan_servo.cpp index aeae838..c4fd42a 100644 --- a/src/Stackchan_servo.cpp +++ b/src/Stackchan_servo.cpp @@ -16,6 +16,14 @@ static long convertDYNIXELXL330(int16_t degree) { return ret; } +static long convertDYNIXELXL330_RT(int16_t degree) { + M5_LOGI("Degree: %d\n", degree); + + long ret = map(degree, -360, 720, -4095, 8191); + M5_LOGI("Position: %d\n", ret); + return ret; +} + // シリアルサーボ用のEasing関数 float quadraticEaseInOut(float p) { //return p; @@ -34,6 +42,13 @@ StackchanSERVO::StackchanSERVO() {} StackchanSERVO::~StackchanSERVO() {} +float StackchanSERVO::getPosition(int x){ + if (_servo_type == RT_DYN_XL330){ + return _dxl.getPresentPosition(x);; + } else { + M5_LOGI("getPosition::Command is only supprted in RT_DYN_XL330"); + } +}; void StackchanSERVO::attachServos() { if (_servo_type == ServoType::SCS) { @@ -68,6 +83,38 @@ void StackchanSERVO::attachServos() { //_dxl.torqueOff(AXIS_X + 1); //_dxl.torqueOff(AXIS_Y + 1); + } else if (_servo_type == ServoType::RT_DYN_XL330){ + M5_LOGI("RT_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_EXTENDED_POSITION); + _dxl.setOperatingMode(AXIS_Y + 1, OP_EXTENDED_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); + + if (_dxl.getPresentPosition(AXIS_X + 1) > 4096) { + _init_param.servo[AXIS_X].offset = _init_param.servo[AXIS_X].offset + 360; + } + if (_dxl.getPresentPosition(AXIS_Y + 1) > 4096) { + _init_param.servo[AXIS_Y].offset = _init_param.servo[AXIS_Y].offset + 360; + } + + _dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(_init_param.servo[AXIS_X].start_degree + _init_param.servo[AXIS_X].offset)); + _dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(_init_param.servo[AXIS_Y].start_degree + _init_param.servo[AXIS_Y].offset)); + //_dxl.torqueOff(AXIS_X + 1); + //_dxl.torqueOff(AXIS_Y + 1); + } else { // SG90 PWM if (_servo_x.attach(_init_param.servo[AXIS_X].pin, @@ -122,7 +169,16 @@ void StackchanSERVO::moveX(int x, uint32_t millis_for_move) { _isMoving = true; vTaskDelay(millis_for_move/portTICK_PERIOD_MS); _isMoving = false; - } else { + } else if (_servo_type == ServoType::RT_DYN_XL330) { + _dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, millis_for_move); + vTaskDelay(10/portTICK_PERIOD_MS); + _dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(x + _init_param.servo[AXIS_X].offset)); + vTaskDelay(10/portTICK_PERIOD_MS); + _isMoving = true; + vTaskDelay(millis_for_move/portTICK_PERIOD_MS); + _isMoving = false; + M5_LOGI("X:%f", getPosition(AXIS_X+1)); + }else { if (millis_for_move == 0) { _servo_x.easeTo(x + _init_param.servo[AXIS_X].offset); } else { @@ -154,6 +210,15 @@ void StackchanSERVO::moveY(int y, uint32_t millis_for_move) { _isMoving = true; vTaskDelay(millis_for_move/portTICK_PERIOD_MS); _isMoving = false; + } else if (_servo_type == ServoType::RT_DYN_XL330) { + _dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, millis_for_move); + vTaskDelay(10/portTICK_PERIOD_MS); + _dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(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; + M5_LOGI("Y:%f", getPosition(AXIS_Y+1)); } else { if (millis_for_move == 0) { _servo_y.easeTo(y + _init_param.servo[AXIS_Y].offset); @@ -191,6 +256,12 @@ void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) { _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 if (_servo_type == ServoType::RT_DYN_XL330) { + _isMoving = true; + _dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(x + _init_param.servo[AXIS_X].offset)); + _dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(y + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。 + _isMoving = false; + M5_LOGI("X:%f, Y:%f", getPosition(AXIS_X+1), getPosition(AXIS_Y+1)); } 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); @@ -215,6 +286,12 @@ void StackchanSERVO::moveXY(servo_param_s servo_param_x, servo_param_s servo_par _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_type == ServoType::RT_DYN_XL330) { + _isMoving = true; + _dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(servo_param_x.degree + _init_param.servo[AXIS_X].offset)); + _dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(servo_param_y.degree + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。 + _isMoving = false; + M5_LOGI("X:%f, Y:%f", getPosition(AXIS_X+1), getPosition(AXIS_Y+1)); } else { if (servo_param_x.degree != 0) { _servo_x.setEaseToD(servo_param_x.degree + servo_param_x.offset, servo_param_x.millis_for_move); diff --git a/src/Stackchan_servo.h b/src/Stackchan_servo.h index d66124d..d36c2aa 100644 --- a/src/Stackchan_servo.h +++ b/src/Stackchan_servo.h @@ -34,7 +34,8 @@ enum ServoAxis { enum ServoType { PWM, // SG90 PWM SCS, // Feetech SCS0009 - DYN_XL330 // Dynamixel XL330 + DYN_XL330, // Dynamixel XL330 + RT_DYN_XL330 // Dynamixel XL330 on RT version stackchan }; typedef struct ServoParam { @@ -70,6 +71,9 @@ class StackchanSERVO { public: StackchanSERVO(); ~StackchanSERVO(); + + float getPosition(int x); + void begin(stackchan_servo_initial_param_s init_params); void begin(int servo_pin_x, int16_t start_degree_x, int16_t offset_x, int servo_pin_y, int16_t start_degree_y, int16_t offset_y, diff --git a/src/Stackchan_system_config.cpp b/src/Stackchan_system_config.cpp index ec83865..761348c 100644 --- a/src/Stackchan_system_config.cpp +++ b/src/Stackchan_system_config.cpp @@ -179,6 +179,9 @@ void StackchanSystemConfig::setSystemConfig(DynamicJsonDocument doc) { if (_servo_type_str.indexOf("SCS") != -1) { // SCS0009 _servo_type = ServoType::SCS; + } else if (_servo_type_str.indexOf("RT_DYN_XL330") != -1) { + // Dynamixel XL330 for RT Version + _servo_type = ServoType::RT_DYN_XL330; } else if (_servo_type_str.indexOf("DYN_XL330") != -1) { // Dynamixel XL330 _servo_type = ServoType::DYN_XL330; From d8478dcc7959ed7ac6fa9dbee495301b32c7b65a Mon Sep 17 00:00:00 2001 From: starf555 Date: Mon, 11 Nov 2024 00:17:27 +0900 Subject: [PATCH 2/2] Add values for RT Version --- data/yaml/SC_BasicConfig.yaml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/data/yaml/SC_BasicConfig.yaml b/data/yaml/SC_BasicConfig.yaml index 45fa40d..03a6c62 100644 --- a/data/yaml/SC_BasicConfig.yaml +++ b/data/yaml/SC_BasicConfig.yaml @@ -6,6 +6,7 @@ servo: # 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 or Dynamixel XL330, x:RX, y:TX (not used) + # RT Version: x:6 y:7 x: 17 y: 18 offset: @@ -17,6 +18,7 @@ servo: # SG90: x:90 y:90 # SCS0009: x:150, y:150 # Dynamixel XL330: x:180, y:270 + # RT Version X:180 Y:5 x: 90 y: 90 lower_limit: @@ -24,6 +26,7 @@ servo: # SG90: x:0, y:60 # SCS0009: x:0, y:120 # Dynamixel XL330: x:0, y:220 + # RT Version X:90 Y:-5 x: 0 y: 60 upper_limit: @@ -31,6 +34,7 @@ servo: # SG90: x:180, y:90 # SCS0009: x:300, y:150 # Dynamixel XL330: x:360, y:270 + # Dynamixel RTVersion X:270 Y:15 x: 90 y: 90 speed: @@ -45,7 +49,7 @@ servo: move_min: 500 move_max: 1000 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 +servo_type: "PWM" # "PWM": SG90PWMServo, "SCS": Feetech SCS0009 "DYN_XL330": Dynamixel XL330, "RT_DYN_XL339": RTVersion ### 以下はアプリケーションによって設定が変わります。 bluetooth: