/*!
 * \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;
}
예제 #2
0
/// @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;
}