diff --git a/kawaii/main.cpp b/kawaii/main.cpp index 541e330..303ff95 100644 --- a/kawaii/main.cpp +++ b/kawaii/main.cpp @@ -28,18 +28,32 @@ chrono::milliseconds turn_right_backoff_time = 500ms; bool stop = false; +// Modes indicate what kawaii is doing at the moment +// Modes prefixed with LABYRINTH are for navigating the Labyrinth +// Modes prefixed with SEARCH are for performing rfid search in the open area enum class mmode { + // Motors stopped, no operation at all STOP, + // Drive straight until a wall is encountered LABYRINTH_FIND_WALL, + // Drive backwards for a short period of time to build up distance to the wall LABYRINTH_BACKOFF, + // Turn to the right - away from the wall followed - to produce a forward movement LABYRINTH_TURN_RIGHT, + // Turn to the right to better align with the encountered wall LABYRINTH_TURN_AWAY, + // Turn to the left until the currently followed wall is hit LABYRINTH_FOLLOW, + // Move forward util encountering a wall or a rfid tag SEARCH_STRAIGHT, + // Turn away from the wall to the right SEARCH_TURN_RIGHT, + // Turn away from the wall to the left SEARCH_TURN_LEFT, + // Perform a short stop to indicate detected rfid tag SEARCH_TAG_STOP, + // Perform a short dance to celebrate successful operation HAPPY }; @@ -52,6 +66,10 @@ int main() { signal(SIGINT, signal_handler); signal(SIGTERM, signal_handler); + + // Initialize everything needed during operation + + // Engines engine right( gpio(13, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON), gpio(20, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON) @@ -60,33 +78,46 @@ int main() gpio(19, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON), gpio(26, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON) ); + + // Black & White Sensors gpio bwleftleft(18, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON); gpio bwleftright(4, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON); gpio bwrightleft(3, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON); gpio bwrightright(2, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON); + + // Emergency stop button emergency_stop stop_button(25); + + // Ultrasound sensors ultrasound_sensor ultrasoundright(23, 24); ultrasound_sensor ultrasoundleft(12, 6); + + // RFID reader rfid_reader rfid; - // TODO This random is unseeded - minstd_rand random; + + minstd_rand random; // This random is unseeded. This however isn't a problem for this application clocktype::time_point mmode_end = clocktype::now(); mmode mode = mmode::STOP; mmode target_mode = mmode::LABYRINTH_FIND_WALL; - clocktype::time_point mode_reset_time; + clocktype::time_point mode_reset_time; // Inidicates how long the current mode should be active (does not apply to all modes) int last_tag = 0x00000000; vector found_tags; - found_tags.reserve(no_tags); + found_tags.reserve(no_tags); // Since memory allocations during operations could cause problems we preallocate enough memory to store all tags in measure measurement("main"); clocktype::time_point next_tick = clocktype::now(); + + // Main loop repsonsible for the acutal operation while (!stop) { measurement.start(); clocktype::time_point tick_start = clocktype::now(); + + // First we check all relevant sensors and decide whether we need to change modes + // If so we store the new mode in target_mode for later mode switching switch (mode) { case mmode::LABYRINTH_FOLLOW: @@ -97,6 +128,7 @@ int main() { if (tick_start >= mode_reset_time) { + // Some modes will run only a specific time before automaticly switching to the next mode switch (mode) { case mmode::LABYRINTH_BACKOFF: @@ -113,11 +145,15 @@ int main() } if (bwrightleft.get_value() || bwrightright.get_value()) { + // We want to follow the left-hand wall so we need to backoff a little if hitting a wall with the right sensor to turn target_mode = mmode::LABYRINTH_BACKOFF; mode_reset_time = tick_start + backoff_time; } else if (bwleftleft.get_value() || bwleftright.get_value()) { + // If the left sensor hits the wall, follow it + // Don't switch to follow mode if the current mode is LABYRINTH_BACKOFF as this can cause bugs when left and right sensors hit + // the wall at the same time which can cause kawaii to drive over the wall if (mode != mmode::LABYRINTH_BACKOFF) { target_mode = mmode::LABYRINTH_TURN_RIGHT; @@ -127,6 +163,7 @@ int main() int last_id = rfid.last_id(); if (last_id == exit_tag) { + // If we detected the labyrinth exit tag switch to seah mode target_mode = mmode::SEARCH_STRAIGHT; last_tag = last_id; } @@ -138,8 +175,10 @@ int main() auto usoundleft = ultrasoundleft.get_value(); if (min(usoundright, usoundleft) < distance_until_turn) { + // If the left and right sensor measure a difference of under 5 cm the wall is roughly in front of kawaii if (std::abs((long long int) usoundright - usoundleft) < 50) { + // If the wall is in front kawaii will turn anything between 90° and 270° to the right int angle = random() % 180 + 90; if (angle <= 180) { @@ -147,28 +186,34 @@ int main() } else { + // If the angle is >180° turn to the left inststead to reach the desired angle target_mode = mmode::SEARCH_TURN_LEFT; angle = 360 - angle; } - mode_reset_time = tick_start + 3ms * angle; + mode_reset_time = tick_start + 3ms * angle; // 3ms is the time kawaii needs to turn roughly 1° } + // If not the wall is to kawaii's left or right - indicated by which sensor measures the lower value else { + // If the wall is to the side turn anything between 90° and 180° away from the wall int angle = random() % 90 + 90; - mode_reset_time = tick_start + 3ms * angle; + mode_reset_time = tick_start + 3ms * angle; // 3ms is the time kawaii needs to turn roughly 1° if (usoundright < usoundleft) target_mode = mmode::SEARCH_TURN_LEFT; else target_mode = mmode::SEARCH_TURN_RIGHT; } } + int last_id = rfid.last_id(); + // Check wether the last detected rfid tag has changed and is not the exit tag if (last_id != last_tag && last_id != exit_tag) { last_tag = last_id; + // Check if the tag is new and if so add it to the list of tags if (find(found_tags.begin(), found_tags.end(), last_tag) == found_tags.end()) { - found_tags.push_back(last_tag); + found_tags.push_back(last_tag); // This won't perform any memory allocations since there was already enough memory for all tags allocated during initialization target_mode = mmode::SEARCH_TAG_STOP; mode_reset_time = tick_start + 200ms; if (found_tags.size() == no_tags) @@ -203,6 +248,7 @@ int main() stop = true; } + // If the target_mode was modified adjust the motor speeds to the requirements of the new mode if (target_mode != mode) { mode = target_mode; diff --git a/realzeitnachweis.pdf b/realzeitnachweis.pdf index df5017b..d77ec0f 100644 Binary files a/realzeitnachweis.pdf and b/realzeitnachweis.pdf differ