/*!
* \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 Initializing
*
* Setting paramters initialized by PowerCubeCtrlParams.h
*/
bool PowerCubeCtrl::Init(PowerCubeCtrlParams * params)
{
int ret = 0;
int DOF = m_params->GetDOF();
std::string CanModule = m_params->GetCanModule();
std::string CanDevice = m_params->GetCanDevice();
std::vector<int> ModulIDs = m_params->GetModuleIDs();
int CanBaudrate = m_params->GetBaudrate();
std::vector<double> MaxVel = m_params->GetMaxVel();
std::vector<double> MaxAcc = m_params->GetMaxAcc();
std::vector<double> Offsets = m_params->GetOffsets();
std::vector<double> LowerLimits = m_params->GetLowerLimits();
std::vector<double> UpperLimits = m_params->GetUpperLimits();
/// Output of current settings in the terminal
std::cout << " D O F :" << DOF << std::endl;
m_status.resize(DOF);
m_dios.resize(DOF);
m_positions.resize(DOF);
std::cout << "=========================================================================== " << std::endl;
std::cout << "PowerCubeCtrl:Init: Trying to initialize with the following parameters: " << std::endl;
std::cout << "DOF: " << DOF << std::endl;
std::cout << "CanModule: " << CanModule << std::endl;
std::cout << "CanDevice: " << CanDevice << std::endl;
std::cout << "CanBaudrate: " << CanBaudrate << std::endl;
std::cout << "ModulIDs: ";
for (int i = 0; i < DOF; i++)
{
std::cout << ModulIDs[i] << " ";
}
std::cout << std::endl << "maxVel: ";
for (int i = 0; i < DOF; i++)
{
std::cout << MaxVel[i] << " ";
}
std::cout << std::endl << "maxAcc: ";
for (int i = 0; i < DOF; i++)
{
std::cout << MaxAcc[i] << " ";
}
std::cout << std::endl << "upperLimits: ";
for (int i = 0; i < DOF; i++)
{
std::cout << UpperLimits[i] << " ";
}
std::cout << std::endl << "lowerLimits: ";
for (int i = 0; i < DOF; i++)
{
std::cout << LowerLimits[i] << " ";
}
std::cout << std::endl << "offsets: ";
for (int i = 0; i < DOF; i++)
{
std::cout << Offsets[i] << " ";
}
std::cout << std::endl << "=========================================================================== " << std::endl;
std::ostringstream InitStr;
InitStr << CanModule << ":" << CanDevice << "," << CanBaudrate;
std::cout << "initstring = " << InitStr.str().c_str() << std::endl;
/// open device
pthread_mutex_lock(&m_mutex);
ret = PCube_openDevice(&m_DeviceHandle, InitStr.str().c_str());
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
std::ostringstream errorMsg;
errorMsg << "Could not open device " << CanDevice << ", m5api error code: " << ret;
m_ErrorMessage = errorMsg.str();
return false;
}
m_CANDeviceOpened = true;
/// reset all modules
pthread_mutex_lock(&m_mutex);
ret = PCube_resetAll(m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
std::ostringstream errorMsg;
errorMsg << "Could not reset all modules, m5api error code: " << ret;
m_ErrorMessage = errorMsg.str();
return false;
}
/// make sure m_IdModules is clear of Elements:
ModulIDs.clear();
/// check number of modules connected to the bus
pthread_mutex_lock(&m_mutex);
int number_of_modules = PCube_getModuleCount(m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
std::cout << "found " << number_of_modules << " modules." << std::endl;
/// Check if the modules are connected
for (int i = 0; i < DOF; i++)
{
unsigned long serNo;
pthread_mutex_lock(&m_mutex);
ret = PCube_getModuleSerialNo(m_DeviceHandle, ModulIDs[i], &serNo);
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
std::ostringstream errorMsg;
errorMsg << "Could not find Module with ID " << ModulIDs[i] << ", m5api error code: " << ret;
m_ErrorMessage = errorMsg.str();
return false;
}
/// otherwise success
std::cout << "Found module " << ModulIDs[i] << std::endl;
}
// modules should be initialized now
m_pc_status = PC_CTRL_OK;
// check if modules are in normal state
std::vector<std::string> errorMessages;
PC_CTRL_STATUS status;
getStatus(status, errorMessages);
if ((status != PC_CTRL_OK) && (status != PC_CTRL_NOT_HOMED))
{
m_ErrorMessage.assign("");
for (int i = 0; i < DOF; i++)
{
m_ErrorMessage.append(errorMessages[i]);
m_ErrorMessage.append("\n");
}
return false;
}
else if (status == PC_CTRL_NOT_HOMED)
{
std::cout << "PowerCubeCtrl:Init: Homing is executed ...\n";
bool successful = false;
successful = doHoming();
if (!successful)
{
std::cout << "PowerCubeCtrl:Init: homing not successful, aborting ...\n";
return false;
}
}
/// Set angle offsets to hardware
for (int i = 0; i < DOF; i++)
{
pthread_mutex_lock(&m_mutex);
//std::cout << "------------------------------> PCube_setHomeOffset()" << std::endl;
PCube_setHomeOffset(m_DeviceHandle, ModulIDs[i], Offsets[i]);
pthread_mutex_unlock(&m_mutex);
}
/// Set limits to hardware
for (int i = 0; i < DOF; i++)
{
pthread_mutex_lock(&m_mutex);
PCube_setMinPos(m_DeviceHandle, ModulIDs[i], LowerLimits[i]);
pthread_mutex_unlock(&m_mutex);
pthread_mutex_lock(&m_mutex);
PCube_setMaxPos(m_DeviceHandle, ModulIDs[i], UpperLimits[i]);
pthread_mutex_unlock(&m_mutex);
}
/// Set max velocity to hardware
setMaxVelocity(MaxVel);
/// Set max acceleration to hardware
setMaxAcceleration(MaxAcc);
/// set synchronous or asynchronous movements
setSyncMotion();
//setASyncMotion();
// All modules initialized successfully
m_pc_status = PC_CTRL_OK;
m_Initialized = true;
return true;
}
