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
}
}
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 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;
}