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