Skip to content

Commit ca75cf2

Browse files
GyuH13GyuH13
committed
Make drive mode setup function to use parameter
Signed-off-by: Hyungyu Kim <kimhg@robotis.com>
1 parent c75d8e7 commit ca75cf2

File tree

4 files changed

+42
-70
lines changed

4 files changed

+42
-70
lines changed

‎c++/example/dynamixel_api/basic_test‎

-120 Bytes
Binary file not shown.

‎c++/example/dynamixel_api/basic_test.cpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ int kbhit(void)
6666

6767
int main()
6868
{
69-
int baudrate = 57600;
69+
int baudrate = 3000000;
7070
std::cout << "Dynamixel API Source Test Code" << std::endl;
7171
std::cout << "┌─────[Test Process]────┐" << std::endl;
7272
std::cout << "│ 1. Ping Test │" << std::endl;
@@ -149,7 +149,7 @@ int main()
149149
if (getch() == 0x1B) {
150150
return 0;
151151
}
152-
result_void = motor1->setPositionControlMode();
152+
result_void = motor1->setOperatingMode(dynamixel::Motor::OperatingMode::POSITION);
153153
if (!result_void.isSuccess()) {
154154
std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl;
155155
return 1;
@@ -237,7 +237,7 @@ int main()
237237
}
238238

239239
std::cout << "───────────[Velocity Control Test]─────────────" << std::endl;
240-
result_void = motor1->setVelocityControlMode();
240+
result_void = motor1->setOperatingMode(dynamixel::Motor::OperatingMode::VELOCITY);
241241
if (!result_void.isSuccess()) {
242242
std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl;
243243
return 1;
@@ -303,7 +303,7 @@ int main()
303303
}
304304

305305
std::cout << "───────────[Direction Test]─────────────" << std::endl;
306-
result_void = motor1->setReverseDirection();
306+
result_void = motor1->setDirection(dynamixel::Motor::Direction::REVERSE);
307307
if (!result_void.isSuccess()) {
308308
std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl;
309309
return 1;
@@ -317,7 +317,7 @@ int main()
317317
usleep(3000000);
318318
motor1->setGoalVelocity(0);
319319
motor1->disableTorque();
320-
result_void = motor1->setNormalDirection();
320+
result_void = motor1->setDirection(dynamixel::Motor::Direction::NORMAL);
321321
if (!result_void.isSuccess()) {
322322
std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl;
323323
return 1;

‎c++/include/dynamixel_sdk/dynamixel_api/motor.hpp‎

Lines changed: 19 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,22 @@ class Connector;
3333
class Motor
3434
{
3535
public:
36+
enum class OperatingMode {
37+
POSITION = 3,
38+
VELOCITY = 1,
39+
CURRENT = 0
40+
};
41+
42+
enum class ProfileConfiguration {
43+
VELOCITY_BASED = 0,
44+
TIME_BASED = 1
45+
};
46+
47+
enum class Direction {
48+
NORMAL = 0,
49+
REVERSE = 1
50+
};
51+
3652
Motor(uint8_t id, uint16_t model_number, Connector * connector);
3753

3854
virtual ~Motor();
@@ -54,13 +70,9 @@ class Motor
5470
Result<uint32_t, DxlError> getVelocityLimit();
5571

5672
Result<void, DxlError> changeID(uint8_t new_id);
57-
Result<void, DxlError> setPositionControlMode();
58-
Result<void, DxlError> setVelocityControlMode();
59-
Result<void, DxlError> setCurrentControlMode();
60-
Result<void, DxlError> setTimeBasedProfile();
61-
Result<void, DxlError> setVelocityBasedProfile();
62-
Result<void, DxlError> setNormalDirection();
63-
Result<void, DxlError> setReverseDirection();
73+
Result<void, DxlError> setOperatingMode(OperatingMode mode);
74+
Result<void, DxlError> setProfileConfiguration(ProfileConfiguration config);
75+
Result<void, DxlError> setDirection(Direction direction);
6476

6577
Result<void, DxlError> reboot();
6678
Result<void, DxlError> factoryResetAll();

‎c++/src/dynamixel_sdk/dynamixel_api/motor.cpp‎

Lines changed: 18 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -213,40 +213,22 @@ Result<void, DxlError> Motor::changeID(uint8_t new_id)
213213
return result;
214214
}
215215

216-
Result<void, DxlError> Motor::setPositionControlMode()
216+
Result<void, DxlError> Motor::setOperatingMode(OperatingMode mode)
217217
{
218218
Result<ControlTableItem, DxlError> item_result = getControlTableItem("Operating Mode");
219219
if (!item_result.isSuccess()) {
220220
return item_result.error();
221221
}
222222
const ControlTableItem & item = item_result.value();
223-
Result<void, DxlError> result = connector_->write1ByteData(id_, item.address, 3);
224-
return result;
225-
}
226-
227-
Result<void, DxlError> Motor::setVelocityControlMode()
228-
{
229-
Result<ControlTableItem, DxlError> item_result = getControlTableItem("Operating Mode");
230-
if (!item_result.isSuccess()) {
231-
return item_result.error();
232-
}
233-
const ControlTableItem & item = item_result.value();
234-
Result<void, DxlError> result = connector_->write1ByteData(id_, item.address, 1);
235-
return result;
236-
}
237-
238-
Result<void, DxlError> Motor::setCurrentControlMode()
239-
{
240-
Result<ControlTableItem, DxlError> item_result = getControlTableItem("Operating Mode");
241-
if (!item_result.isSuccess()) {
242-
return item_result.error();
223+
uint8_t mode_value = static_cast<uint8_t>(mode);
224+
Result<void, DxlError> result = connector_->write1ByteData(id_, item.address, mode_value);
225+
if (!result.isSuccess()) {
226+
return result.error();
243227
}
244-
const ControlTableItem & item = item_result.value();
245-
Result<void, DxlError> result = connector_->write1ByteData(id_, item.address, 0);
246228
return result;
247229
}
248230

249-
Result<void, DxlError> Motor::setTimeBasedProfile()
231+
Result<void, DxlError> Motor::setProfileConfiguration(ProfileConfiguration config)
250232
{
251233
Result<ControlTableItem, DxlError> item_result = getControlTableItem("Drive Mode");
252234
if (!item_result.isSuccess()) {
@@ -257,47 +239,20 @@ Result<void, DxlError> Motor::setTimeBasedProfile()
257239
if (!read_result.isSuccess()) {
258240
return read_result.error();
259241
}
260-
uint8_t drive_mode = read_result.value();
261-
drive_mode |= 0b00000100;
262-
Result<void, DxlError> result = connector_->write1ByteData(id_, item.address, drive_mode);
263-
return result;
264-
}
265242

266-
Result<void, DxlError> Motor::setVelocityBasedProfile()
267-
{
268-
Result<ControlTableItem, DxlError> item_result = getControlTableItem("Drive Mode");
269-
if (!item_result.isSuccess()) {
270-
return item_result.error();
271-
}
272-
const ControlTableItem & item = item_result.value();
273-
auto read_result = connector_->read1ByteData(id_, item.address);
274-
if (!read_result.isSuccess()) {
275-
return read_result.error();
276-
}
277243
uint8_t drive_mode = read_result.value();
278-
drive_mode &= 0b11111011;
279-
Result<void, DxlError> result = connector_->write1ByteData(id_, item.address, drive_mode);
280-
return result;
281-
}
244+
const uint8_t PROFILE_BIT_MASK = 0b00000100;
282245

283-
Result<void, DxlError> Motor::setNormalDirection()
284-
{
285-
Result<ControlTableItem, DxlError> item_result = getControlTableItem("Drive Mode");
286-
if (!item_result.isSuccess()) {
287-
return item_result.error();
288-
}
289-
const ControlTableItem & item = item_result.value();
290-
auto read_result = connector_->read1ByteData(id_, item.address);
291-
if (!read_result.isSuccess()) {
292-
return read_result.error();
246+
if (config == ProfileConfiguration::TIME_BASED) {
247+
drive_mode |= PROFILE_BIT_MASK;
248+
} else if(config == ProfileConfiguration::VELOCITY_BASED) {
249+
drive_mode &= ~PROFILE_BIT_MASK;
293250
}
294-
uint8_t drive_mode = read_result.value();
295-
drive_mode &= 0b11111110;
296251
Result<void, DxlError> result = connector_->write1ByteData(id_, item.address, drive_mode);
297252
return result;
298253
}
299254

300-
Result<void, DxlError> Motor::setReverseDirection()
255+
Result<void, DxlError> Motor::setDirection(Direction direction)
301256
{
302257
Result<ControlTableItem, DxlError> item_result = getControlTableItem("Drive Mode");
303258
if (!item_result.isSuccess()) {
@@ -309,7 +264,12 @@ Result<void, DxlError> Motor::setReverseDirection()
309264
return read_result.error();
310265
}
311266
uint8_t drive_mode = read_result.value();
312-
drive_mode |= 0b00000001;
267+
const uint8_t DIRECTION_BIT_MASK = 0b00000001;
268+
if (direction == Direction::NORMAL) {
269+
drive_mode &= ~DIRECTION_BIT_MASK;
270+
} else if (direction == Direction::REVERSE) {
271+
drive_mode |= DIRECTION_BIT_MASK;
272+
}
313273
Result<void, DxlError> result = connector_->write1ByteData(id_, item.address, drive_mode);
314274
return result;
315275
}

0 commit comments

Comments
 (0)