Abgabe
This commit is contained in:
262
kawaii/main.cpp
Normal file
262
kawaii/main.cpp
Normal file
@@ -0,0 +1,262 @@
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <csignal>
|
||||
#include <cstdint>
|
||||
#include <iostream>
|
||||
#include <random>
|
||||
#include <vector>
|
||||
|
||||
#include "engine.hpp"
|
||||
#include "measure.hpp"
|
||||
#include "rfid_reader.hpp"
|
||||
#include "ultrasound_sensor.hpp"
|
||||
#include "emergency_stop.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
typedef chrono::high_resolution_clock clocktype;
|
||||
|
||||
const int tickrate = 100;
|
||||
const chrono::milliseconds tick_delay = 1000ms / tickrate;
|
||||
const uint32_t exit_tag = 0xAE0B1E2B;
|
||||
const int no_tags = 3;
|
||||
const uint32_t distance_until_turn = 180;
|
||||
chrono::milliseconds backoff_time = 100ms;
|
||||
chrono::milliseconds turn_right_follow_time = 150ms;
|
||||
chrono::milliseconds turn_right_backoff_time = 500ms;
|
||||
|
||||
bool stop = false;
|
||||
|
||||
enum class mmode
|
||||
{
|
||||
STOP,
|
||||
LABYRINTH_FIND_WALL,
|
||||
LABYRINTH_BACKOFF,
|
||||
LABYRINTH_TURN_RIGHT,
|
||||
LABYRINTH_TURN_AWAY,
|
||||
LABYRINTH_FOLLOW,
|
||||
SEARCH_STRAIGHT,
|
||||
SEARCH_TURN_RIGHT,
|
||||
SEARCH_TURN_LEFT,
|
||||
SEARCH_TAG_STOP,
|
||||
HAPPY
|
||||
};
|
||||
|
||||
void signal_handler(int sig)
|
||||
{
|
||||
stop = true;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
signal(SIGINT, signal_handler);
|
||||
signal(SIGTERM, signal_handler);
|
||||
engine right(
|
||||
gpio(13, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
|
||||
gpio(20, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
|
||||
);
|
||||
engine left(
|
||||
gpio(19, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
|
||||
gpio(26, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
|
||||
);
|
||||
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 stop_button(25);
|
||||
ultrasound_sensor ultrasoundright(23, 24);
|
||||
ultrasound_sensor ultrasoundleft(12, 6);
|
||||
rfid_reader rfid;
|
||||
// TODO This random is unseeded
|
||||
minstd_rand random;
|
||||
|
||||
clocktype::time_point mmode_end = clocktype::now();
|
||||
mmode mode = mmode::STOP;
|
||||
mmode target_mode = mmode::LABYRINTH_FIND_WALL;
|
||||
clocktype::time_point mode_reset_time;
|
||||
|
||||
int last_tag = 0x00000000;
|
||||
vector<uint32_t> found_tags;
|
||||
found_tags.reserve(no_tags);
|
||||
|
||||
measure measurement("main");
|
||||
|
||||
clocktype::time_point next_tick = clocktype::now();
|
||||
while (!stop)
|
||||
{
|
||||
measurement.start();
|
||||
clocktype::time_point tick_start = clocktype::now();
|
||||
switch (mode)
|
||||
{
|
||||
case mmode::LABYRINTH_FOLLOW:
|
||||
case mmode::LABYRINTH_BACKOFF:
|
||||
case mmode::LABYRINTH_TURN_RIGHT:
|
||||
case mmode::LABYRINTH_TURN_AWAY:
|
||||
case mmode::LABYRINTH_FIND_WALL:
|
||||
{
|
||||
if (tick_start >= mode_reset_time)
|
||||
{
|
||||
switch (mode)
|
||||
{
|
||||
case mmode::LABYRINTH_BACKOFF:
|
||||
target_mode = mmode::LABYRINTH_TURN_AWAY;
|
||||
mode_reset_time = tick_start + turn_right_backoff_time;
|
||||
break;
|
||||
case mmode::LABYRINTH_TURN_AWAY:
|
||||
target_mode = mmode::LABYRINTH_FIND_WALL;
|
||||
break;
|
||||
case mmode::LABYRINTH_TURN_RIGHT:
|
||||
target_mode = mmode::LABYRINTH_FOLLOW;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (bwrightleft.get_value() || bwrightright.get_value())
|
||||
{
|
||||
target_mode = mmode::LABYRINTH_BACKOFF;
|
||||
mode_reset_time = tick_start + backoff_time;
|
||||
}
|
||||
else if (bwleftleft.get_value() || bwleftright.get_value())
|
||||
{
|
||||
if (mode != mmode::LABYRINTH_BACKOFF)
|
||||
{
|
||||
target_mode = mmode::LABYRINTH_TURN_RIGHT;
|
||||
mode_reset_time = tick_start + turn_right_follow_time;
|
||||
}
|
||||
}
|
||||
int last_id = rfid.last_id();
|
||||
if (last_id == exit_tag)
|
||||
{
|
||||
target_mode = mmode::SEARCH_STRAIGHT;
|
||||
last_tag = last_id;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case mmode::SEARCH_STRAIGHT:
|
||||
{
|
||||
auto usoundright = ultrasoundright.get_value();
|
||||
auto usoundleft = ultrasoundleft.get_value();
|
||||
if (min(usoundright, usoundleft) < distance_until_turn)
|
||||
{
|
||||
if (std::abs((long long int) usoundright - usoundleft) < 50)
|
||||
{
|
||||
int angle = random() % 180 + 90;
|
||||
if (angle <= 180)
|
||||
{
|
||||
target_mode = mmode::SEARCH_TURN_RIGHT;
|
||||
}
|
||||
else
|
||||
{
|
||||
target_mode = mmode::SEARCH_TURN_LEFT;
|
||||
angle = 360 - angle;
|
||||
}
|
||||
mode_reset_time = tick_start + 3ms * angle;
|
||||
}
|
||||
else
|
||||
{
|
||||
int angle = random() % 90 + 90;
|
||||
mode_reset_time = tick_start + 3ms * angle;
|
||||
if (usoundright < usoundleft)
|
||||
target_mode = mmode::SEARCH_TURN_LEFT;
|
||||
else
|
||||
target_mode = mmode::SEARCH_TURN_RIGHT;
|
||||
}
|
||||
}
|
||||
int last_id = rfid.last_id();
|
||||
if (last_id != last_tag && last_id != exit_tag)
|
||||
{
|
||||
last_tag = last_id;
|
||||
if (find(found_tags.begin(), found_tags.end(), last_tag) == found_tags.end())
|
||||
{
|
||||
found_tags.push_back(last_tag);
|
||||
target_mode = mmode::SEARCH_TAG_STOP;
|
||||
mode_reset_time = tick_start + 200ms;
|
||||
if (found_tags.size() == no_tags)
|
||||
{
|
||||
target_mode = mmode::HAPPY;
|
||||
mode_reset_time = tick_start + 2s;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
case mmode::SEARCH_TURN_LEFT:
|
||||
case mmode::SEARCH_TURN_RIGHT:
|
||||
case mmode::SEARCH_TAG_STOP:
|
||||
if (tick_start >= mode_reset_time)
|
||||
{
|
||||
target_mode = mmode::SEARCH_STRAIGHT;
|
||||
}
|
||||
break;
|
||||
case mmode::HAPPY:
|
||||
if (tick_start >= mode_reset_time)
|
||||
{
|
||||
target_mode = mmode::STOP;
|
||||
stop = true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
if (stop_button.get_state())
|
||||
{
|
||||
target_mode = mmode::STOP;
|
||||
stop = true;
|
||||
}
|
||||
|
||||
if (target_mode != mode)
|
||||
{
|
||||
mode = target_mode;
|
||||
switch (mode)
|
||||
{
|
||||
case mmode::STOP:
|
||||
left.set_speed(0);
|
||||
right.set_speed(0);
|
||||
break;
|
||||
case mmode::LABYRINTH_FIND_WALL:
|
||||
left.set_speed(60);
|
||||
right.set_speed(60);
|
||||
break;
|
||||
case mmode::LABYRINTH_BACKOFF:
|
||||
left.set_speed(-100);
|
||||
right.set_speed(-100);
|
||||
break;
|
||||
case mmode::LABYRINTH_TURN_AWAY:
|
||||
case mmode::LABYRINTH_TURN_RIGHT:
|
||||
left.set_speed(100);
|
||||
right.set_speed(-60);
|
||||
break;
|
||||
case mmode::LABYRINTH_FOLLOW:
|
||||
left.set_speed(0);
|
||||
right.set_speed(100);
|
||||
break;
|
||||
case mmode::SEARCH_STRAIGHT:
|
||||
left.set_speed(100);
|
||||
right.set_speed(100);
|
||||
break;
|
||||
case mmode::HAPPY:
|
||||
case mmode::SEARCH_TURN_LEFT:
|
||||
left.set_speed(-100);
|
||||
right.set_speed(100);
|
||||
break;
|
||||
case mmode::SEARCH_TURN_RIGHT:
|
||||
left.set_speed(100);
|
||||
right.set_speed(-100);
|
||||
break;
|
||||
case mmode::SEARCH_TAG_STOP:
|
||||
left.set_speed(-100);
|
||||
right.set_speed(-100);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
measurement.stop();
|
||||
|
||||
if (!stop)
|
||||
{
|
||||
next_tick += tick_delay;
|
||||
this_thread::sleep_until(next_tick);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user