Skip to content

Commit cd3dde1

Browse files
mongonta0716mongonta0716
committed
Support Dynamixel XL330
1 parent b47c68a commit cd3dde1

File tree

6 files changed

+133
-12
lines changed

6 files changed

+133
-12
lines changed

‎.gitignore

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,3 +30,8 @@
3030
*.exe
3131
*.out
3232
*.app
33+
34+
lib/
35+
.vscode/
36+
.pio/
37+
build/

‎data/yaml/SC_BasicConfig.yaml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@ balloon:
4141
led_lr: 0 # 0:stereo, 1:left_only, 2:right_only
4242
led_pin: 15 # GoBottom1:15 GoBottom2:25
4343
takao_base: false # Whether to use takaobase to feed power from the rear connector.(Stack-chan_Takao_Base https://ssci.to/8905)
44-
servo_type: "PWM" # "PWM": SG90PWMServo, "SCS": Feetech SCS0009
44+
servo_type: "PWM" # "PWM": SG90PWMServo, "SCS": Feetech SCS0009 "DYN_XL330": Dynamixel XL330
4545
extend_config_filename: "/yaml/SC_ExConfig.yaml" # Configuration file for the application.
4646
extend_config_filesize: 2048 # Buffer size for feature extensions
4747
secret_config_filename: "/yaml/SC_SecConfig.yaml" # Configuration file for the File for personal information.

‎platformoio.ini

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
; PlatformIO Project Configuration File
2+
;
3+
; Build options: build flags, source filter
4+
; Upload options: custom upload port, speed and extra flags
5+
; Library options: dependencies, extra library storages
6+
; Advanced options: extra scripting
7+
;
8+
; Please visit documentation for the other options and examples
9+
; http://docs.platformio.org/page/projectconf.html
10+
11+
[platformio]
12+
default_envs = m5stack-core
13+
14+
[env]
15+
platform = espressif32
16+
framework = arduino
17+
monitor_speed = 115200
18+
build_flags =
19+
-Llib/
20+
21+
[env:m5stack-core]
22+
board = m5stack-core-esp32
23+
lib_deps = m5stack/M5Unified
24+
25+
[env:m5stack-core-with-aquestalk]
26+
board = m5stack-core-esp32
27+
lib_deps = m5stack/M5Unified
28+
29+
[env:m5stack-fire]
30+
board = m5stack-fire
31+
lib_deps =
32+
m5stack/M5Unified
33+
34+
[env:m5stack-core2]
35+
board = m5stack-core2
36+
lib_deps =
37+
m5stack/M5Unified

‎src/Stackchan_servo.cpp

Lines changed: 65 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,14 @@ long convertSCS0009Pos(int16_t degree) {
88
return map(degree, 0, 300, 1023, 0);
99
}
1010

11+
long convertDYNIXELXL330(int16_t degree) {
12+
M5_LOGI("Degree: %d\n", degree);
13+
14+
long ret = map(degree, 0, 360, 0, 4095);
15+
M5_LOGI("Position: %d\n", ret);
16+
return ret;
17+
}
18+
1119
// シリアルサーボ用のEasing関数
1220
float quadraticEaseInOut(float p) {
1321
//return p;
@@ -21,20 +29,41 @@ float quadraticEaseInOut(float p) {
2129
}
2230
}
2331

32+
2433
StackchanSERVO::StackchanSERVO() {}
2534

2635
StackchanSERVO::~StackchanSERVO() {}
2736

2837

