diff --git a/Makefile b/Makefile index 9921bd1..c7eabff 100644 --- a/Makefile +++ b/Makefile @@ -23,10 +23,10 @@ CXX = g++ TARGET = ias-ss -OBJECTS = ias_ss.o robot.o ias_robot.o +OBJECTS = ias_ss.o robot.o ias_robot.o gui.o DEPENDS = $(OBJECTS:.o=.d) CXXFLAGS = -ansi -pedantic -Wall `pkg-config --cflags playerc++` -LDLIBS = `pkg-config --libs playerc++` -lboost_system -lpthread +LDLIBS = `pkg-config --libs playerc++` -lboost_system -lpthread -lGLU -lGL -lfltk -lfltk_gl all: CXXFLAGS += -O2 -D_NDEBUG all: $(TARGET) diff --git a/ant_bot.inc b/ant_bot.inc index 4136244..50aa2e1 100644 --- a/ant_bot.inc +++ b/ant_bot.inc @@ -51,6 +51,5 @@ define ant_bot position drive "diff" localization "gps" - ant_sonars() ) diff --git a/gui.cpp b/gui.cpp new file mode 100644 index 0000000..a6429e2 --- /dev/null +++ b/gui.cpp @@ -0,0 +1,37 @@ +#include + +#include "gui.hpp" + +static void redraw_callback(void * arg) { + Fl_Gl_Window * window = static_cast(arg); + window->redraw(); + Fl::repeat_timeout(0.016, redraw_callback, window); +} + +GlGui::GlGui(Fl_Window * parent, int x, int y, int w, int h, const char * l) : Fl_Gl_Window(x, y, w, h, l) { + this->parent = parent; + title += l; + initialized = false; + Fl::add_timeout(0.016, redraw_callback, this); + resizable(this); +} + +void GlGui::draw() { + if(!valid()) { + if(!initialized) { + //opengl::initialize(); + + std::cout << "OpenGL Version: " << (char*)glGetString(GL_VERSION) << std::endl; + std::cout << "OpenGL Vendor: " << (char*)glGetString(GL_VENDOR) << std::endl; + + initialized = true; + } + //opengl::reshape(w(), h()); + } + + //opengl::display(); +} + +int GlGui::handle(int event) { + return Fl_Gl_Window::handle(event); +} diff --git a/gui.hpp b/gui.hpp new file mode 100644 index 0000000..949af90 --- /dev/null +++ b/gui.hpp @@ -0,0 +1,27 @@ +#pragma once + +#ifndef GUI_HPP +#define GUI_HPP + +#include +#include +#include +#include + +#include + +class GlGui : public Fl_Gl_Window { +public: + GlGui(Fl_Window * parent, int x, int y, int w, int h, const char * l); + +protected: + virtual void draw(); + virtual int handle(int); + +private: + Fl_Window * parent; + std::string title; + bool initialized; +}; + +#endif diff --git a/ias_robot.cpp b/ias_robot.cpp index d193072..7c5d6fd 100644 --- a/ias_robot.cpp +++ b/ias_robot.cpp @@ -28,8 +28,11 @@ #include "ias_robot.hpp" +static const float TURN_DEG_PER_SEC = 40.0f; +static const float METERS_PER_SEC = 0.4f; static const long HALF_SECOND_USEC = 500000; static const double MIN_DIST_M = 1.5; +static const double CRIT_DIST_M = 1.0; IASSS_Robot::IASSS_Robot(std::string hostname, uint32_t port) : Robot(hostname, port) { std::cout << "Creating IAS-SS robot on host \"" << hostname << "\" and port " << port << "." << std::endl; @@ -44,8 +47,6 @@ void IASSS_Robot::run() { long then, now, delta, wait; struct timeval tv; double dist = std::numeric_limits::infinity(); - double dist_l = 0.0; - double dist_r = 0.0; _p_client->Read(); rv = gettimeofday(&tv, NULL); @@ -57,21 +58,12 @@ void IASSS_Robot::run() { for(int i = 96; i < 126; i++) dist = _r_proxy->GetRange(i) < dist ? _r_proxy->GetRange(i) : dist; - if(dist < MIN_DIST_M) { - for(unsigned int i = 0; i < 96; i++) - dist_r += _r_proxy->GetRange(i); - dist_r /= 96; - - for(unsigned int i = 126; i < _r_proxy->GetRangeCount(); i++) - dist_l += _r_proxy->GetRange(i); - dist_l /= (_r_proxy->GetRangeCount() - 126); - - if(dist_r >= dist_l) - _p_proxy->SetSpeed(0.0f, PlayerCc::dtor(-20)); - else - _p_proxy->SetSpeed(0.0f, PlayerCc::dtor(20)); + if(dist < MIN_DIST_M && dist > CRIT_DIST_M) { + avoid_wall(METERS_PER_SEC, TURN_DEG_PER_SEC); + } else if(dist < CRIT_DIST_M) { + avoid_wall(0.0f, TURN_DEG_PER_SEC); } else - _p_proxy->SetSpeed(0.4f, 0.0f); + _p_proxy->SetSpeed(METERS_PER_SEC, 0.0f); /****************************************************************************** * WALL AVOIDANCE END * ******************************************************************************/ @@ -83,3 +75,21 @@ void IASSS_Robot::run() { wait = rv == 0 ? HALF_SECOND_USEC - delta : HALF_SECOND_USEC; usleep(wait); } + +void IASSS_Robot::avoid_wall(float front_speed, float turn_speed) { + double dist_l = 0.0; + double dist_r = 0.0; + + for(unsigned int i = 0; i < 96; i++) + dist_r += _r_proxy->GetRange(i); + dist_r /= 96; + + for(unsigned int i = 126; i < _r_proxy->GetRangeCount(); i++) + dist_l += _r_proxy->GetRange(i); + dist_l /= (_r_proxy->GetRangeCount() - 126); + + if(dist_r >= dist_l) + _p_proxy->SetSpeed(front_speed, PlayerCc::dtor(-turn_speed)); + else + _p_proxy->SetSpeed(front_speed, PlayerCc::dtor(turn_speed)); +} diff --git a/ias_robot.hpp b/ias_robot.hpp index c4f6fcd..a755846 100644 --- a/ias_robot.hpp +++ b/ias_robot.hpp @@ -45,6 +45,9 @@ public: virtual ~IASSS_Robot(); virtual void run(); + +private: + void avoid_wall(float front_speed, float turn_speed); }; #endif diff --git a/ias_ss.cpp b/ias_ss.cpp index cf48e60..a86fbd0 100644 --- a/ias_ss.cpp +++ b/ias_ss.cpp @@ -28,12 +28,17 @@ #include #include +#include "gui.hpp" #include "ias_robot.hpp" -const uint32_t PORT = PlayerCc::PLAYER_PORTNUM + 1; -const uint32_t NUM_ROBOTS = 4; +const char * TITLE = "Pheromone map"; +const int W = 256; +const int H = 256; +const uint32_t PORT = PlayerCc::PLAYER_PORTNUM + 1; +const uint32_t NUM_ROBOTS = 4; static bool done = false; +static Fl_Window * window; extern "C" void handler(int signal) { done = true; @@ -50,12 +55,20 @@ extern "C" void * robot_thread(void * arg) { return NULL; } +void create_gui(int argc, char **argv) { + window = new Fl_Window(20, 40, W, H, TITLE); + new GlGui(window, 0, 0, W, H, TITLE); + window->end(); + window->show(argc, argv); + window->make_current(); +} + int main(int argc, char **argv) { pthread_t robot_threads[NUM_ROBOTS]; std::vector robots; signal(SIGINT, handler); - + try { // Initialize the robot objects and threads. for(uint32_t i = 0; i < NUM_ROBOTS; ++i) { @@ -67,6 +80,9 @@ int main(int argc, char **argv) { } } + create_gui(argc, argv); + Fl::run(); + // Wait for all the robots to finish. for(uint32_t i = 0; i < NUM_ROBOTS; ++i) { if(pthread_join(robot_threads[i], NULL) != 0) { diff --git a/maps/cave_mask.png b/maps/cave_mask.png new file mode 100644 index 0000000..7d104b3 Binary files /dev/null and b/maps/cave_mask.png differ diff --git a/maps/gradient.png b/maps/gradient.png new file mode 100644 index 0000000..45f217c Binary files /dev/null and b/maps/gradient.png differ diff --git a/robot.cpp b/robot.cpp index 33f40db..e259578 100644 --- a/robot.cpp +++ b/robot.cpp @@ -43,3 +43,7 @@ Robot::~Robot() { delete _r_proxy; delete _p_client; } + +void Robot::log(std::string msg) { + std::cout << "ROBOT(" << _host_name << ":" << _port << ") - " << msg << std::endl; +} diff --git a/robot.hpp b/robot.hpp index d9867ec..7462510 100644 --- a/robot.hpp +++ b/robot.hpp @@ -46,6 +46,8 @@ protected: PlayerCc::PlayerClient * _p_client; PlayerCc::Position2dProxy * _p_proxy; PlayerCc::RangerProxy * _r_proxy; + + void log(std::string msg); }; #endif