int Dynamixel::getRxPacketParameter( int index ){
return dxl_get_rxpacket_parameter( index );
}
void DXLJointInterface::process()
{
switch(controlType())
{
case CT_POSITION:
{
int goalTicks = m_goalPosition / TICKS_TO_RAD;
dxl_write_word(id(), REG_GOAL_POSITION, goalTicks + m_tickOffset);
}
break;
case CT_VELOCITY:
{
int velTicks = m_goalSpeed / VEL_TO_RAD_PER_S;
if(velTicks < 0)
{
velTicks = (1 << 10) | (-velTicks);
}
dxl_write_word(id(), REG_MOVING_SPEED, velTicks);
}
break;
case CT_TORQUE:
{
int torqueTicks = m_goalTorque / TORQUE_TO_NM;
if(torqueTicks < 0)
{
torqueTicks = (1 << 10) | (-torqueTicks);
}
if(torqueTicks == 0)
torqueTicks = 1;
dxl_write_word(id(), REG_GOAL_TORQUE, torqueTicks);
}
break;
}
// int torqueTicks = 1023.0 * (m_torqueLimit / 4.4); // at 12V
// if(torqueTicks > 1023)
// torqueTicks = 1023;
// printf("torque: %4d\n", torqueTicks);
//
// dxl_write_word(id(), 34, torqueTicks);
dxl_set_txpacket_id(id());
dxl_set_txpacket_instruction(INST_READ);
dxl_set_txpacket_length(2 + 2);
dxl_set_txpacket_parameter(0, 36); // start address: present pos
dxl_set_txpacket_parameter(1, 6); // #bytes
dxl_txrx_packet();
if(dxl_get_rxpacket_length() != 6+2)
{
fprintf(stderr, "DXL: Got malformed read answer\n");
return;
}
int currentTicks = (dxl_get_rxpacket_parameter(1) << 8) | dxl_get_rxpacket_parameter(0);
m_currentPosition = TICKS_TO_RAD * (currentTicks - m_tickOffset);
double lastVel = m_currentVelocity;
int velTicks = (dxl_get_rxpacket_parameter(3) << 8) | dxl_get_rxpacket_parameter(2);
if(velTicks & (1 << 10))
velTicks = -(velTicks & 1023);
m_currentVelocity = VEL_TO_RAD_PER_S * velTicks;
int loadTicks = (dxl_get_rxpacket_parameter(5) << 8) | dxl_get_rxpacket_parameter(4);
if(loadTicks & (1 << 10))
loadTicks = -(loadTicks & 1023);
m_currentLoad = (1.0 / 1023.0) * loadTicks;
int current = dxl_read_word(id(), 68) - 2048;
m_currentCurrent = 0.0045 * current;
m_currentAcc = (m_currentVelocity - lastVel) / 0.02;
}