2938
void StackchanSERVO::attachServos() {
30-
if (_servo_type == SCS) {
39+
if (_servo_type == ServoType::SCS) {
3140
// SCS0009
3241
Serial2.begin(1000000, SERIAL_8N1, _init_param.servo[AXIS_X].pin, _init_param.servo[AXIS_Y].pin);
3342
_sc.pSerial = &Serial2;
3443
_sc.WritePos(AXIS_X + 1, convertSCS0009Pos(_init_param.servo[AXIS_X].start_degree + _init_param.servo[AXIS_X].offset), 1000);
3544
_sc.WritePos(AXIS_Y + 1, convertSCS0009Pos(_init_param.servo[AXIS_Y].start_degree + _init_param.servo[AXIS_Y].offset), 1000);
3645
vTaskDelay(1000/portTICK_PERIOD_MS);
3746

47+
} else if (_servo_type == ServoType::DYN_XL330) {
48+
M5_LOGI("DYN_XL330");
49+
Serial2.begin(1000000, SERIAL_8N1, _init_param.servo[AXIS_X].pin, _init_param.servo[AXIS_Y].pin);
50+
_dxl = Dynamixel2Arduino(Serial2);
51+
_dxl.begin(1000000);
52+
_dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
53+
_dxl.ping(AXIS_X + 1);
54+
_dxl.ping(AXIS_Y + 1);
55+
_dxl.torqueOn(AXIS_X + 1);
56+
_dxl.torqueOn(AXIS_Y + 1);
57+
_dxl.setOperatingMode(AXIS_X + 1, OP_POSITION);
58+
_dxl.setOperatingMode(AXIS_Y + 1, OP_POSITION);
59+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, 100);
60+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, 100);
61+
delay(1000);
62+
_dxl.setGoalPosition(AXIS_X + 1, 2048);
63+
_dxl.setGoalPosition(AXIS_Y + 1, 3073);
64+
//_dxl.torqueOff(AXIS_X + 1);
65+
//_dxl.torqueOff(AXIS_Y + 1);
66+
3867
} else {
3968
// SG90 PWM
4069
if (_servo_x.attach(_init_param.servo[AXIS_X].pin,
@@ -81,6 +110,13 @@ void StackchanSERVO::moveX(int x, uint32_t millis_for_move) {
81110
_isMoving = true;
82111
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
83112
_isMoving = false;
113+
} else if (_servo_type == ServoType::DYN_XL330) {
114+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, calcVelocity(ServoAxis::AXIS_X, x, millis_for_move));
115+
vTaskDelay(10/portTICK_PERIOD_MS);
116+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330(x + _init_param.servo[AXIS_X].offset));
117+
_isMoving = true;
118+
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
119+
_isMoving = false;
84120
} else {
85121
if (millis_for_move == 0) {
86122
_servo_x.easeTo(x + _init_param.servo[AXIS_X].offset);
@@ -100,11 +136,18 @@ void StackchanSERVO::moveX(servo_param_s servo_param_x) {
100136
}
101137

102138
void StackchanSERVO::moveY(int y, uint32_t millis_for_move) {
103-
if (_servo_type == SCS) {
139+
if (_servo_type == ServoType::SCS) {
104140
_sc.WritePos(AXIS_Y + 1, convertSCS0009Pos(y + _init_param.servo[AXIS_Y].offset), millis_for_move);
105141
_isMoving = true;
106142
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
107143
_isMoving = false;
144+
} else if (_servo_type == ServoType::DYN_XL330) {
145+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, calcVelocity(ServoAxis::AXIS_Y, y, millis_for_move));
146+
vTaskDelay(10/portTICK_PERIOD_MS);
147+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330(y + 90 + _init_param.servo[AXIS_Y].offset));
148+
_isMoving = true;
149+
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
150+
_isMoving = false;
108151
} else {
109152
if (millis_for_move == 0) {
110153
_servo_y.easeTo(y + _init_param.servo[AXIS_Y].offset);
@@ -123,7 +166,7 @@ void StackchanSERVO::moveY(servo_param_s servo_param_y) {
123166
moveY(servo_param_y.degree, servo_param_y.millis_for_move);
124167
}
125168
void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) {
126-
if (_servo_type == SCS) {
169+
if (_servo_type == ServoType::SCS) {
127170
int increase_degree_x = x - _last_degree_x;
128171
int increase_degree_y = y - _last_degree_y;
129172
uint32_t division_time = millis_for_move / SERIAL_EASE_DIVISION;
@@ -137,6 +180,7 @@ void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) {
137180
//vTaskDelay(division_time);
138181
}
139182
_isMoving = false;
183+
} else if (_servo_type == ServoType::DYN_XL330) {
140184
} else {
141185
_servo_x.setEaseToD(x + _init_param.servo[AXIS_X].offset, millis_for_move);
142186
_servo_y.setEaseToD(y + _init_param.servo[AXIS_Y].offset, millis_for_move);
@@ -150,12 +194,13 @@ void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) {
150194
}
151195

152196
void StackchanSERVO::moveXY(servo_param_s servo_param_x, servo_param_s servo_param_y) {
153-
if (_servo_type == SCS) {
197+
if (_servo_type == ServoType::SCS) {
154198
_sc.WritePos(AXIS_X + 1, convertSCS0009Pos(servo_param_x.degree + servo_param_x.offset), servo_param_x.millis_for_move);
155199
_sc.WritePos(AXIS_Y + 1, convertSCS0009Pos(servo_param_y.degree + servo_param_y.offset), servo_param_y.millis_for_move);
156200
_isMoving = true;
157201
vTaskDelay(max(servo_param_x.millis_for_move, servo_param_y.millis_for_move)/portTICK_PERIOD_MS);
158202
_isMoving = false;
203+
} else if (_servo_type == ServoType::DYN_XL330) {
159204
} else {
160205
if (servo_param_x.degree != 0) {
161206
_servo_x.setEaseToD(servo_param_x.degree + servo_param_x.offset, servo_param_x.millis_for_move);
@@ -211,3 +256,19 @@ void StackchanSERVO::motion(Motion motion_number) {
211256
delay(1000);
212257
moveXY(_init_param.servo[AXIS_X].start_degree, _init_param.servo[AXIS_Y].degree, 1000);
213258
}
259+
260+
// Dynamixel XL330では移動時間の指定はできないので変換する。
261+
long StackchanSERVO::calcVelocity(ServoAxis axis_id, int16_t degree, uint32_t millis_for_move) {
262+
int16_t move_degree = 0;
263+
if (axis_id == ServoAxis::AXIS_X) {
264+
move_degree = abs(_last_degree_x - degree);
265+
} else {
266+
move_degree = abs(_last_degree_y - degree);
267+
}
268+
269+
long rpm = (move_degree / 360) / (60 * (millis_for_move / 1000));
270+
M5_LOGI("RPM: %d\n", rpm);
271+
//return rpm;
272+
273+
return 300;
274+
}

‎src/Stackchan_servo.h

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,10 @@
1111
#include <ServoEasing.h>
1212
#include <SCServo.h>
1313
#include <M5Unified.h>
14+
#include <Dynamixel2Arduino.h>
15+
16+
using namespace ControlTableItem;
17+
1418
#define SERIAL_EASE_DIVISION 5 // シリアルサーボのEasing分割数
1519

1620
enum Motion {
@@ -28,8 +32,9 @@ enum ServoAxis {
2832
};
2933

3034
enum ServoType {
31-
PWM,
32-
SCS
35+
PWM, // SG90 PWM
36+
SCS, // Feetech SCS0009
37+
DYN_XL330 // Dynamixel XL330
3338
};
3439

3540
typedef struct ServoParam {
@@ -47,10 +52,14 @@ typedef struct StackchanServo{
4752
servo_param_s servo[2];
4853
} stackchan_servo_initial_param_s;
4954

55+
56+
const float DXL_PROTOCOL_VERSION = 2.0f;
57+
5058
class StackchanSERVO {
5159
protected:
5260
ServoType _servo_type;
5361
SCSCL _sc;
62+
Dynamixel2Arduino _dxl;
5463
ServoEasing _servo_x;
5564
ServoEasing _servo_y;
5665
void attachServos();
@@ -73,6 +82,6 @@ class StackchanSERVO {
7382
void moveXY(servo_param_s servo_param_x, servo_param_s servo_param_y);
7483
void motion(Motion motion_no);
7584
bool isMoving() { return _isMoving; }
85+
long calcVelocity(ServoAxis axis_id, int16_t degree, uint32_t millis_for_move);
7686
};
77-
7887
#endif // _STACKCHAN_SERVO_H_

‎src/Stackchan_system_config.cpp

Lines changed: 13 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ void StackchanSystemConfig::setDefaultParameters() {
6161
_led_lr = 0;
6262
_led_pin = -1;
6363
_takao_base = false;
64-
_servo_type = 0;
64+
_servo_type = ServoType::PWM;
6565
_servo[AXIS_X].start_degree = 90;
6666
_servo[AXIS_Y].start_degree = 90;
6767
_secret_config_show = false;
@@ -173,11 +173,16 @@ void StackchanSystemConfig::setSystemConfig(DynamicJsonDocument doc) {
173173
_servo_type_str = doc["servo_type"].as<String>();
174174
if (_servo_type_str.indexOf("SCS") != -1) {
175175
// SCS0009
176-
_servo_type = 1;
176+
_servo_type = ServoType::SCS;
177177
_servo[AXIS_X].start_degree = 150;
178178
_servo[AXIS_Y].start_degree = 150;
179-
} else {
180-
_servo_type = 0; // PWMサーボ
179+
} else if (_servo_type_str.indexOf("DYN_XL330") != -1) {
180+
// Dynamixel XL330
181+
_servo_type = ServoType::DYN_XL330;
182+
_servo[AXIS_X].start_degree = 180;
183+
_servo[AXIS_Y].start_degree = 180;
184+
} else {
185+
_servo_type = ServoType::PWM; // PWMサーボ
181186
_servo[AXIS_X].start_degree = 90;
182187
_servo[AXIS_Y].start_degree = 90;
183188
}
@@ -211,6 +216,10 @@ void StackchanSystemConfig::printAllParameters() {
211216
M5_LOGI("servo:pin_y:%d", _servo[AXIS_Y].pin);
212217
M5_LOGI("servo:offset_x:%d", _servo[AXIS_X].offset);
213218
M5_LOGI("servo:offset_y:%d", _servo[AXIS_Y].offset);
219+
M5_LOGI("servo.lower_limit_x:%d", _servo[AXIS_X].lower_limit);
220+
M5_LOGI("servo.lower_limit_y:%d", _servo[AXIS_Y].lower_limit);
221+
M5_LOGI("servo.upper_limit_x:%d", _servo[AXIS_X].upper_limit);
222+
M5_LOGI("servo.upper_limit_y:%d", _servo[AXIS_Y].upper_limit);
214223
for (int i=0;i<_mode_num;i++) {
215224
M5_LOGI("mode:%s", _servo_interval[i].mode_name);
216225
M5_LOGI("interval_min:%d", _servo_interval[i].interval_min);

0 commit comments

Comments
 (0)