Skip to content

Commit

Permalink
Merge pull request #154 from ROBOTIS-GIT/develop
Browse files Browse the repository at this point in the history 
Minor update
  • Loading branch information
@OpusK
OpusK authored Dec 21, 2018
2 parents 06753da + deae1b6 commit 3c110cd
Show file tree
Hide file tree
Showing 7 changed files with 166 additions and 61 deletions.
4 changes: 2 additions & 2 deletions ...cr/libraries/DynamixelWorkbench/include/dynamixel_workbench_toolbox/dynamixel_workbench.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ class DynamixelWorkbench : public DynamixelDriver
DynamixelWorkbench();
~DynamixelWorkbench();

bool torque(uint8_t id, bool onoff, const char **log = NULL);
bool torque(uint8_t id, int32_t onoff, const char **log = NULL);
bool torqueOn(uint8_t id, const char **log = NULL);
bool torqueOff(uint8_t id, const char **log = NULL);

Expand All @@ -38,7 +38,7 @@ class DynamixelWorkbench : public DynamixelDriver
bool itemWrite(uint8_t id, const char *item_name, int32_t data, const char **log = NULL);
bool itemRead(uint8_t id, const char *item_name, int32_t *data, const char **log = NULL);

bool led(uint8_t id, bool onoff, const char **log = NULL);
bool led(uint8_t id, int32_t onoff, const char **log = NULL);
bool ledOn(uint8_t id, const char **log = NULL);
bool ledOff(uint8_t id, const char **log = NULL);

