void Controller::runTWI() {
chrono::milliseconds sleepDuration(2000); //tmp
this_thread::sleep_for(sleepDuration);
TWI twi = TWI();
LngLat currGoal = LngLat(0., 0.);
Db db = Db::getInstance();
while(!endThreads) {
this->lngLatCurrent = db.getFakeTWI(); // todo: get current positon from ATmega
if(sizeof(this->map)>0 && !this->mapLoading) // sprawdź czy pozycja jest granicach mapy
this->positionInBounds = this->map->inBounds(this->lngLatCurrent);
if(this->goalPath.size() > 0) { // ścieżka wyznaczona
if(this->sentPoint == 0) { // nowa ścieżka
this->twiStatus = twi.writeFirstGoal(this->goalPath[0].toLngLat(this->map));
if(this->twiStatus < -1) { // twi ok
currGoal = this->goalPath[1].toLngLat(this->map);
this->twiStatus = twi.writeNextGoal(currGoal);
if(this->twiStatus > -1) { // twi ok
this->sentPoint=1;
} else { // twi error
// elog << "Problem z komunikacją TWI: " << this->twiStatus;
}
} else { // twi error
// elog << "Problem z komunikacją TWI: " << this->twiStatus;
}
} else { // kontynuuj wysyłanie trajektorii
LngLat tmp = twi.getCurrentGoal();
// dlog << "current goal from robot: " << tmp;
// dlog << "current set goal: " << currGoal;
if(tmp == currGoal) { // robot podąża do 'następnego' celu
// wyślij nowy 'następny' cel
this->sentPoint++;
if(this->sentPoint < this->goalPath.size()) { // pozostały punkty trajektorii do wysłania
currGoal = this->goalPath[this->sentPoint].toLngLat(this->map);
this->twiStatus = twi.writeNextGoal(currGoal);
if(this->twiStatus <= -1) { // twi error
// elog << "Problem z komunikacją TWI: " << this->twiStatus;
}
} else { // wysłano ostatni punkt
}
}
}
}
// opóźnij wątek
chrono::milliseconds sleepDuration(2000);
this_thread::sleep_for(sleepDuration);
}
dlog << "koniec threadTWI";
}
void slave_thread(int id)
{
printf(" +++ slave thread %d: starting\n", id);
while (true)
{
boost::unique_lock<boost::mutex> lock(data_ready_mutex);
while (!data_ready)
{
data_ready_cond.wait(lock);
}
data_ready = false;
unsigned work_unit;
{
boost::lock_guard<boost::mutex> lock(work_queue_mutex);
if (work_queue.size() == 0)
abort();
work_unit = work_queue.back();
work_queue.pop_back();
}
printf(" slave thread %d: got work unit %u\n", id, work_unit);
// Pretend to work
boost::chrono::milliseconds sleepDuration(3);
boost::this_thread::sleep_for(sleepDuration);
}
printf(" --- slave thread %d: finished\n", id);
}
void producer_thread()
{
printf("+++ producer thread\n");
for (unsigned work_unit_count = 0; work_unit_count < work_unit_limit; work_unit_count++)
{
// Pretend to work
boost::chrono::milliseconds sleepDuration(5);
boost::this_thread::sleep_for(sleepDuration);
// Generate some work
{
boost::lock_guard<boost::mutex> lock(work_queue_mutex);
printf(" producer: produced %u...\n", work_unit_count);
work_queue.push_back(work_unit_count);
}
// Let one of the slaves know there's work to be done
{
boost::unique_lock<boost::mutex> lock(data_ready_mutex);
data_ready = true;
}
data_ready_cond.notify_one();
}
printf("--- producer thread\n");
}
/**
* metoda wątku 'worker'
*/
void Controller::runWorker() {
while(!endThreads) {
if(!this->workerTask.empty()) {
switch(this->workerTask.front()) {
case 0:
break;
case TASK_FINDPATH: // wyznacz trajektorię
this->astar();
break;
case TASK_LOADMAP: { // wczytaj mapę
string mapName = this->workerTaskParams.front();
this->workerTaskParams.pop();
dlog << "wczytywanie mapy: " << mapName;
this->map = new Map(mapName);
this->mapLoading = false;
break;
} default:
elog << "nieznana komenda dla wątku threadWorker";
break;
}
this->workerTask.pop();
}
chrono::milliseconds sleepDuration(1000);
this_thread::sleep_for(sleepDuration);
}
dlog << "koniec threadWorker";
}
void Controller::runUart() {
int fd = serialOpen(UART_DEVICE, 9600);
int bSize = 0; // ilość znaków w buforze
while(!endThreads) {
bSize = serialDataAvail(fd);
if(bSize==8) { // dwa inty czekają na odczyt
union IntOrByte {
char b[4];
int i;
} u;
// odczyt kierunku wiatru
for (int i =0; i < 4; i++) { //odczytaj 4 bajty
u.b[i] = serialGetchar(fd);
}
this->windDirection = u.i;
// odczyt prędkości wiatru
u.i = 0; // resetujemy union
for (int i =0; i < 4; i++) { //odczytaj 4 bajty
u.b[i] = serialGetchar(fd);
}
this->windSpeed = u.i;
} else if(bSize>8) { // zbyt dużo informacji w buforze
serialFlush(fd);
}
chrono::milliseconds sleepDuration(500);
this_thread::sleep_for(sleepDuration);
}
serialClose(fd);
dlog << "koniec threadWorker";
}
void ProgressReporter::PrintBar() {
int barLength = TerminalWidth() - 28;
int totalPlusses = std::max(2, barLength - (int)title.size());
int plussesPrinted = 0;
// Initialize progress string
const int bufLen = title.size() + totalPlusses + 64;
std::unique_ptr<char[]> buf(new char[bufLen]);
snprintf(buf.get(), bufLen, "\r%s: [", title.c_str());
char *curSpace = buf.get() + strlen(buf.get());
char *s = curSpace;
for (int i = 0; i < totalPlusses; ++i) *s++ = ' ';
*s++ = ']';
*s++ = ' ';
*s++ = '\0';
fputs(buf.get(), stdout);
fflush(stdout);
std::chrono::milliseconds sleepDuration(250);
int iterCount = 0;
while (!exitThread) {
std::this_thread::sleep_for(sleepDuration);
// Periodically increase sleepDuration to reduce overhead of
// updates.
++iterCount;
if (iterCount == 10)
// Up to 0.5s after ~2.5s elapsed
sleepDuration *= 2;
else if (iterCount == 70)
// Up to 1s after an additional ~30s have elapsed.
sleepDuration *= 2;
Float percentDone = Float(workDone) / Float(totalWork);
int plussesNeeded = std::round(totalPlusses * percentDone);
while (plussesPrinted < plussesNeeded) {
*curSpace++ = '+';
++plussesPrinted;
}
fputs(buf.get(), stdout);
// Update elapsed time and estimated time to completion
Float seconds = ElapsedMS() / 1000.f;
Float estRemaining = seconds / percentDone - seconds;
if (percentDone == 1.f)
printf(" (%.1fs) ", seconds);
else if (!std::isinf(estRemaining))
printf(" (%.1fs|%.1fs) ", seconds,
std::max((Float)0., estRemaining));
else
printf(" (%.1fs|?s) ", seconds);
fflush(stdout);
}
}
void work(){
std::chrono::milliseconds timeout(100);
while(true){
if(mutex.try_lock_for(timeout)){
std::cout << std::this_thread::get_id() << ": do work with the mutex" << std::endl;
std::chrono::milliseconds sleepDuration(250);
std::this_thread::sleep_for(sleepDuration);
mutex.unlock();
std::this_thread::sleep_for(sleepDuration);
} else {
std::cout << std::this_thread::get_id() << ": do work without mutex" << std::endl;
std::chrono::milliseconds sleepDuration(100);
std::this_thread::sleep_for(sleepDuration);
}
}
}
int lua_sleep(lua_State *L) {
int ms = luaL_checknumber(L,-1);
lua_pop(L,1);
// FIXME: posix-thread for mingw
#ifdef __MINGW32__
Sleep(ms);
#else
std::chrono::milliseconds sleepDuration(ms);
std::this_thread::sleep_for(sleepDuration);
#endif
return 0;
}
void primary_thread()
{
printf("primary starting\n");
// Pretend to do some work
boost::chrono::milliseconds sleepDuration(500);
boost::this_thread::sleep_for(sleepDuration);
{
boost::unique_lock<boost::mutex> lock(cond_mutex);
finished = true;
}
printf("notify\n");
cond.notify_one();
printf("primary done\n");
}
int main()
{
std::chrono::milliseconds sleepDuration(20);
std::this_thread::sleep_for(sleepDuration);
}
void waitDelay () {
std::chrono::milliseconds sleepDuration(1000);
std::this_thread::sleep_for(sleepDuration);
}
void Controller::runMysql() {
Db db = Db::getInstance();
//wczytanie konfiguracji
this->debug = (db.getConfig("debug")=="1"?true:false);
this->maxSailDeviantion = stoi(db.getConfig("maxSailDeviation"));
this->rotationPenalty = stof(db.getConfig("rotationPenalty"));
//reset data
db.updateLngLat(LngLat(0., 0.));
db.setPathStatus(PATH_SEARCHING);
db.setMapStatus(MAP_NOTLOADED); //mapa niewczytana
// dodaj zadanie wczytania mapy do kolejki wątku 'worker'
this->workerTaskParams.push(db.getMapName());
this->workerTask.push(TASK_LOADMAP);
bool positionInBounds = this->positionInBounds; // czy pozycja znajduje się w obrębie mapy?
string mapName; // nazwa wczytanej mapy pobierana z db
LngLat newGoal; // cel pobierany z db
while(!endThreads) {
db.updateLngLat(this->lngLatCurrent);
// zmiana celu
newGoal = db.getLngLatGoal(); // pobierz cel z bazy danych
if(newGoal != this->lngLatGoal && abs(newGoal.lat)>0. && abs(newGoal.lng)>0.) { // zmieniono cel
this->lngLatGoal = newGoal;
dlog << "nowy cel: " << newGoal.toString();
db.setPathStatus(PATH_SEARCHING);
this->workerTask.push(TASK_FINDPATH);
}
// uaktualnij pole positionInBounds w bazie danych
if(positionInBounds != this->positionInBounds) { //zapisz zmianę do db
positionInBounds = this->positionInBounds;
db.updateDataParam("positionInBounds", to_string(positionInBounds));
}
// zmiana mapy
if(this->map && !this->mapLoading) {
mapName = db.getMapName();
if(mapName != this->map->getMapName()) {
// dodaj zadanie wczytania mapy do kolejki wątku 'worker'
this->workerTaskParams.push(mapName);
this->workerTask.push(TASK_LOADMAP);
this->mapLoading = true;
// dodaj zadanie wyznaczenia trajektori do kolejki wątku 'worker'
db.setPathStatus(PATH_SEARCHING);
this->workerTask.push(TASK_FINDPATH);
}
}
db.setTwiStatus(this->twiStatus); // uaktualnij status TWI
// todo: usunąć po podpięciu czujnika wiatru do uart
this->windDirection = stoi(db.getDataParam("windDirection")); // pobierz informację o kierunku wiatru z bazy danych
// opóźnienie pętli
std::chrono::milliseconds sleepDuration(5000);
std::this_thread::sleep_for(sleepDuration);
}
dlog << "koniec threadMysql";
}
