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