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; }