Expand Down
67 changes: 53 additions & 14 deletions .../opencr/libraries/DynamixelWorkbench/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -569,6 +569,12 @@ bool DynamixelDriver::writeRegister(uint8_t id, uint16_t address, uint16_t lengt
{
ErrorFromSDK sdk_error = {0, false, false, 0};

#if defined(__OPENCR__) || defined(__OPENCM904__)
delay(50);
#else
usleep(1000*50);
#endif

sdk_error.dxl_comm_result = packetHandler_->writeTxRx(portHandler_,
id,
address,
Expand Down Expand Up @@ -610,6 +616,12 @@ bool DynamixelDriver::writeRegister(uint8_t id, const char *item_name, int32_t d
uint16_t data_2_byte = (uint16_t)data;
uint32_t data_4_byte = (uint32_t)data;

#if defined(__OPENCR__) || defined(__OPENCM904__)
delay(10);
#else
usleep(1000*10);
#endif

switch (control_item->data_length)
{
case BYTE:
Expand Down Expand Up @@ -668,6 +680,12 @@ bool DynamixelDriver::writeOnlyRegister(uint8_t id, uint16_t address, uint16_t l
{
ErrorFromSDK sdk_error = {0, false, false, 0};

#if defined(__OPENCR__) || defined(__OPENCM904__)
delay(50);
#else
usleep(1000*50);
#endif

sdk_error.dxl_comm_result = packetHandler_->writeTxOnly(portHandler_,
id,
address,
Expand Down Expand Up @@ -699,38 +717,40 @@ bool DynamixelDriver::writeOnlyRegister(uint8_t id, const char *item_name, int32
control_item = tools_[factor].getControlItem(item_name, log);
if (control_item == NULL) return false;

uint8_t data_1_byte = (uint8_t)data;
uint16_t data_2_byte = (uint16_t)data;
uint32_t data_4_byte = (uint32_t)data;
#if defined(__OPENCR__) || defined(__OPENCM904__)
delay(50);
#else
usleep(1000*50);
#endif

switch (control_item->data_length)
{
case BYTE:
sdk_error.dxl_comm_result = packetHandler_->write1ByteTxOnly(portHandler_,
id,
control_item->address,
data_1_byte);
(uint8_t)data);
break;

case WORD:
sdk_error.dxl_comm_result = packetHandler_->write2ByteTxOnly(portHandler_,
id,
control_item->address,
data_2_byte);
(uint16_t)data);
break;

case DWORD:
sdk_error.dxl_comm_result = packetHandler_->write4ByteTxOnly(portHandler_,
id,
control_item->address,
data_4_byte);
(uint32_t)data);
break;

default:
sdk_error.dxl_comm_result = packetHandler_->write1ByteTxOnly(portHandler_,
id,
control_item->address,
data_1_byte);
(uint8_t)data);
break;
}

Expand Down Expand Up @@ -812,41 +832,41 @@ bool DynamixelDriver::readRegister(uint8_t id, const char *item_name, int32_t *d
control_item = tools_[factor].getControlItem(item_name, log);
if (control_item == NULL) return false;

uint8_t *data_1_byte = (uint8_t *)&data;
uint16_t *data_2_byte = (uint16_t*)&data;
uint32_t *data_4_byte = (uint32_t*)&data;
uint8_t data_1_byte = 0;
uint16_t data_2_byte = 0;
uint32_t data_4_byte = 0;

switch (control_item->data_length)
{
case BYTE:
sdk_error.dxl_comm_result = packetHandler_->read1ByteTxRx(portHandler_,
id,
control_item->address,
data_1_byte,
&data_1_byte,
&sdk_error.dxl_error);
break;

case WORD:
sdk_error.dxl_comm_result = packetHandler_->read2ByteTxRx(portHandler_,
id,
control_item->address,
data_2_byte,
&data_2_byte,
&sdk_error.dxl_error);
break;

case DWORD:
sdk_error.dxl_comm_result = packetHandler_->read4ByteTxRx(portHandler_,
id,
control_item->address,
data_4_byte,
&data_4_byte,
&sdk_error.dxl_error);
break;

default:
sdk_error.dxl_comm_result = packetHandler_->read1ByteTxRx(portHandler_,
id,
control_item->address,
data_1_byte,
&data_1_byte,
&sdk_error.dxl_error);
break;
}
Expand All @@ -863,6 +883,25 @@ bool DynamixelDriver::readRegister(uint8_t id, const char *item_name, int32_t *d
}
else
{
switch (control_item->data_length)
{
case BYTE:
*data = data_1_byte;
break;

case WORD:
*data = data_2_byte;
break;

case DWORD:
*data = data_4_byte;
break;

default:
*data = data_1_byte;
break;
}

if (log != NULL) *log = "[DynamixelDriver] Succeeded to read!";
return true;
}
Expand Down
14 changes: 7 additions & 7 deletions ...encr/libraries/DynamixelWorkbench/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ DynamixelWorkbench::DynamixelWorkbench(){}

DynamixelWorkbench::~DynamixelWorkbench(){}

bool DynamixelWorkbench::torque(uint8_t id, bool onoff, const char **log)
bool DynamixelWorkbench::torque(uint8_t id, int32_t onoff, const char **log)
{
bool result = false;

Expand All @@ -56,7 +56,7 @@ bool DynamixelWorkbench::torqueOn(uint8_t id, const char **log)
{
bool result = false;

result = torque(id, true, log);
result = torque(id, 1, log);

return result;
}
Expand All @@ -65,7 +65,7 @@ bool DynamixelWorkbench::torqueOff(uint8_t id, const char **log)
{
bool result = false;

result = torque(id, false, log);
result = torque(id, 0, log);

return result;
}
Expand Down Expand Up @@ -252,11 +252,11 @@ bool DynamixelWorkbench::itemRead(uint8_t id, const char *item_name, int32_t *da
return readRegister(id, item_name, data, log);
}

bool DynamixelWorkbench::led(uint8_t id, bool onoff, const char **log)
bool DynamixelWorkbench::led(uint8_t id, int32_t onoff, const char **log)
{
bool result = false;

result = writeRegister(id, "LED", (int32_t)onoff, log);
result = writeRegister(id, "LED", onoff, log);
if (result == false)
{
if (log != NULL) *log = "[DynamixelWorkbench] Failed to change led status!";
Expand All @@ -271,7 +271,7 @@ bool DynamixelWorkbench::ledOn(uint8_t id, const char **log)
{
bool result = false;

result = led(id, true, log);
result = led(id, 1, log);

return result;
}
Expand All @@ -280,7 +280,7 @@ bool DynamixelWorkbench::ledOff(uint8_t id, const char **log)
{
bool result = false;

result = led(id, false, log);
result = led(id, 0, log);

return result;
}
Expand Down
18 changes: 9 additions & 9 deletions ...3_with_open_manipulator/turtlebot3_with_open_manipulator_core/open_manipulator_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,7 @@ bool OpenManipulatorDriver::init(uint8_t *joint_id, uint8_t joint_cnt, uint8_t *
result = dxl_wb_.torqueOff(joint_id[num], &log);
if (result == true) DEBUG_SERIAL.println("Succeeded to set torque off");

result = dxl_wb_.setVelocityBasedProfile(joint_id[num], &log);
result = dxl_wb_.setTimeBasedProfile(joint_id[num], &log);
if (result == true) DEBUG_SERIAL.println("Succeeded to set velocity based profile mode");
}

Expand Down Expand Up @@ -183,12 +183,12 @@ bool OpenManipulatorDriver::init(uint8_t *joint_id, uint8_t joint_cnt, uint8_t *
}
}

double init_joint_position[4] = {0.0, -1.57, 1.37, 0.2258};
double init_joint_position[4] = {0.0, -1.57, 1.20, 0.6};
double init_gripper_position[1] = {0.0};

writeJointProfileControlParam(3.0f);
writeJointPosition(init_joint_position);
writeJointProfileControlParam(3.0f, 0.75f);
writeGripperProfileControlParam(0.0f);
writeJointPosition(init_joint_position);
writeGripperPosition(init_gripper_position);

writeJointProfileControlParam(0.0f);
Expand Down Expand Up @@ -457,7 +457,7 @@ bool OpenManipulatorDriver::getCurrent(double *get_data)
return true;
}

bool OpenManipulatorDriver::writeJointProfileControlParam(int32_t set_time)
bool OpenManipulatorDriver::writeJointProfileControlParam(double set_time, double acc)
{
const char *log;
bool result = false;
Expand All @@ -466,7 +466,7 @@ bool OpenManipulatorDriver::writeJointProfileControlParam(int32_t set_time)

for (uint8_t num = 0; num < joint_.cnt * 2; num = num + 2)
{
goal_data[num] = (set_time * 1000) / 4;
goal_data[num] = acc * 1000;
goal_data[num+1] = set_time * 1000;
}

Expand All @@ -476,7 +476,7 @@ bool OpenManipulatorDriver::writeJointProfileControlParam(int32_t set_time)
if (result == false)
{
DEBUG_SERIAL.println(log);
DEBUG_SERIAL.println("Failed to sync write position");
DEBUG_SERIAL.println("Failed to sync write param for profile");
}
}
else
Expand Down Expand Up @@ -509,7 +509,7 @@ bool OpenManipulatorDriver::writeJointPosition(double *set_data)
return true;
}

bool OpenManipulatorDriver::writeGripperProfileControlParam(int32_t set_time)
bool OpenManipulatorDriver::writeGripperProfileControlParam(double set_time)
{
const char *log;
bool result = false;
Expand All @@ -528,7 +528,7 @@ bool OpenManipulatorDriver::writeGripperProfileControlParam(int32_t set_time)
if (result == false)
{
DEBUG_SERIAL.println(log);
DEBUG_SERIAL.println("Failed to sync write position");
DEBUG_SERIAL.println("Failed to sync write param for profile");
}
}
else
Expand Down
4 changes: 2 additions & 2 deletions ...ot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/open_manipulator_driver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,9 +56,9 @@ class OpenManipulatorDriver
bool getVelocity(double *get_data);
bool getCurrent(double *get_data);
bool writeJointPosition(double *set_data);
bool writeJointProfileControlParam(int32_t set_time);
bool writeJointProfileControlParam(double set_time, double acc = 0.0f);
bool writeGripperPosition(double *set_data);
bool writeGripperProfileControlParam(int32_t set_time);
bool writeGripperProfileControlParam(double set_time);

private:
DynamixelWorkbench dxl_wb_;
Expand Down
Loading

0 comments on commit 3c110cd

Please sign in to comment.