Skip to content

Add RTversion Dynamixel #6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Nov 10, 2024
Merged

Add RTversion Dynamixel #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
aa95e64
Add RTversion Dynamixel
starf555 Nov 10, 2024
d8478dc
Add values for RT Version
starf555 Nov 10, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion data/yaml/SC_BasicConfig.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -17,20 +18,23 @@ 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:
# 可動範囲の下限（下限と言っても取り付け方により逆の場合あり, 値の小さい方を指定。）
# 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:
# 可動範囲の上限（上限と言っても取り付け方により逆の場合もあり, 値の大きい方を指定。）
# 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:
Expand All @@ -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:
Expand Down
79 changes: 78 additions & 1 deletion src/Stackchan_servo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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) {
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down
6 changes: 5 additions & 1 deletion src/Stackchan_servo.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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,
Expand Down
3 changes: 3 additions & 0 deletions src/Stackchan_system_config.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down