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