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;
}
void HermesInterface::moveRightArm(std::vector<float> &q_in)
{
for(int i=11;i<=17;i++)
PCube_moveRamp(dev,i,(float)q_in[i-1-10],0.2,0.2);
}
/*!
* \brief Move joints synchronous
*
* Adjusting velocity of all joints to reach the final angules at the same time
*/
bool PowerCubeCtrl::MoveJointSpaceSync(const std::vector<double>& target)
{
PCTRL_CHECK_INITIALIZED();
unsigned int DOF = m_params->GetDOF();
std::vector<std::string> errorMessages;
PC_CTRL_STATUS status;
getStatus(status, errorMessages);
if ((status != PC_CTRL_OK))
{
m_ErrorMessage.assign("");
for (unsigned int i = 0; i < DOF; i++)
{
m_ErrorMessage.append(errorMessages[i]);
m_ErrorMessage.append("\n");
}
return false;
}
std::vector<double> vel(DOF);
std::vector<double> acc(DOF);
double TG = 0;
try
{
/// calculate which joint takes the longest time to reach goal
std::vector<double> times(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
RampCommand rm(m_positions[i], m_velocities[i], target[i], m_params->GetMaxAcc()[i],
m_params->GetMaxVel()[i]);
times[i] = rm.getTotalTime();
}
/// determine the joint index that has the greatest value for time
int furthest = 0;
double max = times[0];
for (unsigned int i = 1; i < DOF; i++)
{
if (times[i] > max)
{
max = times[i];
furthest = i;
}
}
RampCommand rm_furthest(m_positions[furthest], m_velocities[furthest], target[furthest],
m_params->GetMaxAcc()[furthest], m_params->GetMaxVel()[furthest]);
double T1 = rm_furthest.T1();
double T2 = rm_furthest.T2();
double T3 = rm_furthest.T3();
/// total time:
TG = T1 + T2 + T3;
/// calculate velocity and acceleration for all joints:
acc[furthest] = m_params->GetMaxAcc()[furthest];
vel[furthest] = m_params->GetMaxVel()[furthest];
for (unsigned int i = 0; i < DOF; i++)
{
if (int(i) != furthest)
{
double a;
double v;
RampCommand::calculateAV(m_positions[i], m_velocities[i], target[i], TG, T3, m_params->GetMaxAcc()[i],
m_params->GetMaxVel()[i], a, v);
acc[i] = a;
vel[i] = v;
}
}
}
catch (...)
{
return false;
}
/// Send motion commands to hardware
for (unsigned int i = 0; i < DOF; i++)
{
pthread_mutex_lock(&m_mutex);
PCube_moveRamp(m_DeviceHandle, m_params->GetModuleIDs()[i], target[i], fabs(vel[i]), fabs(acc[i]));
pthread_mutex_unlock(&m_mutex);
}
pthread_mutex_lock(&m_mutex);
PCube_startMotionAll(m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
return true;
}
