int main()
{
int ret = 0;
int dev = 0;
int modId = 12;
int numOfModules = 0;
float pos = 0.04;
float vel = 1.0;
float acc = 1.0;
char pInitString[] = "RS232:1,9600";
unsigned long state = 0;
PCube_setDllDebug(1, 0, 0);
std::cout << "Attempting to open device: " << pInitString << std::endl;
ret = PCube_openDevice( &dev, pInitString );
if( ret != 0 )
{
std::cout << "Unable to open device: " << pInitString << ". Error " << ret << std::endl;
return -1;
}
//std::cout << "Scanning for Devices." << std::endl;
numOfModules = PCube_getModuleCount( dev );
std::cout << "Found " << numOfModules << " PowerCubes\n" << std::endl;
std::cout << "Homing module: " << modId << std::endl;
ret = PCube_homeModule( dev, modId );
ret = PCube_waitForHomeEnd(dev, modId, 1e6);
if( ret != 0 )
{
std::cout << "Unable to home module: " << modId << ". Error " << ret << std::endl;
//return -1;
}
do
{
ret = PCube_getModuleState( dev, modId, &state );
if( ret != 0 )
{
std::cout << "Unable to get device state: " << modId << ". Error " << ret << std::endl;
return -1;
}
} while( !(state & STATEID_MOD_HOME) );
ret = PCube_moveRamp( dev, modId, pos, vel, acc );
if( ret != 0 )
{
std::cout << "Unable to move module: " << modId << ". Error " << ret << std::endl;
return -1;
}
ret = PCube_closeDevice( dev );
if( ret != 0 )
{
std::cout << "Unable to close device: " << pInitString << ". Error " << ret << std::endl;
return -1;
}
return 0;
}
/*!
* \brief Does homing for all Modules
*/
bool PowerCubeCtrl::doHoming()
{
unsigned int DOF = m_params->GetDOF();
std::vector<int> ModuleIDs = m_params->GetModuleIDs();
/// start homing
int ret = 0;
for (unsigned int i = 0; i < DOF; i++)
{
pthread_mutex_lock(&m_mutex);
ret = PCube_homeModule(m_DeviceHandle, ModuleIDs[i]);
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
std::ostringstream errorMsg;
errorMsg << "Can't start homing for module " << ModuleIDs[i] << ", m5api error code: " << ret;
m_ErrorMessage = errorMsg.str();
return false;
}
}
/// wait until all modules are homed
double max_homing_time = 10.0; // seconds
double homing_time = 0.0;
double intervall = 0.1;
for (unsigned int i = 0; i < DOF; i++)
{
unsigned long int help;
do
{
pthread_mutex_lock(&m_mutex);
PCube_getModuleState(m_DeviceHandle, ModuleIDs[i], &help);
pthread_mutex_unlock(&m_mutex);
/// convert sec to usec
usleep(intervall * 1000000);
} while ((help & STATEID_MOD_HOME) == 0);
m_status[i] = help;
}
for (unsigned int i = 0; i < DOF; i++)
{
homing_time = 0.0;
/*while ((homing_time < max_homing_time))
{
updateStates();
if ((m_status[i] & STATEID_MOD_HOME))
{
std::cout << "Module " << ModuleIDs[i] << " homed in " << homing_time << "sec." << std::endl;
}
usleep(intervall * 1000000); // convert sec to usec
homing_time = homing_time + intervall;
}
*/
/// check result
if (!(m_status[i] & STATEID_MOD_HOME))
{
std::cout << "Homing failed: Error in Module " << ModuleIDs[i] << std::endl;
m_pc_status = PC_CTRL_NOT_HOMED;
return false;
}
}
// modules successfully homed
m_pc_status = PC_CTRL_OK;
return true;
}
