Skip to content

Commit 3225daa

Browse files
mongonta0716mongonta0716
authored
Merge pull request #6 from starf555/RT_TEST
Add RTversion Dynamixel
2 parents 657df52 + d8478dc commit 3225daa

File tree

4 files changed

+91
-3
lines changed

4 files changed

+91
-3
lines changed

‎data/yaml/SC_BasicConfig.yaml

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ servo:
66
# CoreS3 PortA X:2, Y:1 PortB X:9, Y:8 PortC X:17, Y:18
77
# Stack-chanPCB Core1 X:5,Y:2 Core2 X:19,Y27
88
# When using SCS0009 or Dynamixel XL330, x:RX, y:TX (not used)
9+
# RT Version: x:6 y:7
910
x: 17
1011
y: 18
1112
offset:
@@ -17,20 +18,23 @@ servo:
1718
# SG90: x:90 y:90
1819
# SCS0009: x:150, y:150
1920
# Dynamixel XL330: x:180, y:270
21+
# RT Version X:180 Y:5
2022
x: 90
2123
y: 90
2224
lower_limit:
2325
# 可動範囲の下限（下限と言っても取り付け方により逆の場合あり, 値の小さい方を指定。）
2426
# SG90: x:0, y:60
2527
# SCS0009: x:0, y:120
2628
# Dynamixel XL330: x:0, y:220
29+
# RT Version X:90 Y:-5
2730
x: 0
2831
y: 60
2932
upper_limit:
3033
# 可動範囲の上限（上限と言っても取り付け方により逆の場合もあり, 値の大きい方を指定。）
3134
# SG90: x:180, y:90
3235
# SCS0009: x:300, y:150
3336
# Dynamixel XL330: x:360, y:270
37+
# Dynamixel RTVersion X:270 Y:15
3438
x: 90
3539
y: 90
3640
speed:
@@ -45,7 +49,7 @@ servo:
4549
move_min: 500
4650
move_max: 1000
4751
takao_base: false # Whether to use takaobase to feed power from the rear connector.(Stack-chan_Takao_Base https://ssci.to/8905)
48-
servo_type: "PWM" # "PWM": SG90PWMServo, "SCS": Feetech SCS0009 "DYN_XL330": Dynamixel XL330
52+
servo_type: "PWM" # "PWM": SG90PWMServo, "SCS": Feetech SCS0009 "DYN_XL330": Dynamixel XL330, "RT_DYN_XL339": RTVersion
4953

5054
### 以下はアプリケーションによって設定が変わります。
5155
bluetooth:

‎src/Stackchan_servo.cpp

Lines changed: 78 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,14 @@ static long convertDYNIXELXL330(int16_t degree) {
1616
return ret;
1717
}
1818

