Skip to content

Commit d7c87ac

Browse files
starf555starf555
committed
use signed int for the config value of servo
1 parent 657df52 commit d7c87ac

File tree

4 files changed

+94
-8
lines changed

4 files changed

+94
-8
lines changed

‎src/Stackchan_servo.cpp‎

Lines changed: 80 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,40 @@ 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+
M5_LOGI("CurrentPosition X:%d, Y:%d", _dxl.getPresentPosition(AXIS_X + 1), _dxl.getPresentPosition(AXIS_Y + 1));
107+
108+
if (_dxl.getPresentPosition(AXIS_X + 1) > 4096) {
109+
_init_param.servo[AXIS_X].offset = _init_param.servo[AXIS_X].offset + 360;
110+
}
111+
if (_dxl.getPresentPosition(AXIS_Y + 1) > 4096) {
112+
_init_param.servo[AXIS_Y].offset = _init_param.servo[AXIS_Y].offset + 360;
113+
}
114+
115+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(_init_param.servo[AXIS_X].start_degree + _init_param.servo[AXIS_X].offset));
116+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(_init_param.servo[AXIS_Y].start_degree + _init_param.servo[AXIS_Y].offset));
117+
//_dxl.torqueOff(AXIS_X + 1);
118+
//_dxl.torqueOff(AXIS_Y + 1);
119+
71120
} else {
72121
// SG90 PWM
73122
if (_servo_x.attach(_init_param.servo[AXIS_X].pin,
@@ -122,7 +171,16 @@ void StackchanSERVO::moveX(int x, uint32_t millis_for_move) {
122171
_isMoving = true;
123172
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
124173
_isMoving = false;
125-
} else {
174+
} else if (_servo_type == ServoType::RT_DYN_XL330) {
175+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_X + 1, millis_for_move);
176+
vTaskDelay(10/portTICK_PERIOD_MS);
177+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(x + _init_param.servo[AXIS_X].offset));
178+
vTaskDelay(10/portTICK_PERIOD_MS);
179+
_isMoving = true;
180+
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
181+
_isMoving = false;
182+
M5_LOGI("X:%f", getPosition(AXIS_X+1));
183+
}else {
126184
if (millis_for_move == 0) {
127185
_servo_x.easeTo(x + _init_param.servo[AXIS_X].offset);
128186
} else {
@@ -154,6 +212,15 @@ void StackchanSERVO::moveY(int y, uint32_t millis_for_move) {
154212
_isMoving = true;
155213
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
156214
_isMoving = false;
215+
} else if (_servo_type == ServoType::RT_DYN_XL330) {
216+
_dxl.writeControlTableItem(PROFILE_VELOCITY, AXIS_Y + 1, millis_for_move);
217+
vTaskDelay(10/portTICK_PERIOD_MS);
218+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(y + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
219+
vTaskDelay(10/portTICK_PERIOD_MS);
220+
_isMoving = true;
221+
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
222+
_isMoving = false;
223+
M5_LOGI("Y:%f", getPosition(AXIS_Y+1));
157224
} else {
158225
if (millis_for_move == 0) {
159226
_servo_y.easeTo(y + _init_param.servo[AXIS_Y].offset);
@@ -191,6 +258,12 @@ void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) {
191258
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330(x + _init_param.servo[AXIS_X].offset));
192259
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330(y + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
193260
_isMoving = false;
261+
} else if (_servo_type == ServoType::RT_DYN_XL330) {
262+
_isMoving = true;
263+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(x + _init_param.servo[AXIS_X].offset));
264+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(y + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
265+
_isMoving = false;
266+
M5_LOGI("X:%f, Y:%f", getPosition(AXIS_X+1), getPosition(AXIS_Y+1));
194267
} else {
195268
_servo_x.setEaseToD(x + _init_param.servo[AXIS_X].offset, millis_for_move);
196269
_servo_y.setEaseToD(y + _init_param.servo[AXIS_Y].offset, millis_for_move);
@@ -215,6 +288,12 @@ void StackchanSERVO::moveXY(servo_param_s servo_param_x, servo_param_s servo_par
215288
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330(servo_param_x.degree + _init_param.servo[AXIS_X].offset));
216289
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330(servo_param_y.degree + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
217290
_isMoving = false;
291+
} else if (_servo_type == ServoType::RT_DYN_XL330) {
292+
_isMoving = true;
293+
_dxl.setGoalPosition(AXIS_X + 1, convertDYNIXELXL330_RT(servo_param_x.degree + _init_param.servo[AXIS_X].offset));
294+
_dxl.setGoalPosition(AXIS_Y + 1, convertDYNIXELXL330_RT(servo_param_y.degree + _init_param.servo[AXIS_Y].offset)); // RT版に合わせて+180°しています。
295+
_isMoving = false;
296+
M5_LOGI("X:%f, Y:%f", getPosition(AXIS_X+1), getPosition(AXIS_Y+1));
218297
} else {
219298
if (servo_param_x.degree != 0) {
220299
_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
@@ -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;

‎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)