bool simulatedArm::Init(PowerCubeCtrlParams * params) { m_DOF = params->GetNumberOfDOF();; double vmax = 1.0; double amax = 1.5; setMaxVelocity(vmax); setMaxAcceleration(amax); m_maxVel.resize(m_DOF); m_maxAcc.resize(m_DOF); std::vector<double> ul(m_DOF); std::vector<double> ll(m_DOF); ul = params->GetUpperLimits(); ll = params->GetLowerLimits(); m_maxVel = params->GetMaxVel(); m_maxAcc = params->GetMaxAcc(); for (int i=0; i < m_DOF; i++) { m_motors.push_back( simulatedMotor(ll[i], ul[i], amax, vmax) ); } std::cerr << "===========Initializing Simulated Powercubes\n"; m_Initialized = true; return true; }
Sprite& Sprite::operator=(const Sprite& rhs) { setName( rhs.getName() ); setPosition(rhs.getPosition()); setVelocity(rhs.getVelocity()); setMaxVelocity(rhs.getMaxVelocity()); frame = rhs.frame; return *this; }
SingleFrameSprite& SingleFrameSprite::operator=(const SingleFrameSprite& rhs) { setName( rhs.getName() ); setPosition(rhs.getPosition()); setVelocity(rhs.getVelocity()); setMaxVelocity(rhs.getMaxVelocity()); Location(rhs.Location()); setDirection(rhs.getDirection()); frame = rhs.frame; return *this; }
BigBullet& BigBullet::operator=(const BigBullet& rhs) { setName( rhs.getName() ); setPosition(rhs.getPosition()); setVelocity(rhs.getVelocity()); setMaxVelocity(rhs.getMaxVelocity()); currentFrame(rhs.currentFrame()); change(rhs.change()); setDirection(rhs.getDirection()); dt = rhs.dt; strength = rhs.strength; power = rhs.power; offset = rhs.offset; bulletDirection = rhs.bulletDirection; setFrameVector(rhs.getFrameVector()); rnormal = rhs.rnormal; lnormal = rhs.lnormal; return *this; }
Miner::Miner() { m_istamina = max_stamina; m_eLocation = home; setPosition({ 20.0f, 50.0f }); //setVelocity({ 0.5f, 0.5f }); // non dovrebbe avere una base di velocità, dovrebbe essere 0 setAcceleration({ 0.5f, 0.5f }); setMaxVelocity(2.0f); m_eAgentType = GNOME; m_pMinerStateMachine = new MinerStateMachine(this); State<Miner>* m_pStartState = (State<Miner>*)&Idle::getIInstance(); m_pMinerStateMachine->SetCurrentState(m_pStartState); m_pSteeringBehaviors = new SteeringBehaviors(this); }
TrapezoidProfile::TrapezoidProfile(double maxV, double timeToMaxV) { setMaxVelocity(maxV); setTimeToMaxV(timeToMaxV); }
/*! * \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; }
SCurveProfile::SCurveProfile(double maxV, double maxA, double timeToMaxA) { setMaxVelocity(maxV); setMaxAcceleration(maxA); setTimeToMaxA(timeToMaxA); }