19+
static long convertDYNIXELXL330_RT(int16_t degree) {
20+
M5_LOGI("Degree: %d\n", degree);
21+
22+
long ret = map(degree, -360, 720, -4095, 8191);
23+
M5_LOGI("Position: %d\n", ret);
24+
return ret;
25+
}
26+
1927
// シリアルサーボ用のEasing関数
2028
float quadraticEaseInOut(float p) {
2129
//return p;
@@ -34,6 +42,13 @@ StackchanSERVO::StackchanSERVO() {}
3442

3543
StackchanSERVO::~StackchanSERVO() {}
3644

45+
float StackchanSERVO::getPosition(int x){
46+
if (_servo_type == RT_DYN_XL330){
47+
return _dxl.getPresentPosition(x);;
48+
} else {
49+
M5_LOGI("getPosition::Command is only supprted in RT_DYN_XL330");
50+
}
51+
};
3752

3853
void StackchanSERVO::attachServos() {
3954
if (_servo_type == ServoType::SCS) {
@@ -68,6 +83,38 @@ void StackchanSERVO::attachServos() {
6883
//_dxl.torqueOff(AXIS_X + 1);
6984
//_dxl.torqueOff(AXIS_Y + 1);
7085

86+
} else if (_servo_type == ServoType::RT_DYN_XL330){
87+
M5_LOGI("RT_DYN_XL330");
88+
Serial2.begin(1000000, SERIAL_8N1, _init_param.servo[AXIS_X].pin, _init_param.servo[AXIS_Y].pin);
89+
_dxl = Dynamixel2Arduino(Serial2);
90+
_dxl.begin(1000000);
91+
_dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
92+
_dxl.ping(AXIS_X + 1);
93+
_dxl.ping(AXIS_Y + 1);
94+
_dxl.setOperatingMode(AXIS_X + 1, OP_EXTENDED_POSITION);
95+
_dxl.setOperatingMode(AXIS_Y + 1, OP_EXTENDED_POSITION);
96+
_dxl.writeControlTableItem(DRIVE_MODE, AXIS_X + 1, 4); // Velocityのパラメータを移動時間(msec)で指定するモードに変更
97+
_dxl.writeControlTableItem(DRIVE_MODE, AXIS_Y + 1, 4); // Velocityのパラメータを移動時間(msec)で指定するモードに変更
98+
_dxl.torqueOn(AXIS_X + 1);
99+
delay(10); // ここでWaitを入れないと、Y(tilt)サーボが動かない場合がある。
100+
_dxl.torqueOn(AXIS_Y + 1);
101+
delay(100);
102+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, 1000);
103+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, 1000);
104+
delay(100);
105+
106+
if (_dxl.getPresentPosition(AXIS_X + 1) > 4096) {
107+
_init_param.servo[AXIS_X].offset = _init_param.servo[AXIS_X].offset + 360;
108+
}
109+
if (_dxl.getPresentPosition(AXIS_Y + 1) > 4096) {
110+
_init_param.servo[AXIS_Y].offset = _init_param.servo[AXIS_Y].offset + 360;
111+
}
112+
113+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(_init_param.servo[AXIS_X].start_degree + _init_param.servo[AXIS_X].offset));
114+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(_init_param.servo[AXIS_Y].start_degree + _init_param.servo[AXIS_Y].offset));
115+
//_dxl.torqueOff(AXIS_X + 1);
116+
//_dxl.torqueOff(AXIS_Y + 1);
117+
71118
} else {
72119
// SG90 PWM
73120
if (_servo_x.attach(_init_param.servo[AXIS_X].pin,
@@ -122,7 +169,16 @@ void StackchanSERVO::moveX(int x, uint32_t millis_for_move) {
122169
_isMoving = true;
123170
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
124171
_isMoving = false;
125-
} else {
172+
} else if (_servo_type == ServoType::RT_DYN_XL330) {
173+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, millis_for_move);
174+
vTaskDelay(10/portTICK_PERIOD_MS);
175+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(x + _init_param.servo[AXIS_X].offset));
176+
vTaskDelay(10/portTICK_PERIOD_MS);
177+
_isMoving = true;
178+
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
179+
_isMoving = false;
180+
M5_LOGI("X:%f", getPosition(AXIS_X+1));
181+
}else {
126182
if (millis_for_move == 0) {
127183
_servo_x.easeTo(x + _init_param.servo[AXIS_X].offset);
128184
} else {
@@ -154,6 +210,15 @@ void StackchanSERVO::moveY(int y, uint32_t millis_for_move) {
154210
_isMoving = true;
155211
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
156212
_isMoving = false;
213+
} else if (_servo_type == ServoType::RT_DYN_XL330) {
214+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, millis_for_move);
215+
vTaskDelay(10/portTICK_PERIOD_MS);
216+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(y + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
217+
vTaskDelay(10/portTICK_PERIOD_MS);
218+
_isMoving = true;
219+
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
220+
_isMoving = false;
221+
M5_LOGI("Y:%f", getPosition(AXIS_Y+1));
157222
} else {
158223
if (millis_for_move == 0) {
159224
_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) {
191256
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330(x + _init_param.servo[AXIS_X].offset));
192257
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330(y + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
193258
_isMoving = false;
259+
} else if (_servo_type == ServoType::RT_DYN_XL330) {
260+
_isMoving = true;
261+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(x + _init_param.servo[AXIS_X].offset));
262+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(y + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
263+
_isMoving = false;
264+
M5_LOGI("X:%f, Y:%f", getPosition(AXIS_X+1), getPosition(AXIS_Y+1));
194265
} else {
195266
_servo_x.setEaseToD(x + _init_param.servo[AXIS_X].offset, millis_for_move);
196267
_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
215286
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330(servo_param_x.degree + _init_param.servo[AXIS_X].offset));
216287
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330(servo_param_y.degree + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
217288
_isMoving = false;
289+
} else if (_servo_type == ServoType::RT_DYN_XL330) {
290+
_isMoving = true;
291+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(servo_param_x.degree + _init_param.servo[AXIS_X].offset));
292+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(servo_param_y.degree + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
293+
_isMoving = false;
294+
M5_LOGI("X:%f, Y:%f", getPosition(AXIS_X+1), getPosition(AXIS_Y+1));
218295
} else {
219296
if (servo_param_x.degree != 0) {
220297
_servo_x.setEaseToD(servo_param_x.degree + servo_param_x.offset, servo_param_x.millis_for_move);

‎src/Stackchan_servo.h

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,8 @@ enum ServoAxis {
3434
enum ServoType {
3535
PWM, // SG90 PWM
3636
SCS, // Feetech SCS0009
37-
DYN_XL330 // Dynamixel XL330
37+
DYN_XL330, // Dynamixel XL330
38+
RT_DYN_XL330 // Dynamixel XL330 on RT version stackchan
3839
};
3940

4041
typedef struct ServoParam {
@@ -70,6 +71,9 @@ class StackchanSERVO {
7071
public:
7172
StackchanSERVO();
7273
~StackchanSERVO();
74+
75+
float getPosition(int x);
76+
7377
void begin(stackchan_servo_initial_param_s init_params);
7478
void begin(int servo_pin_x, int16_t start_degree_x, int16_t offset_x,
7579
int servo_pin_y, int16_t start_degree_y, int16_t offset_y,

‎src/Stackchan_system_config.cpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -179,6 +179,9 @@ void StackchanSystemConfig::setSystemConfig(DynamicJsonDocument doc) {
179179
if (_servo_type_str.indexOf("SCS") != -1) {
180180
// SCS0009
181181
_servo_type = ServoType::SCS;
182+
} else if (_servo_type_str.indexOf("RT_DYN_XL330") != -1) {
183+
// Dynamixel XL330 for RT Version
184+
_servo_type = ServoType::RT_DYN_XL330;
182185
} else if (_servo_type_str.indexOf("DYN_XL330") != -1) {
183186
// Dynamixel XL330
184187
_servo_type = ServoType::DYN_XL330;

0 commit comments

Comments
 (0)