edit sync function
@@ -585,12 +585,10 @@ void DynamixelDriver::addSyncWrite(const char *item_name)
syncWriteHandler_[sync_write_handler_cnt_].cti = cti;
- syncWriteHandler_[sync_write_handler_cnt_].groupSyncWrite = new dynamixel::GroupSyncWrite(portHandler_,
+ syncWriteHandler_[sync_write_handler_cnt_++].groupSyncWrite = new dynamixel::GroupSyncWrite(portHandler_,
packetHandler_,
cti->address,
cti->data_length);
-
- sync_write_handler_cnt_++;
}
bool DynamixelDriver::syncWrite(const char *item_name, int32_t *data)
@@ -607,8 +605,7 @@ bool DynamixelDriver::syncWrite(const char *item_name, int32_t *data)
{
if (!strncmp(syncWriteHandler_[index].cti->item_name, item_name, strlen(item_name)))
{
- swh.groupSyncWrite = syncWriteHandler_[index].groupSyncWrite;
- swh.cti = syncWriteHandler_[index].cti;
+ swh = syncWriteHandler_[index];
}
}
@@ -647,12 +644,10 @@ void DynamixelDriver::addSyncRead(const char *item_name)
syncReadHandler_[sync_read_handler_cnt_].cti = cti;
- syncReadHandler_[sync_read_handler_cnt_].groupSyncRead = new dynamixel::GroupSyncRead(portHandler_,
+ syncReadHandler_[sync_read_handler_cnt_++].groupSyncRead = new dynamixel::GroupSyncRead(portHandler_,
packetHandler_,
cti->address,
cti->data_length);
-
- sync_read_handler_cnt_++;
}
bool DynamixelDriver::syncRead(const char *item_name, int32_t *data)
@@ -669,8 +664,7 @@ bool DynamixelDriver::syncRead(const char *item_name, int32_t *data)
{
if (!strncmp(syncReadHandler_[index].cti->item_name, item_name, strlen(item_name)))
{
- srh.groupSyncRead = syncReadHandler_[index].groupSyncRead;
- srh.cti = syncReadHandler_[index].cti;
+ srh = syncReadHandler_[index];
}
}
@@ -876,7 +870,7 @@ int32_t DynamixelDriver::convertVelocity2Value(uint8_t id, float velocity)
float DynamixelDriver::convertValue2Velocity(uint8_t id, int32_t value)
{
- int32_t velocity = 0;
+ float velocity = 0;
int8_t factor = getToolsFactor(id);
velocity = value / tools_[factor].getVelocityToValueRatio();
