static IStorage *Create(const char *pApplicationName, const char *pArgv0)
{
CStorage *p = new CStorage();
if(p->Init(pApplicationName, pArgv0))
{
delete p;
p = 0;
}
return p;
}
static IStorage *Create(const char *pApplicationName, int StorageType, int NumArgs, const char **ppArguments)
{
CStorage *p = new CStorage();
if(p && p->Init(pApplicationName, StorageType, NumArgs, ppArguments))
{
dbg_msg("storage", "initialisation failed");
delete p;
p = 0;
}
return p;
}
IStorage *CreateLocalStorage()
{
CStorage *pStorage = new CStorage();
if(pStorage)
{
if(!fs_getcwd(pStorage->m_aCurrentdir, sizeof(pStorage->m_aCurrentdir)))
{
delete pStorage;
return NULL;
}
pStorage->AddPath("$CURRENTDIR");
}
return pStorage;
}
void ExamplePointerToClasses(void) {
CStorage objStatic;
objStatic.Set(1);
cout << " + objStatic.Get() = " << objStatic.Get() << endl;
CStorage *pObject = new CStorage[2];
pObject->Set(2);
cout << " + pObject[0].Get() = " << pObject[0].Get() << endl;
pObject[1].Set(3);
cout << " + pObject[0].Get() = " << pObject[0].Get() << endl;
cout << " + pObject[1].Get() = " << pObject[1].Get() << endl;
}
void CApplication::MoveToNotebook(int nNoteId, LPCTSTR sFileName)
{
if (GetDataFileName() != sFileName)
{
CNote note = m_datafile->GetNote(nNoteId);
note.SetId(0);
note.SetModifiedDate(dateutils::GetCurrentDateTime());
CStorage target;
target.SetDataFile(sFileName);
target.SaveNote(note, NM_ALL);
target.Release();
m_state.SetLastDataFile(sFileName);
m_local_storage.Write(m_state);
DeleteNote(nNoteId, true);
}
}
CGenerator( CStorage& storage ) : storage( storage ) {
std::vector<std::string> regNames = {
"rax", "rbx", "rcx", "rdx", "rsi", "rdi"
};
for( auto& name : regNames) {
regHolders.emplace_back(new CTemp(storage.Get(name)));
registers.push_back(regHolders.back().get());
}
}
void* vmcThreadFunction(void* param)
#endif
{
#ifdef REAL_TIME
rt_task_set_periodic(NULL, TM_NOW, 40000000);
#endif
CVmc* pThread = (CVmc*)param; // typecast
ros::NodeHandle n;
ros::Publisher pub = n.advertise<volksbot::ticks>("VMC", 20);
volksbot::ticks t;
t.header.frame_id = "/base_link";
ros::Subscriber sub = n.subscribe("Vel", 100, Vcallback,
ros::TransportHints().reliable().udp().maxDatagramSize(100));
ros::Subscriber cmd_vel_sub_ = n.subscribe<geometry_msgs::Twist>("cmd_vel", 10, CVcallback,
ros::TransportHints().reliable().udp().maxDatagramSize(100));
// ros::TransportHints().reliable().tcp().tcpNoDelay(true));
ros::ServiceServer service = n.advertiseService("Controls", callback);
pThread->clearResponseList();
pThread->addStateToResponseList(CVmc::MOTOR_TICKS_ABSOLUTE);
ROS_INFO("VolksBot starting main loop!");
pThread->enterCriticalSection();
while(pThread->isConnected())
{
ros::Time current = ros::Time::now();
if (current - lastcommand < ros::Duration(50.5) ) {
#ifdef REAL_TIME
pThread->setMotors(leftvel, rightvel);
#else
pThread->_pwmOut2 = -1*leftvel*pThread->_maxRpm/100;
pThread->_pwmOut1 = rightvel*pThread->_maxRpm/100;
#endif
} else {
#ifdef REAL_TIME
pThread->setMotors(0, 0);
#else
pThread->_pwmOut2 = 0;
pThread->_pwmOut1 = 0;
#endif
}
// old setting motor values could crash because of wrong mutex use
// pThread->_apiObject->useVMC().MotorRPMs.Set(pThread->_pwmOut1,
// pThread->_pwmOut2, pThread->_pwmOut3);
CStorage store = pThread->_apiObject->useVMC();
store.MotorRPMs.Set(pThread->_pwmOut1,
pThread->_pwmOut2, pThread->_pwmOut3);
if(pThread->_digitalInputUpdate) {
pThread->_apiObject->useVMC().DigitalIN.Update();
}
// setup ticks message with timestamp, ticks and velocity commands
#ifdef REAL_TIME
uint64_t ts = store.getTimeStamp();
t.header.stamp.sec = ts / 1000000000;
t.header.stamp.nsec = ts % 1000000000;
#else
t.header.stamp = ros::Time::now();
#endif
t.left = -1* (int) ((long)store.Motor[1].AbsolutRotations.getValue());
t.right = (int) ((long)store.Motor[0].AbsolutRotations.getValue());
t.vx = vx;
t.vth = vth;
pub.publish(t);
#ifdef REAL_TIME
rt_task_wait_period(NULL);
#else
pThread->wait(pThread->_cycleTime);
#endif
}
pThread->leaveCriticalSection();
#ifndef REAL_TIME
return 0;
#endif
}
