/*!
* \brief Returns the current states
*/
bool PowerCubeCtrl::updateStates()
{
PCTRL_CHECK_INITIALIZED();
unsigned int DOF = m_params->GetDOF();
unsigned long state;
unsigned char dio;
float position;
int ret = 0;
for (unsigned int i = 0; i < DOF; i++)
{
pthread_mutex_lock(&m_mutex);
ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
m_pc_status = PC_CTRL_ERR;
}
m_status[i] = state;
m_dios[i] = dio;
m_positions[i] = position;
/// ToDo: calculate vel and acc
///m_velocities = ???;
///m_accelerations = ???
}
return true;
}
/// @brief Returns the current states
bool PowerCubeCtrl::updateStates()
{
PCTRL_CHECK_INITIALIZED();
unsigned int DOF = m_params->GetDOF();
unsigned long state;
unsigned char dio;
float position;
for (unsigned int i = 0; i < DOF; i++)
{
pthread_mutex_lock(&m_mutex);
//std::cout << "------------------------------> PCube_getStateDioPos()" << std::endl;
PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
pthread_mutex_unlock(&m_mutex);
m_status[i] = state;
m_dios[i] = dio;
m_positions[i] = position;
// @todo calculate vel and acc
//m_velocities = ???;
//m_accelerations = ???
}
return true;
}
/*!
* \brief Returns the current states
*/
bool PowerCubeCtrl::updateStates()
{
PCTRL_CHECK_INITIALIZED();
unsigned int DOF = m_params->GetDOF();
unsigned long state;
unsigned long state_s; // for debug
unsigned char dio;
float position;
int ret = 0;
for (unsigned int i = 0; i < DOF; i++)
{
state_s = m_status[i];
pthread_mutex_lock(&m_mutex);
ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
//m_pc_status = PC_CTRL_ERR;
std::cout << "State: Error com" << std::endl;
}
else
{ if( state_s != state)
{
std::cout << "State: " << state << "Joint: " << i << std::endl;
}
m_status[i] = state;
m_dios[i] = dio;
m_positions[i] = position;
}
/// ToDo: calculate vel and acc
///m_velocities = ???;
///m_accelerations = ???
}
return true;
}