void HermesInterface::init()
{
int ret=0;
int nModulos=-1;
const char* nameDev;
const char* nameInit;
ret=PCube_openDevice(&dev,pInitString.c_str());
nameDev=PCube_getDeviceName(dev);
std::cout << "dev: " << dev << " Nombre: " << nameDev <<std::endl;
nameInit=PCube_getDeviceRevision( dev );
std::cout << "InitString: " << nameInit <<std::endl;
if(ret==0){
nModulos=PCube_getModuleCount(dev);
std::cout << "Modules Conected: " << nModulos << std::endl;
// Read init position
readPositionLeftArm();
readPositionRightArm();
ret=PCube_homeAll(dev); // Home all Modules
ret=PCube_haltAll(dev); // Stop all modules
for(int i=1;i<=7;i++)
PCube_resetModule(dev,i); // Reset leftArm
for(int i=10;i<=17;i++)
PCube_resetModule(dev,i);// Reset rightArm
}
}
void HermesInterface::softStopAll()
{
PCube_haltAll(dev);
for(int i=1;i<=7;i++)
PCube_resetModule(dev,i); // Reset leftArm
for(int i=10;i<=17;i++)
PCube_resetModule(dev,i);// Reset rightArm
}
/*!
* \brief Recovers the manipulator after an emergency stop
*/
bool PowerCubeCtrl::Recover()
{
std::vector<std::string> errorMessages;
PC_CTRL_STATUS status;
pthread_mutex_lock(&m_mutex);
PCube_haltAll(m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
usleep(500000);
pthread_mutex_lock(&m_mutex);
PCube_resetAll(m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
usleep(500000);
updateStates();
getStatus(status, errorMessages);
if (status == PC_CTRL_NOT_HOMED)
{
if (!doHoming())
{
return false;
}
}
usleep(500000);
// modules should be recovered now
m_pc_status = PC_CTRL_OK;
updateStates();
// check if modules are really back to normal state
getStatus(status, errorMessages);
if ((status != PC_CTRL_OK))
{
m_ErrorMessage.assign("");
for (int i = 0; i < m_params->GetDOF(); i++)
{
m_ErrorMessage.append(errorMessages[i]);
}
return false;
}
/// modules successfully recovered
m_pc_status = PC_CTRL_OK;
return true;
}
/*!
* \brief Stops the manipulator immediately
*/
bool PowerCubeCtrl::Stop()
{
/// stop should be executes without checking any conditions
pthread_mutex_lock(&m_mutex);
PCube_haltAll(m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
/// after halt the modules don't accept move commands any more, they first have to be reseted
usleep(500000);
pthread_mutex_lock(&m_mutex);
PCube_resetAll(m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
return true;
}
