htwg/resy
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added documentation, updated realzeitnachweis

Browse Source
This commit is contained in:
Simon Wörner
2017-07-10 16:44:39 +02:00
parent fe8806a4d5
commit 5c5e1df435
2 changed files with 53 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
kawaii/main.cpp
View File

@@ -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<uint32_t> 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;