263 lines
6.2 KiB
C++
263 lines
6.2 KiB
C++
#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;
|
|
}
|