Skip to content

Commit 8623e5c

Browse files
mongonta0716mongonta0716
authored
Merge branch 'main' into develop
2 parents ea0e8f4 + 9205ccf commit 8623e5c

File tree

5 files changed

+104
-11
lines changed

5 files changed

+104
-11
lines changed

‎data/yaml/SC_BasicConfig.yaml‎

Lines changed: 6 additions & 2 deletions
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,21 +18,24 @@ 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
34-
x: 90
37+
# Dynamixel RTVersion X:270 Y:15
38+
x: 180
3539
y: 90
3640
speed:
3741
normal_mode:
@@ -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_XL330": RTVersion
4953

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

‎src/Stackchan_servo.cpp‎

Lines changed: 84 additions & 2 deletions
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) {
@@ -58,7 +73,7 @@ void StackchanSERVO::attachServos() {
5873
_dxl.writeControlTableItem(DRIVE_MODE, AXIS_X + 1, 4); // Velocityのパラメータを移動時間(msec)で指定するモードに変更
5974
_dxl.writeControlTableItem(DRIVE_MODE, AXIS_Y + 1, 4); // Velocityのパラメータを移動時間(msec)で指定するモードに変更
6075
_dxl.torqueOn(AXIS_X + 1);
61-
delay(10); // ここでWaitを入れないと、Y(tilt)サーボが動かない場合がある。
76+
delay(100); // ここでWaitを入れないと、Y(tilt)サーボが動かない場合がある。
6277
_dxl.torqueOn(AXIS_Y + 1);
6378
delay(100);
6479
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, 1000);
@@ -69,6 +84,43 @@ void StackchanSERVO::attachServos() {
6984
//_dxl.torqueOff(AXIS_X + 1);
7085
//_dxl.torqueOff(AXIS_Y + 1);
7186

87+
} else if (_servo_type == ServoType::RT_DYN_XL330){
88+
M5_LOGI("RT_DYN_XL330");
89+
Serial2.begin(1000000, SERIAL_8N1, _init_param.servo[AXIS_X].pin, _init_param.servo[AXIS_Y].pin);
90+
_dxl = Dynamixel2Arduino(Serial2);
91+
_dxl.begin(1000000);
92+
_dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
93+
_dxl.ping(AXIS_X + 1);
94+
_dxl.ping(AXIS_Y + 1);
95+
_dxl.setOperatingMode(AXIS_X + 1, OP_EXTENDED_POSITION);
96+
_dxl.setOperatingMode(AXIS_Y + 1, OP_EXTENDED_POSITION);
97+
_dxl.writeControlTableItem(DRIVE_MODE, AXIS_X + 1, 4); // Velocityのパラメータを移動時間(msec)で指定するモードに変更
98+
_dxl.writeControlTableItem(DRIVE_MODE, AXIS_Y + 1, 4); // Velocityのパラメータを移動時間(msec)で指定するモードに変更
99+
_dxl.torqueOn(AXIS_X + 1);
100+
delay(10); // ここでWaitを入れないと、Y(tilt)サーボが動かない場合がある。
101+
_dxl.torqueOn(AXIS_Y + 1);
102+
delay(100);
103+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, 1000);
104+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, 1000);
105+
delay(100);
106+
107+
M5_LOGI("CurrentPosition X:%f, Y:%f", _dxl.getPresentPosition(AXIS_X + 1), _dxl.getPresentPosition(AXIS_Y + 1));
108+
109+
if (_dxl.getPresentPosition(AXIS_X + 1) > 4096) {
110+
_init_param.servo[AXIS_X].offset = _init_param.servo[AXIS_X].offset + 360;
111+
}
112+
if ((_dxl.getPresentPosition(AXIS_Y + 1)-convertDYNIXELXL330_RT(_init_param.servo[AXIS_Y].lower_limit + _init_param.servo[AXIS_Y].offset)) > convertDYNIXELXL330_RT(270)) {
113+
_init_param.servo[AXIS_Y].offset = _init_param.servo[AXIS_Y].offset + 360;
114+
}
115+
//_init_param.servo[AXIS_Y].offset = 360;
116+
117+
M5_LOGI("Current Offset X:%d, Y:%d", _init_param.servo[AXIS_X].offset, _init_param.servo[AXIS_Y].offset);
118+
119+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(_init_param.servo[AXIS_X].start_degree + _init_param.servo[AXIS_X].offset));
120+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(_init_param.servo[AXIS_Y].start_degree + _init_param.servo[AXIS_Y].offset));
121+
//_dxl.torqueOff(AXIS_X + 1);
122+
//_dxl.torqueOff(AXIS_Y + 1);
123+
72124
} else {
73125
// SG90 PWM
74126
if (_servo_x.attach(_init_param.servo[AXIS_X].pin,
@@ -123,7 +175,16 @@ void StackchanSERVO::moveX(int x, uint32_t millis_for_move) {
123175
_isMoving = true;
124176
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
125177
_isMoving = false;
126-
} else {
178+
} else if (_servo_type == ServoType::RT_DYN_XL330) {
179+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, millis_for_move);
180+
vTaskDelay(10/portTICK_PERIOD_MS);
181+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(x + _init_param.servo[AXIS_X].offset));
182+
vTaskDelay(10/portTICK_PERIOD_MS);
183+
_isMoving = true;
184+
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
185+
_isMoving = false;
186+
M5_LOGI("X:%f", getPosition(AXIS_X+1));
187+
}else {
127188
if (millis_for_move == 0) {
128189
_servo_x.easeTo(x + _init_param.servo[AXIS_X].offset);
129190
} else {
@@ -155,6 +216,15 @@ void StackchanSERVO::moveY(int y, uint32_t millis_for_move) {
155216
_isMoving = true;
156217
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
157218
_isMoving = false;
219+
} else if (_servo_type == ServoType::RT_DYN_XL330) {
220+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, millis_for_move);
221+
vTaskDelay(10/portTICK_PERIOD_MS);
222+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(y + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
223+
vTaskDelay(10/portTICK_PERIOD_MS);
224+
_isMoving = true;
225+
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
226+
_isMoving = false;
227+
M5_LOGI("Y:%f", getPosition(AXIS_Y+1));
158228
} else {
159229
if (millis_for_move == 0) {
160230
_servo_y.easeTo(y + _init_param.servo[AXIS_Y].offset);
@@ -192,6 +262,12 @@ void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) {
192262
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330(x + _init_param.servo[AXIS_X].offset));
193263
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330(y + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
194264
_isMoving = false;
265+
} else if (_servo_type == ServoType::RT_DYN_XL330) {
266+
_isMoving = true;
267+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(x + _init_param.servo[AXIS_X].offset));
268+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(y + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
269+
_isMoving = false;
270+
M5_LOGI("X:%f, Y:%f", getPosition(AXIS_X+1), getPosition(AXIS_Y+1));
195271
} else {
196272
_servo_x.setEaseToD(x + _init_param.servo[AXIS_X].offset, millis_for_move);
197273
_servo_y.setEaseToD(y + _init_param.servo[AXIS_Y].offset, millis_for_move);
@@ -216,6 +292,12 @@ void StackchanSERVO::moveXY(servo_param_s servo_param_x, servo_param_s servo_par
216292
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330(servo_param_x.degree + _init_param.servo[AXIS_X].offset));
217293
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330(servo_param_y.degree + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
218294
_isMoving = false;
295+
} else if (_servo_type == ServoType::RT_DYN_XL330) {
296+
_isMoving = true;
297+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(servo_param_x.degree + _init_param.servo[AXIS_X].offset));
298+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(servo_param_y.degree + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
299+
_isMoving = false;
300+
M5_LOGI("X:%f, Y:%f", getPosition(AXIS_X+1), getPosition(AXIS_Y+1));
219301
} else {
220302
if (servo_param_x.degree != 0) {
221303
_servo_x.setEaseToD(servo_param_x.degree + servo_param_x.offset, servo_param_x.millis_for_move);

‎src/Stackchan_servo.h‎

Lines changed: 7 additions & 3 deletions
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 {
@@ -43,8 +44,8 @@ typedef struct ServoParam {
4344
int16_t offset; // オフセット（90°からの+-）
4445
int16_t degree; // 角度
4546
uint32_t millis_for_move; // 移動時間(msec)
46-
uint16_t lower_limit; // サーボ角度の下限
47-
uint16_t upper_limit; // サーボ角度の上限
47+
int16_t lower_limit; // サーボ角度の下限
48+
int16_t upper_limit; // サーボ角度の上限
4849
} servo_param_s;
4950

5051

@@ -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
@@ -181,6 +181,9 @@ void StackchanSystemConfig::setSystemConfig(DynamicJsonDocument doc) {
181181
if (_servo_type_str.indexOf("SCS") != -1) {
182182
// SCS0009
183183
_servo_type = ServoType::SCS;
184+
} else if (_servo_type_str.indexOf("RT_DYN_XL330") != -1) {
185+
// Dynamixel XL330 for RT Version
186+
_servo_type = ServoType::RT_DYN_XL330;
184187
} else if (_servo_type_str.indexOf("DYN_XL330") != -1) {
185188
// Dynamixel XL330
186189
_servo_type = ServoType::DYN_XL330;

‎src/Stackchan_system_config.h‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -41,10 +41,10 @@ typedef struct SecretItems {
4141

4242
typedef struct ServoInitialParam {
4343
uint8_t pin;
44-
uint16_t offset;
45-
uint16_t upper_limit;
46-
uint16_t lower_limit;
47-
uint16_t start_degree;
44+
int16_t offset;
45+
int16_t upper_limit;
46+
int16_t lower_limit;
47+
int16_t start_degree;
4848
} servo_initial_param_s;
4949

5050
enum AvatarMode {

0 commit comments

Comments
 (0)