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 for (unsigned int i = 0; i < DOF; i++) { pthread_mutex_lock(&m_mutex); PCube_homeModule(m_DeviceHandle, ModuleIDs[i]); pthread_mutex_unlock(&m_mutex); } // 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++) { homing_time = 0.0; while ((homing_time < max_homing_time)) { 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; } } // homing all modules successfully m_pc_status = PC_CTRL_OK; return true; }
/*! * \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; }