From 9fdd6704947174fae292dfc6e14cc1029a7f7a6c Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 7 Feb 2004 16:55:41 +0000 Subject: Initial revision --- 2004/i/nono/src/GNUmakefile | 44 ++++ 2004/i/nono/src/README | 1 + 2004/i/nono/src/camera/Makefile.defs | 11 + 2004/i/nono/src/camera/camera.cc | 173 ++++++++++++++ 2004/i/nono/src/camera/camera.h | 28 +++ 2004/i/nono/src/camera/dumpimage.cc | 36 +++ 2004/i/nono/src/camera/test_camera.cc | 64 ++++++ 2004/i/nono/src/config/Makefile.defs | 4 + 2004/i/nono/src/config/config.cc | 162 +++++++++++++ 2004/i/nono/src/config/config.h | 64 ++++++ 2004/i/nono/src/config/config_lex.h | 60 +++++ 2004/i/nono/src/config/config_lex.ll | 127 +++++++++++ 2004/i/nono/src/date/Makefile.defs | 8 + 2004/i/nono/src/date/date.cc | 74 ++++++ 2004/i/nono/src/date/date.h | 54 +++++ 2004/i/nono/src/date/test_date.cc | 42 ++++ 2004/i/nono/src/erreur/Makefile.defs | 4 + 2004/i/nono/src/erreur/erreur.cc | 31 +++ 2004/i/nono/src/erreur/erreur.h | 39 ++++ 2004/i/nono/src/kernel_mips/pbus.c | 393 ++++++++++++++++++++++++++++++++ 2004/i/nono/src/kernel_mips/pbus.h | 35 +++ 2004/i/nono/src/motor/Makefile.defs | 19 ++ 2004/i/nono/src/motor/asserv.cc | 385 +++++++++++++++++++++++++++++++ 2004/i/nono/src/motor/asserv.h | 106 +++++++++ 2004/i/nono/src/motor/motor.cc | 216 ++++++++++++++++++ 2004/i/nono/src/motor/motor.h | 80 +++++++ 2004/i/nono/src/motor/movement.h | 51 +++++ 2004/i/nono/src/motor/movement_basic.cc | 61 +++++ 2004/i/nono/src/motor/movement_basic.h | 47 ++++ 2004/i/nono/src/motor/movement_goto.cc | 68 ++++++ 2004/i/nono/src/motor/movement_goto.h | 55 +++++ 2004/i/nono/src/motor/movement_types.h | 30 +++ 2004/i/nono/src/motor/test_asserv.cc | 113 +++++++++ 2004/i/nono/src/motor/test_motor.cc | 113 +++++++++ 2004/i/nono/src/motor/test_tracker.cc | 82 +++++++ 2004/i/nono/src/motor/tracker.cc | 230 +++++++++++++++++++ 2004/i/nono/src/motor/tracker.h | 66 ++++++ 2004/i/nono/src/serial/Makefile.defs | 8 + 2004/i/nono/src/serial/serial.cc | 97 ++++++++ 2004/i/nono/src/serial/serial.h | 50 ++++ 2004/i/nono/src/serial/test_serial.cc | 48 ++++ 2004/i/nono/src/vision/Makefile.defs | 11 + 2004/i/nono/src/vision/image.cc | 344 ++++++++++++++++++++++++++++ 2004/i/nono/src/vision/image.h | 48 ++++ 2004/i/nono/src/vision/rgbyuv.h | 57 +++++ 2004/i/nono/src/vision/test_image.cc | 34 +++ 2004/i/nono/src/vision/test_vision.cc | 48 ++++ 2004/i/nono/src/vision/thresholds.cc | 55 +++++ 2004/i/nono/src/vision/thresholds.h | 42 ++++ 49 files changed, 4018 insertions(+) create mode 100644 2004/i/nono/src/GNUmakefile create mode 100644 2004/i/nono/src/README create mode 100644 2004/i/nono/src/camera/Makefile.defs create mode 100644 2004/i/nono/src/camera/camera.cc create mode 100644 2004/i/nono/src/camera/camera.h create mode 100644 2004/i/nono/src/camera/dumpimage.cc create mode 100644 2004/i/nono/src/camera/test_camera.cc create mode 100644 2004/i/nono/src/config/Makefile.defs create mode 100644 2004/i/nono/src/config/config.cc create mode 100644 2004/i/nono/src/config/config.h create mode 100644 2004/i/nono/src/config/config_lex.h create mode 100644 2004/i/nono/src/config/config_lex.ll create mode 100644 2004/i/nono/src/date/Makefile.defs create mode 100644 2004/i/nono/src/date/date.cc create mode 100644 2004/i/nono/src/date/date.h create mode 100644 2004/i/nono/src/date/test_date.cc create mode 100644 2004/i/nono/src/erreur/Makefile.defs create mode 100644 2004/i/nono/src/erreur/erreur.cc create mode 100644 2004/i/nono/src/erreur/erreur.h create mode 100644 2004/i/nono/src/kernel_mips/pbus.c create mode 100644 2004/i/nono/src/kernel_mips/pbus.h create mode 100644 2004/i/nono/src/motor/Makefile.defs create mode 100644 2004/i/nono/src/motor/asserv.cc create mode 100644 2004/i/nono/src/motor/asserv.h create mode 100644 2004/i/nono/src/motor/motor.cc create mode 100644 2004/i/nono/src/motor/motor.h create mode 100644 2004/i/nono/src/motor/movement.h create mode 100644 2004/i/nono/src/motor/movement_basic.cc create mode 100644 2004/i/nono/src/motor/movement_basic.h create mode 100644 2004/i/nono/src/motor/movement_goto.cc create mode 100644 2004/i/nono/src/motor/movement_goto.h create mode 100644 2004/i/nono/src/motor/movement_types.h create mode 100644 2004/i/nono/src/motor/test_asserv.cc create mode 100644 2004/i/nono/src/motor/test_motor.cc create mode 100644 2004/i/nono/src/motor/test_tracker.cc create mode 100644 2004/i/nono/src/motor/tracker.cc create mode 100644 2004/i/nono/src/motor/tracker.h create mode 100644 2004/i/nono/src/serial/Makefile.defs create mode 100644 2004/i/nono/src/serial/serial.cc create mode 100644 2004/i/nono/src/serial/serial.h create mode 100644 2004/i/nono/src/serial/test_serial.cc create mode 100644 2004/i/nono/src/vision/Makefile.defs create mode 100644 2004/i/nono/src/vision/image.cc create mode 100644 2004/i/nono/src/vision/image.h create mode 100644 2004/i/nono/src/vision/rgbyuv.h create mode 100644 2004/i/nono/src/vision/test_image.cc create mode 100644 2004/i/nono/src/vision/test_vision.cc create mode 100644 2004/i/nono/src/vision/thresholds.cc create mode 100644 2004/i/nono/src/vision/thresholds.h (limited to '2004/i/nono/src') diff --git a/2004/i/nono/src/GNUmakefile b/2004/i/nono/src/GNUmakefile new file mode 100644 index 0000000..624278b --- /dev/null +++ b/2004/i/nono/src/GNUmakefile @@ -0,0 +1,44 @@ +SUBDIRS = camera config date erreur motor serial vision + + +TARGETS = +LIBS = -lm +CXXFLAGS = -Wall -g +CPPFLAGS = -I. -I$(SRCDIR) -I/usr/pkg/include $(DEFINES) +#LDFLAGS = --static -L/usr/pkg/lib +LDFLAGS = -L/usr/pkg/lib + +SRCDIR ?= . +LIBPPM ?= -lppm +LINK.o = $(CXX) $(LDFLAGS) $(TARGET_ARCH) + +VPATH = $(SUBDIRS:%=$(SRCDIR)/%) + +all: alltargets + +include $(SUBDIRS:%=$(SRCDIR)/%/Makefile.defs) + +alltargets: $(TARGETS) + +%.cc %.hh: %.yy + bison -o$( $@; \ + [ -s $@ ] || rm -f $@ + +-include $(foreach target, $(TARGETS) $(subst .,_,$(LIBS)), $(patsubst %.cc, .dep/%.d, $(filter %.cc, $($(target)_SOURCES)))) + +.dep: + mkdir .dep + +clean: + rm -f $(TARGETS) $(extra_clean) *.o *.a + rm -rf .dep + +.PHONY: all clean test alltargets diff --git a/2004/i/nono/src/README b/2004/i/nono/src/README new file mode 100644 index 0000000..275eac2 --- /dev/null +++ b/2004/i/nono/src/README @@ -0,0 +1 @@ +nono - programme du robot 2004. diff --git a/2004/i/nono/src/camera/Makefile.defs b/2004/i/nono/src/camera/Makefile.defs new file mode 100644 index 0000000..6c85f1b --- /dev/null +++ b/2004/i/nono/src/camera/Makefile.defs @@ -0,0 +1,11 @@ +TARGETS += test_camera dumpimage +LIBS += camera.a +test_camera_SOURCES = camera.cc test_camera.cc erreur.a config.a date.a +dumpimage_SOURCES = camera.cc dumpimage.cc erreur.a config.a date.a +camera_a_SOURCES = camera.cc + +dumpimage: $(dumpimage_SOURCES:%.cc=%.o) + +test_camera: $(test_camera_SOURCES:%.cc=%.o) + +camera.a: ${camera_a_SOURCES:%.cc=camera.a(%.o)} diff --git a/2004/i/nono/src/camera/camera.cc b/2004/i/nono/src/camera/camera.cc new file mode 100644 index 0000000..52a038e --- /dev/null +++ b/2004/i/nono/src/camera/camera.cc @@ -0,0 +1,173 @@ +// camera.cc +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet +// +#include "camera.h" +#include "erreur/erreur.h" +#include "kernel_mips/pbus.h" +#include "config/config.h" +#include "date/date.h" + +#include +#include +#include // Debug. +#include // +#include + +using namespace std; + +#define CAM_FILE "/dev/robotcam" + +#define CAMERA_DEBUG + +// Constructeur. +Camera::Camera () +{ + // Taille par défaut. + m_w = 352; m_h = 288; m_d = 1; + m_cut = 0; + // Ouvre le périphérique. + m_fd = open (CAM_FILE, O_RDONLY); + if (m_fd == -1) + throw ErreurFatale ("Impossible d'ouvrir le périphérique de la" + " camera.\n"); + // Paramètre la camera. + sccb_io param; + Config rc ("rc/camera"); + int addr, data; + while (!rc.eof ()) + { + if (rc.isId ("width")) + { + rc.getId (); + m_w = rc.getNum (); +#ifdef CAMERA_DEBUG + cout << "camera width " << m_w << endl; +#endif // CAMERA_DEBUG + } + else if (rc.isId ("height")) + { + rc.getId (); + m_h = rc.getNum (); +#ifdef CAMERA_DEBUG + cout << "camera height " << m_h << endl; +#endif // CAMERA_DEBUG + } + else if (rc.isId ("depth")) + { + rc.getId (); + m_d = rc.getNum (); +#ifdef CAMERA_DEBUG + cout << "camera depth " << m_d << endl; +#endif // CAMERA_DEBUG + } + else if (rc.isId ("cut")) + { + rc.getId (); + m_cut = rc.getNum (); +#ifdef CAMERA_DEBUG + cout << "camera cut " << m_cut << endl; +#endif // CAMERA_DEBUG + } + else if (rc.isId ("setup")) + { + rc.getId (); + addr = rc.getNum (); + data = rc.getNum (); + param.addr = addr; param.data = data; +#ifdef CAMERA_DEBUG + cout << "camera sccbwrite 0x" << hex << addr << " 0x" << data << + endl; +#endif // CAMERA_DEBUG + ioctl (m_fd, CAM_SCCBWRITE, ¶m); + } + else + rc.noId (); + } + // Affiche les paramètres. +#ifdef CAMERA_DEBUG + cout << hex << "camera sccbdump"; + for (addr = 0; addr < 0x50; ++addr) + { + if (!(addr % 16)) cout << endl; + param.addr = addr; + ioctl (m_fd, CAM_SCCBREAD, ¶m); + data = param.data; + cout << setw (2) << setfill ('0') << data << ' '; + + } + cout << endl << dec; +#endif // CAMERA_DEBUG + m_lastRead = 0; + // Paramètre la taille de frame. + int hcount = m_w * m_d; + ioctl (m_fd, CAM_SETHCOUNT, &hcount); + m_frameSize = m_w * m_h * m_d + m_cut; + ioctl (m_fd, CAM_SETFRAMESIZE, &m_frameSize); +} + +// Destructeur. +Camera::~Camera () +{ + // Ferme le périphérique de camera. + close (m_fd); +} + +// Lit une image. +int +Camera::read (unsigned char *image) +{ + int r; + int t = Date::getInstance ().start (); + // Attention, pas trops vite. + if (m_lastRead + 100 > t) + return 0; + m_lastRead = t; + // Lit les données sur la camera. + if (m_cut) + r = ::read (m_fd, image, m_cut); + if (!m_cut || r) + r = ::read (m_fd, image, m_frameSize - m_cut); +#ifdef CAMERA_DEBUG + cout << "camera read " << r << endl; +#endif // CAMERA_DEBUG + return r; +} + +// Lit une image en RGB. +int +Camera::readRGB (unsigned char *rgb) +{ + int ret; + int t = Date::getInstance ().start (); + // Attention, pas trops vite. + if (m_lastRead + 100 > t) + return 0; + m_lastRead = t; + // Alloue de la mémoire. + unsigned char *image = new unsigned char[m_frameSize]; + // Lit les données sur la camera. + ret = ::read (m_fd, image, m_frameSize); +#ifdef CAMERA_DEBUG + cout << "camera read " << ret << endl; +#endif // CAMERA_DEBUG + if (!ret) return 0; + // Positions de débuts. + unsigned char *r, *g, *b; + r = image + m_cut + 2; + g = image + m_cut + 1; + b = image + m_cut; + // Décode les couleurs. + for (int i = 0; i < m_w * m_h; ++i) + { + *rgb++ = *r; + r += 4; + *rgb++ = *g; + g += 4; + *rgb++ = *b; + b += 4; + } + delete image; + return ret - m_cut; +} + diff --git a/2004/i/nono/src/camera/camera.h b/2004/i/nono/src/camera/camera.h new file mode 100644 index 0000000..b6c003e --- /dev/null +++ b/2004/i/nono/src/camera/camera.h @@ -0,0 +1,28 @@ +#ifndef camera_h +#define camera_h +// camera.h +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet + +class Camera +{ + int m_fd; + int m_w, m_h, m_d; + int m_cut; + int m_frameSize; + int m_lastRead; + public: + // Constructeur. + Camera (); + // Destructeur. + ~Camera (); + // Lit une image. + int read (unsigned char *image); + // Lit une image en RGB. + int readRGB (unsigned char *rgb); + // Lit la taille. + void getSize (int &w, int &h) const { w = m_w; h = m_h; } + void getSize (int &w, int &h, int &d) const { w = m_w; h = m_h; d = m_d; } +}; + +#endif // camera_h diff --git a/2004/i/nono/src/camera/dumpimage.cc b/2004/i/nono/src/camera/dumpimage.cc new file mode 100644 index 0000000..90909af --- /dev/null +++ b/2004/i/nono/src/camera/dumpimage.cc @@ -0,0 +1,36 @@ +// dumpimage.cc +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet +// +#include "camera.h" +#include "date/date.h" + +#include +#include + +int +main (int argc, char **argv) +{ + Camera cam; + Date date; + int w, h, d; + cam.getSize (w, h, d); + unsigned char *image = new unsigned char[w * h * d]; + cout << hex; + int j = 0; + int pixel; + while (1) + { + cout << dec << "\n- " << j++ << hex << " ---------"; + while (cam.read (image) == 0); + for (int i = 0; i < 256; ++i) + { + if (!(i % 16)) + cout << endl; + pixel = image[i]; + cout << setw (2) << setfill ('0') << pixel << ' '; + } + } + delete image; + return 0; +} diff --git a/2004/i/nono/src/camera/test_camera.cc b/2004/i/nono/src/camera/test_camera.cc new file mode 100644 index 0000000..9b616af --- /dev/null +++ b/2004/i/nono/src/camera/test_camera.cc @@ -0,0 +1,64 @@ +// test_camera.cc +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet +// +#include "camera.h" +#include "erreur/erreur.h" +#include "date/date.h" + +#include +#include +#include + +int +main (int argc, char **argv) +{ + try + { + Camera cam; + Date date; + sleep (5); + int w, h, d; + cam.getSize (w, h, d); + unsigned char *image = new unsigned char[w * h * d]; + unsigned char *image2 = new unsigned char[w * h * 3]; + for (int i = 0; i < 10; ++i) + { + char s[256]; + sprintf (s, "camera%d.gray", i); + ofstream o (s); + while (cam.read (image) == 0) sleep (1); + o.write (image, w * h * d); + o.close (); + if (d == 4) + { + for (int j = 0; j < w * h; ++j) + { + image2[j * 3] = image[j * 4 + 2]; + image2[j * 3 + 1] = image[j * 4 + 1]; + image2[j * 3 + 2] = image[j * 4]; + } + sprintf (s, "camera%d.rgb", i); + o.open (s); + o.write (image2, w * h * 3); + o.close (); + } + for (int j = 0; j < w * h; ++j) + { + int t = ((int) image[j * 4 + 2] >> 1) - ((int) image[j * 4 + 1] >> 1); + t = t > 0 ? t : -t; + image2[j] = t; + } + sprintf (s, "camera%d.diff.gray", i); + o.open (s); + o.write (image2, w * h); + o.close (); + } + return 0; + } + catch (Erreur &e) + { + cout << e.what (); + return 1; + } +} diff --git a/2004/i/nono/src/config/Makefile.defs b/2004/i/nono/src/config/Makefile.defs new file mode 100644 index 0000000..3aadde7 --- /dev/null +++ b/2004/i/nono/src/config/Makefile.defs @@ -0,0 +1,4 @@ +LIBS += config.a +config_a_SOURCES = config.cc config_lex.cc + +config.a: ${config_a_SOURCES:%.cc=config.a(%.o)} diff --git a/2004/i/nono/src/config/config.cc b/2004/i/nono/src/config/config.cc new file mode 100644 index 0000000..c36d75f --- /dev/null +++ b/2004/i/nono/src/config/config.cc @@ -0,0 +1,162 @@ +// config.cc - Lecture de fichiers de configuration. +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "config.h" +#include "config_lex.h" +#include "erreur/erreur.h" + +#include + +// Constructeur, prend l'identificateur de configuration en paramètre (nom de +// fichier). +Config::Config (const char *filename) + : m_filename (filename), + m_type (-1) +{ +#ifdef CONFIG_VARIANT + if (config_yyopen (filename, wrap_handler, this) == -1) +#else + if (config_yyopen (filename, 0, 0) == -1) +#endif + throw ErreurConfig (filename, "Erreur d'ouverture.\n"); +} + +// Destructeur. +Config::~Config (void) +{ + config_yyclose (); +} + +// Attend un token, sinon, jette une exception. +int +Config::getNum (void) +{ + if (m_type == -1) m_type = config_yylex (); + if (m_type != NUM) + throw ErreurConfig (m_filename.c_str (), "Nombre attendu.\n"); + m_type = -1; + return config_yylval.num; +} + +const char * +Config::getId (void) +{ + if (m_type == -1) m_type = config_yylex (); + if (m_type != ID) + throw ErreurConfig (m_filename.c_str (), "Identificateur attendu.\n"); + m_type = -1; + return config_yylval.id; +} + +bool +Config::getBool (void) +{ + if (m_type == -1) m_type = config_yylex (); + if (m_type != BOOL) + throw ErreurConfig (m_filename.c_str (), "Booléen attendu.\n"); + m_type = -1; + return config_yylval.boolean; +} + +double +Config::getFloat (void) +{ + if (m_type == -1) m_type = config_yylex (); + if (m_type == FLOAT) + { + m_type = -1; + return config_yylval.fl; + } + else if (m_type == NUM) + { + m_type = -1; + return (double) config_yylval.num; + } + else + throw ErreurConfig (m_filename.c_str (), "Flotant attendu.\n"); +} + +void +Config::getString (std::string &s) +{ + if (m_type == -1) m_type = config_yylex (); + if (m_type != STRING) + throw ErreurConfig (m_filename.c_str (), + "Chaîne de caractères attendue.\n"); + m_type = -1; + s = config_yylval.str; +} + +void +Config::getEof (void) +{ + if (m_type == -1) m_type = config_yylex (); + if (m_type) + throw ErreurConfig (m_filename.c_str (), + "Fin de fichier attendue.\n"); +} + +// Retourne true si à la fin du fichier. +bool +Config::eof (void) +{ + if (m_type == -1) m_type = config_yylex (); + return m_type == 0; +} + +// Vérifie si le prochain token est... +bool +Config::isId (const char *s) +{ + if (m_type == -1) m_type = config_yylex (); + if (m_type != ID) + throw ErreurConfig (m_filename.c_str (), "Identificateur attendu.\n"); + return strcmp (config_yylval.id, s) == 0; +} + +// Lance une erreur, on a pas trouvé ce qu'on voulais. +void +Config::noId (void) +{ + throw ErreurConfig (m_filename.c_str (), config_yylval.id, + "Identificateur inconnu.\n"); +} + +// Appellé par le lexer pour changer de fichier. +bool +Config::wrap_handler (void *p) +{ + Config *c = reinterpret_cast (p); + return c->wrap (); +} + +bool +Config::wrap (void) +{ +#ifdef CONFIG_VARIANT + return (config_yyopen ((m_filename + "." CONFIG_VARIANT).c_str (), 0, 0) + == -1) ? false : true; +#endif +} + diff --git a/2004/i/nono/src/config/config.h b/2004/i/nono/src/config/config.h new file mode 100644 index 0000000..a958844 --- /dev/null +++ b/2004/i/nono/src/config/config.h @@ -0,0 +1,64 @@ +#ifndef config_h +#define config_h +// config.h - Lecture de fichiers de configuration. +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#include + +class Config +{ + std::string m_filename; + int m_type; + public: + // Constructeur, prend l'identificateur de configuration en paramètre (nom + // de fichier). + Config (const char *filename); + // Destructeur. + ~Config (void); + // Attend un token, sinon, jette une exception. + int getNum (void); + Config &operator>> (int &num) { num = getNum (); return *this; } + const char *getId (void); + Config &operator>> (const char *&s) { s = getId (); return *this; } + bool getBool (void); + Config &operator>> (bool &b) { b = getBool (); return *this; } + double getFloat (void); + Config &operator>> (double &fl) { fl = getFloat (); return *this; } + void getString (std::string &s); + Config &operator>> (std::string &s) { getString (s); return *this; } + void getEof (void); + // Vérifie si le prochain token est... + bool isId (const char *s); + // Retourne true si à la fin du fichier. + bool eof (void); + // Lance une erreur, on a pas trouvé ce qu'on voulais. + void noId (void); + private: + // Appellé par le lexer pour changer de fichier. + static bool wrap_handler (void *p); + bool wrap (void); +}; + +#endif // config_h diff --git a/2004/i/nono/src/config/config_lex.h b/2004/i/nono/src/config/config_lex.h new file mode 100644 index 0000000..0ef7003 --- /dev/null +++ b/2004/i/nono/src/config/config_lex.h @@ -0,0 +1,60 @@ +#ifndef config_lex_h +#define config_lex_h +// config_lex.h +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#define NUM 0x100 +#define FLOAT 0x101 +#define ID 0x102 +#define BOOL 0x103 +#define STRING 0x104 +#define ERR 0x105 + +union config_yytype +{ + double fl; + int num; + const char *id; + bool boolean; + const char *str; +}; + +extern config_yytype config_yylval; + +typedef bool (*config_wrap_f) (void *); + +// Fournis par flex : renvois le type du prochain token. +int +config_yylex (void); + +// Ouvre un fichier pour l'utiliser avec le lexer. +int +config_yyopen (const char *filename, config_wrap_f wrap, void *data); + +// Ferme le fichier du lexer. +void +config_yyclose (void); + +#endif // config_lex_h diff --git a/2004/i/nono/src/config/config_lex.ll b/2004/i/nono/src/config/config_lex.ll new file mode 100644 index 0000000..1387203 --- /dev/null +++ b/2004/i/nono/src/config/config_lex.ll @@ -0,0 +1,127 @@ +%{ +/* nono - programme du robot 2004. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2004. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +#include "config/config_lex.h" + +config_yytype config_yylval; +config_wrap_f wrap_f = 0; +void *wrap_data = 0; + +%} + +ID [_a-zA-Z][-_a-zA-Z0-9]* + +NUM [-+]?[0-9]+ + +HNUM "0x"[0-9a-fzA-F]+ + +BNUM "0b"[01]+ + +FLOAT [-+]?[0-9]*"."[0-9]* + +STRING \"[^\n\"]*\" + +%option nounput +%option prefix="config_yy" +%option yylineno + +%% + +{FLOAT} { + config_yylval.fl = strtod (config_yytext, 0); + return FLOAT; +} + +{BNUM} { + config_yylval.num = strtol (config_yytext + 2, 0, 2); + return NUM; +} + +{HNUM} { + config_yylval.num = strtol (config_yytext + 2, 0, 16); + return NUM; +} + +{NUM} { + config_yylval.num = strtol (config_yytext, 0, 10); + return NUM; +} + +"true" { config_yylval.boolean = true; return BOOL; } +"false" { config_yylval.boolean = false; return BOOL; } + +{ID} { + config_yylval.id = config_yytext; + return ID; +} + +{STRING} { + config_yytext[config_yyleng - 1] = '\0'; + config_yylval.str = config_yytext + 1; + return STRING; +} + +"#".* /* Commentaires. */ + +[ \t\n]+ /* Rien à battre. */ + +. return ERR; + +%% + +// Ouvre un fichier pour l'utiliser avec le lexer. +int +config_yyopen (const char *filename, config_wrap_f wrap, void *data) +{ + if (config_yyin && config_yyin != stdin) + fclose (config_yyin); + config_yyin = fopen (filename, "r"); + if (!config_yyin) return -1; + wrap_f = wrap; + wrap_data = data; + return 0; +} + +// Ferme le fichier du lexer. +void +config_yyclose (void) +{ + if (config_yyin && config_yyin != stdin) + fclose (config_yyin); + wrap_f = 0; + wrap_data = 0; +} + +// Appelé par flex pour changer de fichier. +int +config_yywrap (void) +{ + if (wrap_f) + return wrap_f (wrap_data) ? 0 : 1; + else + return 1; +} + +// vim: ft=lex diff --git a/2004/i/nono/src/date/Makefile.defs b/2004/i/nono/src/date/Makefile.defs new file mode 100644 index 0000000..1d4474c --- /dev/null +++ b/2004/i/nono/src/date/Makefile.defs @@ -0,0 +1,8 @@ +LIBS += date.a +TARGETS += test_date +date_a_SOURCES = date.cc +test_date_SOURCES = test_date.cc date.a + +test_date: $(test_date_SOURCES:%.cc=%.o) + +date.a: ${date_a_SOURCES:%.cc=date.a(%.o)} diff --git a/2004/i/nono/src/date/date.cc b/2004/i/nono/src/date/date.cc new file mode 100644 index 0000000..25243ad --- /dev/null +++ b/2004/i/nono/src/date/date.cc @@ -0,0 +1,74 @@ +// date.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "date.h" + +#include +#include + +// Pointeur vers l'instance unique. +Date *Date::m_instance = 0; + +// Constructeur. +Date::Date (void) +{ + m_start = getDate (); + m_round = -1; + m_instance = this; +} + +// Destructeur. +Date::~Date (void) +{ + m_instance = 0; +} + +// Signale le début du match. +void +Date::startRound (void) +{ + m_round = getDate (); +} + +// Retourne la date système. +int +Date::getDate (void) +{ + timeval tv; + gettimeofday (&tv, 0); + // Ça veut dire, un bug tous les 11 jours... + // Cette année : le 26/05/2003, et le 06/05/2003. Mais le robot n'est pas + // à l'heure de toute façon. + return (tv.tv_sec % 1000000) * 1000 + tv.tv_usec / 1000; +} + +// Attend un nombre de millisecondes. +void +Date::wait (int t) +{ + timespec ts, r; + ts.tv_sec = t / 1000; + ts.tv_nsec = (t % 1000) * 1000000; + nanosleep (&ts, &r); +} diff --git a/2004/i/nono/src/date/date.h b/2004/i/nono/src/date/date.h new file mode 100644 index 0000000..1a40adc --- /dev/null +++ b/2004/i/nono/src/date/date.h @@ -0,0 +1,54 @@ +#ifndef date_h +#define date_h +// date.h +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +class Date +{ + // On travaille en millième de seconde, ça nous fait 24 jours dans un int. + int m_start, m_round; + // Pointeur vers l'instance unique. + static Date *m_instance; + public: + // Constructeur. + Date (void); + // Destructeur. + ~Date (void); + // Retourne une référence vers l'instance unique. + static Date &getInstance (void) { return *m_instance; } + // Retourne la date depuis le début du match, en millième de secondes. + int round (void) { return m_round == -1 ? 0 : getDate () - m_round; } + // Retourne la date depuis le début du programme. + int start (void) { return getDate () - m_start; } + // Signale le début du match. + void startRound (void); + // Attend un nombre de millisecondes. + static void wait (int t); + private: + // Retourne la date système. + int getDate (void); +}; + +#endif // date_h diff --git a/2004/i/nono/src/date/test_date.cc b/2004/i/nono/src/date/test_date.cc new file mode 100644 index 0000000..fe020e1 --- /dev/null +++ b/2004/i/nono/src/date/test_date.cc @@ -0,0 +1,42 @@ +// test_date.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "date.h" + +#include +#include + +int +main (void) +{ + Date d; + sleep (2); + cout << "date " << d.start () << ' ' << d.round () << endl; + d.startRound (); + sleep (1); + cout << "date " << d.start () << ' ' << d.round () << endl; + sleep (2); + cout << "date " << d.start () << ' ' << d.round () << endl; + return 0; +} diff --git a/2004/i/nono/src/erreur/Makefile.defs b/2004/i/nono/src/erreur/Makefile.defs new file mode 100644 index 0000000..9ff08a8 --- /dev/null +++ b/2004/i/nono/src/erreur/Makefile.defs @@ -0,0 +1,4 @@ +LIBS += erreur.a +erreur_a_SOURCES = erreur.cc + +erreur.a: ${erreur_a_SOURCES:%.cc=erreur.a(%.o)} diff --git a/2004/i/nono/src/erreur/erreur.cc b/2004/i/nono/src/erreur/erreur.cc new file mode 100644 index 0000000..241b243 --- /dev/null +++ b/2004/i/nono/src/erreur/erreur.cc @@ -0,0 +1,31 @@ +// erreur.cc +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet +// +#include "erreur.h" + +#include + +// Retourne une description de l'erreur. +const char * +Erreur::what () const throw () +{ + return "Erreur non spécifiée.\n"; +} + +const char * +ErreurFatale::what () const throw () +{ + return m_descr; +} + +const char * +ErreurConfig::what () const throw () +{ + static char descr[1024]; + if (!m_id) + sprintf (descr, "%s: %s", m_file, m_descr); + else + sprintf (descr, "%s: %s: %s", m_file, m_id, m_descr); + return descr; +} diff --git a/2004/i/nono/src/erreur/erreur.h b/2004/i/nono/src/erreur/erreur.h new file mode 100644 index 0000000..fcbb0eb --- /dev/null +++ b/2004/i/nono/src/erreur/erreur.h @@ -0,0 +1,39 @@ +#ifndef erreur_h +#define erreur_h +// erreur.h +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet + +#include + +class Erreur : public std::exception +{ + public: + // Retourne une description de l'erreur. + virtual const char *what () const throw (); +}; + +class ErreurFatale : public Erreur +{ + const char *m_descr; + public: + ErreurFatale (const char *descr) { m_descr = descr; } + virtual const char *what () const throw (); +}; + +class ErreurConfig : public Erreur +{ + const char *m_file; + const char *m_id; + const char *m_descr; + public: + ErreurConfig (const char *file, const char *id, + const char *descr) + { m_file = file; m_id = id; m_descr = descr; } + ErreurConfig (const char *file, const char *descr) + { m_file = file; m_id = 0; m_descr = descr; } + virtual const char *what () const throw (); +}; + + +#endif // erreur_h diff --git a/2004/i/nono/src/kernel_mips/pbus.c b/2004/i/nono/src/kernel_mips/pbus.c new file mode 100644 index 0000000..4d16589 --- /dev/null +++ b/2004/i/nono/src/kernel_mips/pbus.c @@ -0,0 +1,393 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define BONITO_I2C +#include +#include +#include +#include + +/* autoconfig stuff... */ + +#define BUFSZ (256<<10) +#define DMASIZE (2*BUFSZ) + +struct pbus_softc { + struct device sc_dev; /* REQUIRED first entry */ + void *sc_ih; /* interrupt handle */ + bus_space_tag_t iot; + bus_space_handle_t ioh; + bus_dma_tag_t dmat; + unsigned long iobase; + bus_dmamap_t sc_dmam; /* bus dma map */ + unsigned char *sc_cpudma; /* dma memory space from cpu space*/ + unsigned long sc_bonitodma; /* dma memory space from pci space*/ + unsigned long sc_dmasize; /* dma buffer size */ + struct proc *cproc; /* Control process */ + int csignal; /* Control signal */ + int in_dma; + int buf_ok; + int framesize; + int buf_consumed; + int pbusinten; +}; + +static int pbusprobe(struct device *, struct cfdata *, void *); +static void pbusattach(struct device *, struct device *, void *); +unsigned char i2c_ovt_read(unsigned char addr); +int i2c_ovt_write(unsigned char addr, unsigned char data); + +struct cfattach pbus_ca = { + sizeof(struct pbus_softc), pbusprobe, pbusattach +}; + +#define BUFSYNC_PRE(n) \ + bus_dmamap_sync(sc->dmat,sc->sc_dmam,(n)*BUFSZ,BUFSZ,BUS_DMASYNC_PREREAD) +#define BUFSYNC_POST(n) \ + bus_dmamap_sync(sc->dmat,sc->sc_dmam,(n)*BUFSZ,BUFSZ,BUS_DMASYNC_POSTREAD) + +#define PBUS_WRITE_4(reg, val) \ + bus_space_write_4(sc->iot, sc->ioh, reg, val) +#define PBUS_READ_4(reg) \ + bus_space_read_4(sc->iot, sc->ioh, reg) + +static int +pbusprobe(struct device *parent, struct cfdata *cf, void *aux) +{ + struct bonito_io_attach *bia = aux; + struct pbus_softc scp; + struct pbus_softc *sc=&scp; + sc->iobase=bia->base; + + return 1; +} + +int pbus_intr(void *p); + +static void +pbusattach(parent, self, aux) + struct device *parent, *self; + void *aux; +{ + bus_dma_segment_t seg; + int rseg; + struct bonito_io_attach *bia = aux; + struct pbus_softc *sc = (struct pbus_softc *)self; + + sc->iot=bia->iot; + sc->dmat=bia->dmat; + bus_space_map(sc->iot, bia->base, 12, 0, &(sc->ioh)); + + sc->iobase=(unsigned long)sc->ioh; + printf("\n"); + + + PBUS_WRITE_4(0x08,PBUS_RnW); + PBUS_WRITE_4(0x0,0); + sc->sc_dmasize=DMASIZE; + sc->in_dma=0; + sc->buf_ok=0; + sc->framesize=BUFSZ; + sc->csignal=SIGUSR1; + sc->pbusinten=0; + sc->buf_consumed=0; + + /* + * Allocate a DMA area for the card. + */ + if (bus_dmamem_alloc(sc->dmat, sc->sc_dmasize, NBPG, 0, &seg, 1, + &rseg, BUS_DMA_NOWAIT|BUS_DMA_DIRECT)) + { + printf("%s: couldn't allocate DMA\n", + sc->sc_dev.dv_xname); + return; + } + if (bus_dmamem_map(sc->dmat, &seg, rseg, sc->sc_dmasize, + (caddr_t *)&sc->sc_cpudma, + BUS_DMA_NOWAIT|BUS_DMA_DIRECT)) + { + printf("%s: couldn't map DMA\n", + sc->sc_dev.dv_xname); + return; + } + + /* + * Create and load the DMA map for the DMA area. + */ + if (bus_dmamap_create(sc->dmat, sc->sc_dmasize, 1, + sc->sc_dmasize, 0, BUS_DMA_NOWAIT|BUS_DMA_DIRECT, &sc->sc_dmam)) + { + printf("%s: couldn't create DMA map\n", + sc->sc_dev.dv_xname); + bus_dmamem_free(sc->dmat, &seg, rseg); + return; + } + if (bus_dmamap_load(sc->dmat, sc->sc_dmam, + sc->sc_cpudma, sc->sc_dmasize, NULL, BUS_DMA_NOWAIT|BUS_DMA_DIRECT)) + { + printf("%s: coundn't load DMA map\n", + sc->sc_dev.dv_xname); + bus_dmamem_free(sc->dmat, &seg, rseg); + return; + } + + sc->sc_bonitodma = sc->sc_dmam->dm_segs[0].ds_addr; + bzero(sc->sc_cpudma,DMASIZE); + + sc->sc_ih = bonito_intr_establish(bia->irq, IPL_NET, pbus_intr, sc); + if (sc->sc_ih == NULL) + { + printf("%s: couldn't establish interrupt at %d\n", + sc->sc_dev.dv_xname,bia->irq); + return; + } + + printf("%s: DMA map:0x%08lx (PHYS:0x%08lx)\n", sc->sc_dev.dv_xname, (unsigned long) sc->sc_cpudma, sc->sc_bonitodma); + printf("%s: interrupting at %d\n", sc->sc_dev.dv_xname, bia->irq); + +} + + + +/* + * operational routines: + * open, close, read, write, + * ioctl, mmap + */ + +dev_type_open(pbusopen); +dev_type_close(pbusclose); +dev_type_read(pbusread); +dev_type_write(pbuswrite); +dev_type_ioctl(pbusioctl); +dev_type_mmap(pbusmmap); + +int +pbusopen(dev, flag, fmt, proc) + dev_t dev; + int flag, fmt; + struct proc *proc; +{ + struct pbus_softc *sc= pbus_cd.cd_devs[0]; + + if (minor(dev)==1) /* PBUS */ + sc->cproc=proc; + return 0; +} + +int +pbusclose(dev, flag, fmt, proc) + dev_t dev; + int flag, fmt; + struct proc *proc; +{ + struct pbus_softc *sc= pbus_cd.cd_devs[0]; + if (minor(dev)==1) /* PBUS */ + sc->cproc=NULL; + return 0; +} + +int +pbusread(dev, uio, flags) + dev_t dev; + struct uio *uio; + int flags; +{ + int bufok,remain; + int resid=uio->uio_resid; + int s; + struct pbus_softc *sc= pbus_cd.cd_devs[0]; + s=splhigh(); + bufok=*((volatile int *)(&sc->buf_ok)); + if (!bufok) + { + BUFSYNC_PRE(0); + sc->in_dma=1; + PBUS_WRITE_4(0x4,sc->sc_bonitodma); + } + splx(s); + if (!resid) return 0; + if (!bufok) return 0; + + remain=sc->framesize-sc->buf_consumed; + if (resid>remain) resid=remain; + + uiomove(sc->sc_cpudma+sc->buf_consumed,resid,uio); + sc->buf_consumed+=resid; + remain-=resid; + if (!remain) + { + int s=splhigh(); + sc->buf_consumed=0; + sc->buf_ok=0; + if (!sc->in_dma) + { + BUFSYNC_PRE(0); + sc->in_dma=1; + PBUS_WRITE_4(0x4,sc->sc_bonitodma); + } + splx(s); + } + return 0; +} + +int +pbuswrite(dev, uio, flags) + dev_t dev; + struct uio *uio; + int flags; +{ + return ENODEV; +} + +int pbus_intr(void *p) +{ + struct pbus_softc *sc = (struct pbus_softc *)p; + unsigned long status=PBUS_READ_4(0); + if ((status&0x20000000)&&(sc->pbusinten)) + { + /* Désactive les interuptions. */ + PBUS_WRITE_4(8,PBUS_READ_4(8)&0x3FFF); + sc->pbusinten=0; + /* Envoie le signal à l'application. */ + if (sc->cproc!=NULL) psignal(sc->cproc,sc->csignal); + } + if (status&0x40000000) + { + /* Frame Ok */ + BUFSYNC_POST(0); + sc->buf_ok=1; + sc->in_dma=0; + wakeup(sc); + } + return 0; +} + +#define PBUS_WAIT \ + tsleep(sc,PUSER | PCATCH,"pbusio",(HZ/10000)?(HZ/10000):1) + + +#define OVTDEV 0xC0 +#define OVTCHN BOARDI2C + +unsigned char i2c_ovt_read(unsigned char addr) +{ + int retval; + + i2c_send_start(OVTCHN); + i2c_write_byte(OVTCHN,OVTDEV|0); + i2c_write_byte(OVTCHN,addr); + i2c_send_stop(OVTCHN); + i2c_send_start(OVTCHN); + i2c_write_byte(OVTCHN,OVTDEV|1); + retval=i2c_read_byte(OVTCHN,0); + i2c_send_stop(OVTCHN); + + return retval; +} + +int i2c_ovt_write(unsigned char addr, unsigned char data) +{ + i2c_send_start(OVTCHN); + i2c_write_byte(OVTCHN,OVTDEV|0); + i2c_write_byte(OVTCHN,addr); + i2c_write_byte(OVTCHN,data); + i2c_send_stop(OVTCHN); + return 1; +} + +int +pbusioctl(dev, cmd, addr, flag, proc) + dev_t dev; + u_long cmd; + int flag; + caddr_t addr; + struct proc *proc; +{ + struct pbus_softc *sc= pbus_cd.cd_devs[0]; + struct pbus_io *parm=(struct pbus_io *)addr; + struct sccb_io *sccb=(struct sccb_io *)addr; + unsigned long *fsize=(unsigned long *)addr; + /* PBUS IOctl */ + if (minor(dev)==1) /* PBUS */ + { + switch (cmd) + { + case PBUS_READ: + PBUS_WRITE_4(8,(parm->addr<<8)|PBUS_RnW|sc->pbusinten); + PBUS_WRITE_4(8,(parm->addr<<8)|PBUS_CLK|PBUS_RnW|sc->pbusinten); + PBUS_WAIT; + parm->data=PBUS_READ_4(8)&0xFF; + PBUS_WRITE_4(8,(parm->addr<<8)|PBUS_RnW|sc->pbusinten); + PBUS_WAIT; + break; + case PBUS_WRITE: + PBUS_WRITE_4(8,(parm->addr<<8)|(parm->data)|sc->pbusinten); + PBUS_WRITE_4(8,(parm->addr<<8)|PBUS_CLK|(parm->data)|sc->pbusinten); + PBUS_WAIT; + PBUS_WRITE_4(8,(parm->addr<<8)|(parm->data)|sc->pbusinten); + PBUS_WAIT; + PBUS_WRITE_4(8,(parm->addr<<8)|PBUS_RnW|sc->pbusinten); + break; + case PBUS_INTDISABLE: + sc->pbusinten=0; + PBUS_WRITE_4(8,0|PBUS_RnW|sc->pbusinten); + break; + case PBUS_INTENABLE: + sc->pbusinten=PBUS_INTEN; + PBUS_WRITE_4(8,0|PBUS_RnW|sc->pbusinten); + break; + case PBUS_INTREAD: + parm->ints=(PBUS_READ_4(8)>>16)&0x1F; + break; + } + } + + if (minor(dev)==0) /* CAM */ + { + int s; + switch (cmd) + { + case CAM_SETHCOUNT: + PBUS_WRITE_4(0xC,*fsize); + break; + case CAM_SETFRAMESIZE: + s=splhigh(); + PBUS_WRITE_4(0x0,1); + sc->framesize=*fsize; + sc->buf_consumed=0; + sc->buf_ok=0; + if (!sc->in_dma) + { + BUFSYNC_PRE(0); + sc->in_dma=1; + PBUS_WRITE_4(0x4,sc->sc_bonitodma); + } + splx(s); + break; + case CAM_SCCBREAD: + sccb->data=i2c_ovt_read(sccb->addr); + break; + case CAM_SCCBWRITE: + i2c_ovt_write(sccb->addr,sccb->data); + break; + } + } + + return 0; +} + +paddr_t +pbusmmap(dev_t dev, off_t offset, int nprot) +{ + return ENXIO; +} + + diff --git a/2004/i/nono/src/kernel_mips/pbus.h b/2004/i/nono/src/kernel_mips/pbus.h new file mode 100644 index 0000000..351b144 --- /dev/null +++ b/2004/i/nono/src/kernel_mips/pbus.h @@ -0,0 +1,35 @@ +#ifndef _PBUS_H_ +#define _PBUS_H_ + +#include + +struct pbus_io +{ + unsigned char addr; /* Address */ + unsigned char data; /* Data */ + unsigned char ints; /* IRQ lines */ +}; + +#define PBUS_DATAMASK 0x00FF +#define PBUS_ADDRMASK 0x0F00 +#define PBUS_RnW 0x1000 +#define PBUS_CLK 0x2000 +#define PBUS_INTEN 0x4000 + +#define PBUS_READ _IOWR('R', 1, struct pbus_io) +#define PBUS_WRITE _IOWR('R', 2, struct pbus_io) +#define PBUS_INTENABLE _IO('R', 3) +#define PBUS_INTDISABLE _IO('R', 4) +#define PBUS_INTREAD _IOWR('R', 5, struct pbus_io) + +struct sccb_io +{ + unsigned char addr; /* Address */ + unsigned char data; /* Data */ +}; + +#define CAM_SETFRAMESIZE _IOW('R', 10, unsigned long) +#define CAM_SCCBREAD _IOWR('R',11, struct sccb_io) +#define CAM_SCCBWRITE _IOWR('R',12, struct sccb_io) +#define CAM_SETHCOUNT _IOW('R', 13, unsigned long) +#endif diff --git a/2004/i/nono/src/motor/Makefile.defs b/2004/i/nono/src/motor/Makefile.defs new file mode 100644 index 0000000..5fda0ed --- /dev/null +++ b/2004/i/nono/src/motor/Makefile.defs @@ -0,0 +1,19 @@ +TARGETS += test_motor test_asserv test_tracker +LIBS += motor.a +test_motor_SOURCES = test_motor.cc motor.a serial.a erreur.a config.a date.a +test_asserv_SOURCES = test_asserv.cc motor.a serial.a erreur.a config.a date.a +test_tracker_SOURCES = test_tracker.cc motor.a serial.a erreur.a config.a \ + date.a +motor_a_SOURCES = asserv.cc tracker.cc motor.cc movement_basic.cc \ + movement_goto.cc + +test_motor: $(test_motor_SOURCES:%.cc=%.o) + +test_asserv: $(test_asserv_SOURCES:%.cc=%.o) + +test_tracker: $(test_tracker_SOURCES:%.cc=%.o) + +motor.a: ${motor_a_SOURCES:%.cc=motor.a(%.o)} + +clean_motor: + rm -f test_motor test_asserv test_tracker motor.a diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc new file mode 100644 index 0000000..2d005b9 --- /dev/null +++ b/2004/i/nono/src/motor/asserv.cc @@ -0,0 +1,385 @@ +// asserv.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "asserv.h" +#include "config/config.h" +#include "date/date.h" + +#include + +// Table de conversion en hexa. +const char *Asserv::hexaTbl_ = "0123456789abcdef"; + +// Constructeur. +Asserv::Asserv (AsservTracker &asservTracker) + : accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1), + statMotor_ (false), counter_ (false), + inBufSize_ (64), inBufPos_ (0), inBuf_ (0), + firstCounter_ (true), + countLeft_ (0), countRight_ (0), + asservTracker_ (asservTracker) +{ + // Lit la conf. + Config rc ("rc/asserv"); + while (!rc.eof ()) + { + if (rc.isId ("accel")) + { + rc.getId (); + rc >> accel_; + } + else if (rc.isId ("kp")) + { + rc.getId (); + rc >> kp_; + } + else if (rc.isId ("ki")) + { + rc.getId (); + rc >> ki_; + } + else if (rc.isId ("kd")) + { + rc.getId (); + rc >> kd_; + } + else if (rc.isId ("tty")) + { + rc.getId (); + rc >> ttyname_; + } + else if (rc.isId ("stats")) + { + rc.getId (); + rc >> statMotor_; + } + else if (rc.isId ("counter")) + { + rc.getId (); + rc >> counter_; + } + else if (rc.isId ("buffer")) + { + rc.getId (); + rc >> inBufSize_; + } + else rc.noId (); + } + // Ouvre le port série. + serial_.open (ttyname_.c_str ()); + // Alloue la mémoire du tampon d'entrée. + inBuf_ = new char[inBufSize_]; +} + +// Destructeur. +Asserv::~Asserv (void) +{ + // Envois le signal de reset. + send ('z'); + waitOk (); + delete inBuf_; +} + +// Reset la carte et envois les paramètres. +void +Asserv::reset (void) +{ + // Reset les données. + countLeft_ = countRight_ = 0; + inBufPos_ = 0; + while (!sendQueue_.empty ()) sendQueue_.pop (); + // Envois le signal de reset. + send ('z'); + // Renvois les paramètres. + if (accel_ != -1) + setAccel (accel_); + if (kp_ != -1) + setKp (kp_); + if (ki_ != -1) + setKi (ki_); + if (kd_ != -1) + setKi (kd_); + if (statMotor_) + setStatMotor (statMotor_); + if (counter_) + setCounter (counter_); +} + +// Active l'asservissement. +void +Asserv::go (bool fl) +{ + send ('g', fl); +} + +// Stop ! +void +Asserv::stop (void) +{ + send ('s'); +} + +// Réglage de la vitesse. +void +Asserv::speed (int l, int r) +{ + send ('v', l, r); +} + +// Teste si l'émission est terminée. +bool +Asserv::ok (void) +{ + read (); + return sendQueue_.empty (); +} + +// Attend que toute les émissions soit terminées. +void +Asserv::waitOk (void) +{ + while (!ok ()) Date::wait (1); +} + +// Lit et traite les messages de la cartes. +void +Asserv::read (void) +{ + int c; + while ((c = serial_.getchar ()) != -1) + { + if (c == '\n' || c == '\r') + { + if (inBufPos_) + // Tampon plein. + handleMessage (); + } + else + { + if (c == '!' || inBufPos_ >= inBufSize_) + inBufPos_ = 0; + inBuf_[inBufPos_++] = c; + } + } +} + +// Change les paramètres de la carte. +void +Asserv::setAccel (int accel) +{ + send ('a', accel); + accel_ = accel; +} + +void +Asserv::setKp (int kp) +{ + send ('p', kp); + kp_ = kp; +} + +void +Asserv::setKi (int ki) +{ + send ('i', ki); + ki_ = ki; +} + +void +Asserv::setKd (int kd) +{ + send ('d', kd); + kd_ = kd; +} + +void +Asserv::setStatMotor (bool fl) +{ + send ('m', fl); + statMotor_ = fl; +} + +void +Asserv::setCounter (bool fl = true) +{ + firstCounter_ = true; + send ('c', fl); + counter_ = fl; +} + +// Envoie un message. +void +Asserv::send (char com) +{ + bool wasEmpty = ok (); + std::string s; + s += '!'; + s += com; + s += '\r'; + sendQueue_.push (s); + if (wasEmpty) sendLast (); +} + +void +Asserv::send (char com, bool fl) +{ + bool wasEmpty = ok (); + std::string s; + s += '!'; + s += com; + s += fl ? '1' : '0'; + s += '\r'; + sendQueue_.push (s); + if (wasEmpty) sendLast (); +} + +void +Asserv::send (char com, int a1) +{ + bool wasEmpty = ok (); + std::string s; + s += '!'; + s += com; + s += hexaTbl_[(a1 >> 4) & 0x0f]; + s += hexaTbl_[a1 & 0x0f]; + s += '\r'; + sendQueue_.push (s); + if (wasEmpty) sendLast (); +} + +void +Asserv::send (char com, int a1, int a2) +{ + bool wasEmpty = ok (); + std::string s; + s += '!'; + s += com; + s += hexaTbl_[(a1 >> 4) & 0x0f]; + s += hexaTbl_[a1 & 0x0f]; + s += hexaTbl_[(a2 >> 4) & 0x0f]; + s += hexaTbl_[a2 & 0x0f]; + s += '\r'; + sendQueue_.push (s); + if (wasEmpty) sendLast (); +} + +// Renvois le dernier message. +void +Asserv::sendLast (void) +{ + if (sendQueue_.empty ()) return; + std::string &s = sendQueue_.front (); +std::cout << "send " << s << std::endl; + serial_.write (s.data (), s.size ()); +} + +// Traite un message. +void +Asserv::handleMessage (void) +{ +inBuf_[inBufPos_] = 0; +//std::cout << "recv " << inBuf_ << std::endl; + if (inBufPos_ > 1 && inBuf_[0] == '!') + { + switch (inBuf_[1]) + { + case 'o': + case 'z': + // Ok. + if (!sendQueue_.empty ()) + sendQueue_.pop (); + sendLast (); + break; + case 'e': + // Erreur, renvois la dernière commande. + sendLast (); + break; + case 'm': + // Recois des nouvelles stats. + // handleStatMotor (); + break; + case 'c': + // Recois une nouvelle valeur pour les compteurs. + handleCounter (); + break; + } + } + // Efface le tampon. + inBufPos_ = 0; +} + +// Traite un message du compteur. +void +Asserv::handleCounter (void) +{ + if (inBufPos_ != 2 + 4 + 1 + 4) + { +std::cout << "counter error" << std::endl; + // Mauvaise transmission. + return; + } + int l = getSignedShort (inBuf_ + 2); + int r = getSignedShort (inBuf_ + 2 + 4 + 1); + if (firstCounter_) + { + // Première valeur ignorée. + countLeft_ = l; + countRight_ = r; + firstCounter_ = false; + } + // Attention à l'overflow. +//std::cout << "counter before " << countLeft_ << ' ' << countRight_ << ' ' << l << ' ' << r << std::endl; + if (l > 0x4000 && countLeft_ < -0x4000) + countLeft_ += 0x10000; + if (l < -0x4000 && countLeft_ > 0x4000) + countLeft_ -= 0x10000; + if (r > 0x4000 && countRight_ < -0x4000) + countRight_ += 0x10000; + if (r < -0x4000 && countRight_ > 0x4000) + countRight_ -= 0x10000; +//std::cout << "counter after " << countLeft_ << ' ' << countRight_ << ' ' << l << ' ' << r << std::endl; + // Met à jour le tracker. + asservTracker_.updateCounter (l - countLeft_, r - countRight_); +//std::cout << "tracker update " << l - countLeft_ << ' ' << r - countRight_ << std::endl; + // Retiens les anciennes valeurs. + countLeft_ = l; + countRight_ = r; +} + +// Décode un mot signé (2 octets). +int +Asserv::getSignedShort (const char *s) const +{ + return (short) (hex2digit (s[0]) << 12 + | hex2digit (s[1]) << 8 + | hex2digit (s[2]) << 4 + | hex2digit (s[3])); +} + +// Décode un chiffre hexa. +int +Asserv::hex2digit (char c) const +{ + return (c >= '0' && c <= '9') ? c - '0' : c - 'a' + 10; +} diff --git a/2004/i/nono/src/motor/asserv.h b/2004/i/nono/src/motor/asserv.h new file mode 100644 index 0000000..d6f1da1 --- /dev/null +++ b/2004/i/nono/src/motor/asserv.h @@ -0,0 +1,106 @@ +#ifndef asserv_h +#define asserv_h +// asserv.h +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "serial/serial.h" + +#include +#include + +// Class dont on doit dériver pour pouvoir recevoir les informations de la +// carte d'asservissement. +class AsservTracker +{ + public: + // Appelée lors d'une mise à jour des compteurs. + virtual void updateCounter (int l, int r) { } +}; + +class Asserv +{ + // Ligne série. + Serial serial_; + std::string ttyname_; + // Paramètres. + int accel_, kp_, ki_, kd_; + bool statMotor_, counter_; + // File d'emmission. + std::queue sendQueue_; + // Table de conversion en hexa. + static const char *hexaTbl_; + // Buffer de reception. + int inBufSize_, inBufPos_; + char *inBuf_; + // Anciènne valeur des compteur. + bool firstCounter_; + int countLeft_, countRight_; + // Objet interessé par les stats. + AsservTracker &asservTracker_; + public: + // Constructeur. + Asserv (AsservTracker &asservTracker); + // Destructeur. + ~Asserv (void); + // Reset la carte et envois les paramètres. + void reset (void); + // Active l'asservissement. + void go (bool fl = true); + // Stop ! + void stop (void); + // Réglage de la vitesse. + void speed (int l, int r); + // Teste si l'émission est terminée. + bool ok (void); + // Attend que toute les émissions soit terminées. + void waitOk (void); + // Lit et traite les messages de la cartes. + void read (void); + // Change les paramètres de la carte. + void setAccel (int accel); + int getAccel (void) { return accel_; } + void setKp (int kp); + void setKi (int ki); + void setKd (int kd); + void setStatMotor (bool fl = true); + void setCounter (bool fl = true); + protected: + // Envoie un message. + void send (char com); + void send (char com, bool fl); + void send (char com, int a1); + void send (char com, int a1, int a2); + // Renvois le dernier message. + void sendLast (void); + // Traite un message. + void handleMessage (void); + // Traite un message du compteur. + void handleCounter (void); + // Décode un mot signé (2 octets). + int getSignedShort (const char *s) const; + // Décode un chiffre hexa. + int hex2digit (char c) const; +}; + +#endif // asserv_h diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc new file mode 100644 index 0000000..615511c --- /dev/null +++ b/2004/i/nono/src/motor/motor.cc @@ -0,0 +1,216 @@ +// motor.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2003 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "motor.h" +#include "config/config.h" + +#include +#include + +// Pointeur vers l'instance unique. +Motor *Motor::instance_ = 0; + +// Constructeur. +Motor::Motor (void) + : asserv_ (*this), + s_ (1.0), vs_ (1.0), as_ (1.0), + movement_ (0), maxSpeed_ (0x40), minSpeed_ (0x04) +{ + // Mémorise le pointeur d'instance. + instance_ = this; + // Lit la conf. + Config rc ("rc/motor"); + while (!rc.eof ()) + { + if (rc.isId ("scale")) + { + rc.getId (); + rc >> s_; + } + else if (rc.isId ("speedscale")) + { + rc.getId (); + rc >> vs_; + } + else if (rc.isId ("accelscale")) + { + rc.getId (); + rc >> as_; + } + else if (rc.isId ("maxspeed")) + { + rc.getId (); + rc >> maxSpeed_; + } + else if (rc.isId ("minspeed")) + { + rc.getId (); + rc >> minSpeed_; + } + else rc.noId (); + } + asserv_.reset (); +} + +// Destructeur. +Motor::~Motor (void) +{ + instance_ = 0; + delete movement_; +} + +// Appelée lors d'une mise à jour des compteurs. +void +Motor::updateCounter (int l, int r) +{ + double dL = l * s_; + double dR = r * s_; + tracker_.update (dL, dR); + if (movement_) + if (!movement_->control ()) + { + delete movement_; + movement_ = 0; + asserv_.stop (); + } +} + +// Choisi le mouvement en cours. +void +Motor::setMovement (Movement *m) +{ + delete movement_; + movement_ = m; + m->init (tracker_, asserv_, *this); +} + +// Teste si les mouvements sont terminés. +bool +Motor::end (void) +{ + ok (); + return !movement_; +} + +// Attend la fin de tout les mouvements. +void +Motor::waitEnd (void) +{ + do { + waitOk (); + } while (!end ()); +} + +// Teste si les moteurs sont arrétés. +bool +Motor::stopped (void) +{ + ok (); + return tracker_.stopped (); +} + +// Attend que les moteurs soient arrétés. +void +Motor::waitStopped (void) +{ + do { + waitOk (); + } while (!stopped ()); +} + +// Paramètre la vitesse des moteurs. +void +Motor::speed (double l, double r) +{ + int vL = (int) (l / vs_); + int vR = (int) (r / vs_); + // Teste le dépassement de vitesse minimale. + while ((l != 0.0 || r != 0.0) && vL > -minSpeed_ && vL < minSpeed_ && vR > + -minSpeed_ && vR < minSpeed_) + { + l *= 1.5; + r *= 1.5; + vL = (int) (l / vs_); + vR = (int) (r / vs_); + } + // Teste le dépassement de vitesse maximal. + if (vL > maxSpeed_) + { + vL = maxSpeed_; + vR = (int) (r / l * vL); + } + else if (vL < -maxSpeed_) + { + vL = -maxSpeed_; + vR = (int) (r / l * vL); + } + if (vR > maxSpeed_) + { + vR = maxSpeed_; + vL = (int) (l / r * vR); + } + else if (vR < -maxSpeed_) + { + vR = -maxSpeed_; + vL = (int) (l / r * vR); + } + // Envois la commande. +std::cout << "motor speed " << l << ' ' << r << ' ' << vL << ' ' << vR << std::endl;; + asserv_.speed (vL, vR); +} + +// Va vers un point, renvois faux si atteind. +bool +Motor::goTo (double x, double y, double eps/*5.0*/) +{ + return false; +} + +// Va vers un point, en formant un arc de cercle, renvois faux si atteind. +bool +Motor::goToArc (double x, double y, double eps/*5.0*/) +{ + double l, r; + if (tracker_.computeArcs (x, y, l, r, eps)) + { +std::cout << "goto arc " << l << ' ' << r; + if (l > 0.0) + l = sqrt (2.0 * asserv_.getAccel () * as_ * l); + else + l = -sqrt (2.0 * asserv_.getAccel () * as_ * -l); + if (r > 0.0) + r = sqrt (2.0 * asserv_.getAccel () * as_ * r); + else + r = -sqrt (2.0 * asserv_.getAccel () * as_ * -r); +std::cout << ' ' << l << ' ' << r << std::endl;; + speed (l, r); + return true; + } + else + { + asserv_.stop (); + return false; + } +} + diff --git a/2004/i/nono/src/motor/motor.h b/2004/i/nono/src/motor/motor.h new file mode 100644 index 0000000..ddc32ee --- /dev/null +++ b/2004/i/nono/src/motor/motor.h @@ -0,0 +1,80 @@ +#ifndef motor_h +#define motor_h +// motor.h +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2003 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "asserv.h" +#include "tracker.h" +#include "movement.h" + +class Motor : public AsservTracker +{ + // Object d'asservissement. + Asserv asserv_; + // Suiveur de position. + Tracker tracker_; + // Pointeur vers l'instance unique. + static Motor *instance_; + // Echelles (mm/unit, mm/s.unit, mm/s^2.unit). + double s_, vs_, as_; + // Mouvement en cours. + Movement *movement_; + // Vitesses maximale et minimale (unit). + int maxSpeed_, minSpeed_; + public: + // Constructeur. + Motor (void); + // Destructeur. + virtual ~Motor (void); + // Récupère l'instance unique. + static Motor &getInstance (void) { return *instance_; } + // Appelée lors d'une mise à jour des compteurs. + void updateCounter (int l, int r); + // Choisi le mouvement en cours. + void setMovement (Movement *m); + // Active l'asservissement. + void go (bool fl = true) { asserv_.go (fl); } + // Teste si l'émission est terminée. + bool ok (void) { return asserv_.ok (); } + // Attend que toute les émissions soit terminées. + void waitOk (void) { asserv_.waitOk (); } + // Teste si les mouvements sont terminés. + bool end (void); + // Attend la fin de tout les mouvements. + void waitEnd (void); + // Teste si les moteurs sont arrétés. + bool stopped (void); + // Attend que les moteurs soient arrétés. + void waitStopped (void); + // Récupère l'object tracker. + const Tracker &getTracker (void) const { return tracker_; } + // Paramètre la vitesse des moteurs. + void speed (double l, double r); + // Va vers un point, renvois faux si atteind. + bool goTo (double x, double y, double eps = 5.0); + // Va vers un point, en formant un arc de cercle, renvois faux si atteind. + bool goToArc (double x, double y, double eps = 5.0); +}; + +#endif // motor_h diff --git a/2004/i/nono/src/motor/movement.h b/2004/i/nono/src/motor/movement.h new file mode 100644 index 0000000..5a3deaf --- /dev/null +++ b/2004/i/nono/src/motor/movement.h @@ -0,0 +1,51 @@ +#ifndef movement_h +#define movement_h +// movement.h +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#include + +class Tracker; +class Asserv; +class Motor; + +class Movement +{ + protected: + const Tracker *t_; + Asserv *a_; + Motor *m_; + public: + // Destructeur. + virtual ~Movement (void) { } + // Initialise le mouvement, appelé juste quand le mouvement est mis en + // service. + virtual void init (const Tracker &t, Asserv &a, Motor &m) + { t_ = &t; a_ = &a; m_ = &m; } + // Controlle la vitesse, retourne faux si mouvement terminé. + virtual bool control (void) { return false; } +}; + +#endif // movement_h diff --git a/2004/i/nono/src/motor/movement_basic.cc b/2004/i/nono/src/motor/movement_basic.cc new file mode 100644 index 0000000..2b1cb6e --- /dev/null +++ b/2004/i/nono/src/motor/movement_basic.cc @@ -0,0 +1,61 @@ +// movement_basic.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "movement_basic.h" +#include "tracker.h" +#include "asserv.h" +#include "motor.h" + +// Constructeur. +MovementBasic::MovementBasic (double speed, double lenght) + : speed_ (speed), lenght2_ (lenght * lenght), + sX_ (0.0), sY_ (0.0) +{ +} + +// Initialise le mouvement, appelé juste quand le mouvement est mis en +// service. +void +MovementBasic::init (const Tracker &t, Asserv &a, Motor &m) +{ + Movement::init (t, a, m); + sX_ = t.getX (); + sY_ = t.getY (); +} + +// Controlle la vitesse, retourne faux si mouvement terminé. +bool +MovementBasic::control (void) +{ + double dX = t_->getX () - sX_; + double dY = t_->getY () - sY_; + if (dX * dX + dY * dY >= lenght2_) + { + a_->stop (); + return false; + } + m_->speed (speed_, speed_); + return true; +} + diff --git a/2004/i/nono/src/motor/movement_basic.h b/2004/i/nono/src/motor/movement_basic.h new file mode 100644 index 0000000..4c9a899 --- /dev/null +++ b/2004/i/nono/src/motor/movement_basic.h @@ -0,0 +1,47 @@ +#ifndef movement_basic_h +#define movement_basic_h +// movement_basic.h +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "movement.h" + +class MovementBasic : public Movement +{ + // Vitesse. + double speed_; + // Distance au carré. + double lenght2_; + // Position de départ. + double sX_, sY_; + public: + // Constructeur. + MovementBasic (double speed, double lenght); + // Initialise le mouvement, appelé juste quand le mouvement est mis en + // service. + void init (const Tracker &t, Asserv &a, Motor &m); + // Controlle la vitesse, retourne faux si mouvement terminé. + bool control (void); +}; + +#endif // movement_basic_h diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc new file mode 100644 index 0000000..69c1b91 --- /dev/null +++ b/2004/i/nono/src/motor/movement_goto.cc @@ -0,0 +1,68 @@ +// movement_goto.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "movement_goto.h" +#include "tracker.h" +#include "motor.h" + +// Constructeur. +MovementGoTo::MovementGoTo (double x, double y, double eps, double dist, + double kpl, double kpa, double kil, double kia, + double kdl, double kda) + : dX_ (x), dY_ (y), eps_ (eps), dist_ (dist), + kpl_ (kpl), kpa_ (kpa), kil_ (kil), kia_ (kia), kdl_ (kdl), kda_ (kda), + il_ (0.0), ia_ (0.0), lel_ (0.0), lea_ (0.0) +{ +} + +// Controlle la vitesse, retourne faux si mouvement terminé. +bool +MovementGoTo::control (void) +{ + // Calcule l'erreur. + double el, ea; + if (!t_->computeError (dX_, dY_, el, ea, eps_, dist_)) + { + return false; + } + // Pas de marche arrière. + if (el < 0.0) el = 0.0; +std::cout << "movement goto error " << el << ' ' << ea << std::endl; + // Calcule les intégrales saturées. + il_ += el; + if (il_ > 1000) il_ = 1000; + else if (il_ < -1000) il_ = -1000; + ia_ += ea; + if (ia_ > 1000) ia_ = 1000; + else if (ia_ < -1000) ia_ = -1000; + // Commande les moteurs. + double l = kpl_ * (el + kil_ * il_ + kdl_ * (el - lel_)); + double a = kpa_ * (ea + kia_ * ia_ + kda_ * (ea - lea_)); + m_->speed (l - a, l + a); + // Retiens l'erreur pour la dérivée. + lel_ = el; + lea_ = ea; + return true; +} + diff --git a/2004/i/nono/src/motor/movement_goto.h b/2004/i/nono/src/motor/movement_goto.h new file mode 100644 index 0000000..dbd4e67 --- /dev/null +++ b/2004/i/nono/src/motor/movement_goto.h @@ -0,0 +1,55 @@ +#ifndef movement_goto_h +#define movement_goto_h +// movement_goto.h +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "movement.h" + +class MovementGoTo : public Movement +{ + // Position d'arrivée. + double dX_, dY_; + // Epsilon. + double eps_; + // Distance du point cible courant. + double dist_; + // Coefficient proportionnel linéaire et angulaire. + double kpl_, kpa_; + // Coefficient intégrals linéaire et angulaire. + double kil_, kia_; + // Coefficient de dérivée linéaire et angulaire. + double kdl_, kda_; + // Integrales. + double il_, ia_; + // Dernière valeurs d'erreur. + double lel_, lea_; + public: + // Constructeur. + MovementGoTo (double x, double y, double eps, double dist, double kpl, + double kpa, double kil, double kia, double kdl, double kda); + // Controlle la vitesse, retourne faux si mouvement terminé. + bool control (void); +}; + +#endif // movement_goto_h diff --git a/2004/i/nono/src/motor/movement_types.h b/2004/i/nono/src/motor/movement_types.h new file mode 100644 index 0000000..4872d89 --- /dev/null +++ b/2004/i/nono/src/motor/movement_types.h @@ -0,0 +1,30 @@ +#ifndef movement_types_h +#define movement_types_h +// movement_types.h +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "movement_basic.h" +#include "movement_goto.h" + +#endif // movement_types_h diff --git a/2004/i/nono/src/motor/test_asserv.cc b/2004/i/nono/src/motor/test_asserv.cc new file mode 100644 index 0000000..7ac096c --- /dev/null +++ b/2004/i/nono/src/motor/test_asserv.cc @@ -0,0 +1,113 @@ +// test_asserv.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "asserv.h" +#include "erreur/erreur.h" +#include "date/date.h" + +#include +#include +#include + +using namespace std; + +class AsservTrackerC : public AsservTracker +{ + int s; + public: + // Appelée lors d'une mise à jour des compteurs. + virtual void updateCounter (int l, int r) { s += l; } + // Affiche. + void aff (void) + { std::cout << s << std::endl; } + // Reset. + void reset (void) + { s = 0; } +}; + +int +main (int argc, char **argv) +{ + try + { + AsservTrackerC at; + Asserv m (at); + int i = 1; + while (i < argc) + { + switch (argv[i][0]) + { + case 's': + at.reset (); + cout << "stop" << endl; + i++; + m.stop (); + break; + case 'z': + cout << "rezet" << endl; + i++; + m.reset (); + break; + case 'g': + cout << "go" << endl; + i++; + m.go (); + break; + case 'G': + cout << "ungo" << endl; + i++; + m.go (false); + break; + case 'v': + i++; + int vg, vd; + if (i >= argc) break; + vg = atoi (argv[i++]); + if (i >= argc) break; + vd = atoi (argv[i++]); + cout << "speed " << vg << ' ' << vd << endl; + m.speed (vg, vd); + break; + default: + int s; + s = atoi (argv[i++]); + cout << "sleep " << s << endl; + for (int j = 0; j < s * 1000 / 50; ++j) + { + m.ok (); + Date::wait (1); + } + break; + } + m.waitOk (); + } + at.aff (); + return 0; + } + catch (Erreur &e) + { + cerr << e.what () << endl; + return 1; + } +} diff --git a/2004/i/nono/src/motor/test_motor.cc b/2004/i/nono/src/motor/test_motor.cc new file mode 100644 index 0000000..325975e --- /dev/null +++ b/2004/i/nono/src/motor/test_motor.cc @@ -0,0 +1,113 @@ +// test_motor.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "motor.h" +#include "erreur/erreur.h" +#include "date/date.h" +#include "movement_types.h" + +#include +#include +#include +#include + +using namespace std; + +int +main (int argc, char **argv) +{ + try + { + Motor m; + int i = 1; + while (i < argc) + { + switch (argv[i][0]) + { + case 'g': + cout << "go" << endl; + i++; + m.go (); + break; + case 'G': + cout << "ungo" << endl; + i++; + m.go (false); + break; + case 'w': + cout << "wait stop" << endl; + i++; + m.waitStopped (); + break; + case 'b': + { + i++; + double speed; + double len; + if (i >= argc) break; + speed = atof (argv[i++]); + if (i >= argc) break; + len = atof (argv[i++]); + cout << "basic " << speed << ' ' << len << endl; + Movement *mov = new MovementBasic (speed, len); + m.setMovement (mov); + } + break; + case 't': + { + i++; + double dX, dY; + if (i >= argc) break; + dX = atof (argv[i++]); + if (i >= argc) break; + dY = atof (argv[i++]); + cout << "goto " << dX << ' ' << dY << endl; + Movement *mov = new MovementGoTo (dX, dY, 10.0, 0.0, + 1.0, 1.5, + 0.1, 0.1, + 0.7, 0.7); + m.setMovement (mov); + } + break; + default: + int s; + s = atoi (argv[i++]); + cout << "sleep " << s << endl; + for (int j = 0; j < s * 1000 / 50; ++j) + { + m.ok (); + Date::wait (50); + } + break; + } + m.waitEnd (); + } + return 0; + } + catch (Erreur &e) + { + cerr << e.what () << endl; + return 1; + } +} diff --git a/2004/i/nono/src/motor/test_tracker.cc b/2004/i/nono/src/motor/test_tracker.cc new file mode 100644 index 0000000..197156e --- /dev/null +++ b/2004/i/nono/src/motor/test_tracker.cc @@ -0,0 +1,82 @@ +// test_tracker.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "tracker.h" +#include "erreur/erreur.h" + +#include +#include + +int +main (int argc, char **argv) +{ + try + { + Tracker t; + if (argc < 2) return 1; + switch (argv[1][0]) + { + case 'e': + // Teste le calcul d'erreur. + for (int i = 2; i < argc - 1; i += 2) + { + double x = atof (argv[i]); + double y = atof (argv[i + 1]); + double el, ea; + t.computeError (x, y, el, ea, 50.0, 10.0); + std::cout << "error " << x << ' ' << y << ' ' << el << ' ' << + ea << std::endl; + } + break; + case 'a': + // Teste le calcul d'arcs. + for (int i = 2; i < argc - 1; i += 2) + { + double x = atof (argv[i]); + double y = atof (argv[i + 1]); + double l, r; + t.computeArcs (x, y, l, r, 50.0); + std::cout << "arc " << x << ' ' << y << ' ' << l << ' ' << r + << std::endl; + } + break; + default: + // Teste les updates. + for (int i = 1; i < argc - 1; i += 2) + { + double l = atof (argv[i]); + double r = atof (argv[i + 1]); + t.update (l, r); + std::cout << "update " << l << ' ' << r << " pos " << t << + std::endl; + } + } + return 0; + } + catch (Erreur &e) + { + cerr << e.what () << endl; + return 1; + } +} diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc new file mode 100644 index 0000000..4229864 --- /dev/null +++ b/2004/i/nono/src/motor/tracker.cc @@ -0,0 +1,230 @@ +// tracker.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "tracker.h" +#include "config/config.h" + +#include +#include + +// Constructeur. +Tracker::Tracker (void) + : posX_ (0.0), posY_ (0.0), angle_ (0.0), + f_ (10.0), zero_ (0) +{ + // Lit la conf. + Config rc ("rc/tracker"); + while (!rc.eof ()) + { + if (rc.isId ("startx")) + { + rc.getId (); + rc >> posX_; + } + else if (rc.isId ("starty")) + { + rc.getId (); + rc >> posY_; + } + else if (rc.isId ("startangle")) + { + rc.getId (); + rc >> angle_; + } + else if (rc.isId ("footing")) + { + rc.getId (); + rc >> f_; + } + else rc.noId (); + } +} + +// Destructeur. +Tracker::~Tracker (void) +{ +} + +// Set the position. +void +Tracker::setPos (double x, double y, double angle) +{ + posX_ = x; + posY_ = y; + angle_ = angle; +} + +// Get the position. +void +Tracker::getPos (double &x, double &y, double &angle) const +{ + x = posX_; + y = posY_; + angle = angle_; +} + +// Calcule l'erreur linéaire et angulaire pour atteindre un point, +// retourne faux si atteind. +bool +Tracker::computeError (double x, double y, double &el, double &ea, double eps, + double dist) const +{ + // Calcule le point cible courant. + double aX = posX_ + dist * cos (angle_); + double aY = posY_ + dist * sin (angle_); + // Calcule la diférence avec la cible voulue. + double eX = x - aX; + double eY = y - aY; + // Vérifie l'eps. + if (eX * eX + eY * eY < eps * eps) + { + el = 0; + ea = 0; + return false; + } + // Calcule l'erreur linéaire (projection sur la direction courante). + el = eX * cos (angle_) + eY * sin (angle_); + // Calcule l'erreur angulaire (projection sur la direction + // perpendiculaire). + ea = eX * -sin (angle_) + eY * cos (angle_); + return true; +} + +// Calcule la longueur de l'arc gauche et droite pour atteindre un point. +// EPS précise la distance minimale. Renvois faux si atteind. +bool +Tracker::computeArcs (double x, double y, double &l, double &r, double eps) + const +{ + // Vecteur vers le point à atteindre. + double dX = x - posX_, dY = y - posY_; + // Calcule la distance au point (x, y). + double d = sqrt (dX * dX + dY * dY); + if (d < eps) + { + l = r = 0.0; +std::cout << "compute arc eps" << std::endl; + return false; + } + // Calcule l'angle entre la direction courante et la direction du point à + // atteindre, évite d'utiliser un atan. + // Calcule le vecteur perpendiculaire au vecteur normal selon la + // direction courante. + double vX = -sin (angle_); + double vY = cos (angle_); + // Calcul de l'angle devant/deriere. + if (dX * vY + dY * -vX < 0.0) + { + l = r = 0; +std::cout << "compute arc back" << std::endl; + return false; + } + // Calcul de l'angle par produit scalaire. + double s = dX * vX + dY * vY; + double dA = asin (s / d); // sin (a) = cos (a + pi/2) +std::cout << "compute arc dA " << dA << " d " << d << std::endl; + // Si l'angle est trops petit, on va tout droit, sinon /0. + if (dA > -0.0001 && dA < 0.0001) + { + l = r = d; +std::cout << "compute arc small angle" << std::endl; + } + // Si l'angle est trops grand, on ne fait pas le tour du monde. + else if (dA > M_PI_2 || dA < -M_PI_2) + { + l = r = 0; +std::cout << "compute arc big angle" << std::endl; + return false; + } + else + { + // Calcule le rayon de courbure à suivre. + double ro = 0.5 * d / sin (dA); // sin (dA) = cos (pi/2 - dA) + // Rayon plus petit que l'empatement/2, stop. + if (ro < 0.25 * f_ && ro > 0.25 * f_) + { + l = r = 0.0; +std::cout << "compute arc small ro " << ro << std::endl; + return false; + } + // L'angle du déplacement est le même que dA. + l = (ro - 0.5 * f_) * dA; + r = (ro + 0.5 * f_) * dA; +std::cout << "compute arc ro " << ro << std::endl; + } + return true; +} + +// Met à jour la position. +void +Tracker::update (double dL, double dR) +{ + // Compte les zeros. + if (dL == 0.0 && dR == 0.0) + { + zero_++; + return; + } + else + { + zero_ = 0; + } + // Calcule l'angle et l'avancement moyen. + // Avec a petit (c'est le cas, car f_ >> abs (dR - dL)), a ~= atan (a). + double dA = (dR - dL) / f_; + double dS = 0.5 * (dL + dR); +//std::cout << "tracker update dL " << dL << " dR " << dR << " dA " << dA << " dS " << dS << std::endl; + // Si l'angle est petit, évite une division par presque 0. + if (dA < 0.0001 && dA > -0.0001) + { + // Considère que l'on avance en ligne droite selon la moitié de + // l'angle. + double a = angle_ + 0.5 * dA; + posX_ += dS * cos (a); + posY_ += dS * sin (a); + angle_ += dA; +//std::cout << "tracker small dX " << dS * cos (a) << " dY " << dS * sin (a) << std::endl; + } + else + { + // dS : arc de cercle parcouru (grâce à Thales). + // R : rayon, dS = dA * R, R = dS / dA + // On a plus qu'à calculer les dX, dY avec cos et sin. + double oldA = angle_; + angle_ += dA; + posX_ += (sin (angle_) - sin (oldA)) * dS / dA; + posY_ += (cos (oldA) - cos (angle_)) * dS / dA; +//std::cout << "tracker big dX " << -(sin (oldA) - sin (angle_)) * dS / dA << " dY " << -(cos (oldA) - cos (angle_)) * dS / dA << " R " << dS / dA << std::endl; + } +std::cout << "update pos " << *this << std::endl; +} + +// Affiche la position. +std::ostream & +operator<< (std::ostream &os, const Tracker &t) +{ + os << t.getX () << ' ' << t.getY () << ' ' << t.getAngle (); + return os; +} + diff --git a/2004/i/nono/src/motor/tracker.h b/2004/i/nono/src/motor/tracker.h new file mode 100644 index 0000000..5271c08 --- /dev/null +++ b/2004/i/nono/src/motor/tracker.h @@ -0,0 +1,66 @@ +#ifndef tracker_h +#define tracker_h +// tracker.h +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#include + +class Tracker +{ + // Position et angle (mm et rad). + double posX_, posY_, angle_; + // Distance entre les deux roues (mm). + double f_; + // Nombre d'updates nuls consécutifs. + int zero_; + public: + // Constructeur. + Tracker (void); + // Destructeur. + ~Tracker (void); + // Set the position. + void setPos (double x, double y, double angle); + // Get the position. + void getPos (double &x, double &y, double &angle) const; + double getX (void) const { return posX_; } + double getY (void) const { return posY_; } + double getAngle (void) const { return angle_; } + bool stopped (void) { return zero_ > 10; } + // Calcule l'erreur linéaire et angulaire pour atteindre un point, + // retourne faux si atteind. + bool computeError (double x, double y, double &el, double &ea, double eps, + double dist) const; + // Calcule la longueur de l'arc gauche et droite pour atteindre un point. + // EPS précise la distance minimale. Renvois faux si atteind. + bool computeArcs (double x, double y, double &l, double &r, double eps) + const; + // Met à jour la position. + void update (double dL, double dR); +}; + +// Affiche la position. +std::ostream &operator<< (std::ostream &os, const Tracker &t); + +#endif // tracker_h diff --git a/2004/i/nono/src/serial/Makefile.defs b/2004/i/nono/src/serial/Makefile.defs new file mode 100644 index 0000000..ddafe5c --- /dev/null +++ b/2004/i/nono/src/serial/Makefile.defs @@ -0,0 +1,8 @@ +LIBS += serial.a +TARGETS += test_serial +test_serial_SOURCES = test_serial.cc serial.a erreur.a +serial_a_SOURCES = serial.cc + +serial.a: ${serial_a_SOURCES:%.cc=serial.a(%.o)} + +test_serial: $(test_serial_SOURCES:%.cc=%.o) diff --git a/2004/i/nono/src/serial/serial.cc b/2004/i/nono/src/serial/serial.cc new file mode 100644 index 0000000..ab047ce --- /dev/null +++ b/2004/i/nono/src/serial/serial.cc @@ -0,0 +1,97 @@ +// serial.cc - Contrôle du port serie. +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "serial.h" +#include "erreur/erreur.h" + +#include + +// Constructeur. +Serial::Serial (void) + : fd_ (-1) +{ +} + +// Destructeur. +Serial::~Serial (void) +{ + close (); +} + +// Ouvre et paramètre le port série. +void +Serial::open (const char *ttyname) +{ + struct termios tios; + // Ouvre le port série. + fd_ = ::open (ttyname, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (fd_ < 0) + throw ErreurFatale ("Erreur d'ouverture du port série.\n"); + // Rendre le port série asynchrone. + //fcntl (fd_, F_SETFL, FASYNC); + // Sauver la configuration courante du port série. + tcgetattr (fd_, &old_); + // Paramètrer les options du port série. + tios.c_cflag = CS8 | CLOCAL | CREAD /*| BAUDRATE*/; + tios.c_iflag = IGNBRK | IGNPAR; + tios.c_oflag = 0; + tios.c_lflag = 0; + // On n'attend jamais. + tios.c_cc[VMIN] = 0; + tios.c_cc[VTIME] = 0; + //cfsetspeed (&tios, B115200); + //cfsetspeed (&tios, B9600); + cfsetspeed (&tios, B19200); + // Vider et configuer le port série. + tcflush (fd_, TCIFLUSH); + tcsetattr (fd_, TCSANOW, &tios); +} + +// Ferme le port série, appellé dans le destructeur. +void +Serial::close (void) +{ + tcsetattr (fd_, TCSANOW, &old_); + ::close (fd_); + fd_ = -1; +} + +// Lit un caractère, ou -1. +int +Serial::getchar (void) +{ + char c; + if (read (fd_, &c, 1) == 1) + return c; + else + return -1; +} + +// Ecrit un bloc. +ssize_t +Serial::write (const void *buf, size_t size) +{ + return ::write (fd_, buf, size); +} + diff --git a/2004/i/nono/src/serial/serial.h b/2004/i/nono/src/serial/serial.h new file mode 100644 index 0000000..4c91b55 --- /dev/null +++ b/2004/i/nono/src/serial/serial.h @@ -0,0 +1,50 @@ +#ifndef serial_h +#define serial_h +// serial.h - Contrôle du port serie. +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#include +#include + +class Serial +{ + struct termios old_; + int fd_; + public: + // Constructeur. + Serial (void); + // Destructeur. + ~Serial (void); + // Ouvre et paramètre le port série. + void open (const char *ttyname); + // Ferme le port série, appellé dans le destructeur. + void close (void); + // Lit un caractère, ou -1. + int getchar (void); + // Ecrit un bloc. + ssize_t write (const void *buf, size_t size); +}; + +#endif // serial_h diff --git a/2004/i/nono/src/serial/test_serial.cc b/2004/i/nono/src/serial/test_serial.cc new file mode 100644 index 0000000..7404997 --- /dev/null +++ b/2004/i/nono/src/serial/test_serial.cc @@ -0,0 +1,48 @@ +// test_serial.cc - Programme de test pour le port série. +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "serial.h" +#include "erreur/erreur.h" + +#include + +int +main (void) +{ + try + { + Serial s; + s.open ("/dev/tty00"); + s.write ("!g\r", 3); + sleep (1); + s.write ("!v0303\r", 7); + sleep (5); + s.write ("!s\r", 3); + } + catch (Erreur &e) + { + cerr << e.what (); + return 1; + } +} diff --git a/2004/i/nono/src/vision/Makefile.defs b/2004/i/nono/src/vision/Makefile.defs new file mode 100644 index 0000000..e13c735 --- /dev/null +++ b/2004/i/nono/src/vision/Makefile.defs @@ -0,0 +1,11 @@ +TARGETS += test_image test_vision +LIBS += vision.a +test_vision_SOURCES = test_vision.cc vision.a camera.a config.a date.a erreur.a $(LIBPPM) +test_image_SOURCES = test_image.cc vision.a camera.a config.a date.a erreur.a $(LIBPPM) +vision_a_SOURCES = image.cc thresholds.cc + +test_image: $(test_image_SOURCES:%.cc=%.o) + +test_vision: $(test_vision_SOURCES:%.cc=%.o) + +vision.a: ${vision_a_SOURCES:%.cc=vision.a(%.o)} diff --git a/2004/i/nono/src/vision/image.cc b/2004/i/nono/src/vision/image.cc new file mode 100644 index 0000000..f91e9e6 --- /dev/null +++ b/2004/i/nono/src/vision/image.cc @@ -0,0 +1,344 @@ +// image.cc - Classe Image +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet +#include "image.h" + +extern "C" { +#include +}; + +// Attention, l'un des paramètres est evalué deux fois. +#define min(a,b) ((a) < (b) ? (a) : (b)) +#define max(a,b) ((a) > (b) ? (a) : (b)) + +// Constructeurs. +Image::Image (const char *filename, const Thresholds *thresholds) +{ + pixel **img; + pixel *p; + pixval maxval; + int x, y, w, h; + FILE *fp; + unsigned char *pi; + // Open the file. + fp = fopen (filename, "r"); + if (!fp) throw "Image::Image: fopen failled"; + // Read it. + img = ppm_readppm (fp, &w, &h, &maxval); + // Close it. + fclose (fp); + if (!img) throw "Image::Image: ppm_readppm failled"; + // Allocate memory... + m_image = new unsigned char [w*h*4]; + m_width = w; + m_height = h; + // Extrait les info RGB ou YUV. + pi = m_image; + for (y = 0; y < h; y++) + { + p = img[y]; + for (x = 0; x < w; x++) + { + *pi++ = (unsigned char) (PPM_GETB (*p) * 255 / maxval); + *pi++ = (unsigned char) (PPM_GETG (*p) * 255 / maxval); + *pi++ = (unsigned char) (PPM_GETR (*p) * 255 / maxval); + *pi++ = 0; + p++; + } + } + // Alloue de la memoire pour les zones. + m_zones = new unsigned char [m_width * m_height]; + // Initalisation + m_groups = 0; + m_thresholds = thresholds; + m_camera = 0; +} + +Image::Image (Camera *camera, const Thresholds *thresholds) +{ + // Copie la camera. + m_camera = camera; + // Allocate memory... + m_camera->getSize (m_width, m_height); + m_image = new unsigned char [m_width * m_height * 4]; + // Alloue de la memoire pour les zones. + m_zones = new unsigned char [m_width * m_height]; + // Initalisation + m_groups = 0; + m_thresholds = thresholds; +} + +// Destructeur +Image::~Image (void) +{ + ImageGroup *g, *g2; + if (m_image) delete [] m_image; + if (m_zones) delete [] m_zones; + for (g = m_groups; g; g = g2) + { + g2 = g->next; + delete g; + } +} + +// Filtre l'image pour trouver les zones. +void Image::filter (void) +{ + int i; + unsigned char *pz, *pi; + unsigned char r, g, b; + // Filtre. + pz = m_zones; + pi = m_image; + for (i = 0; i < m_width * m_height; i++) + { + b = *pi++; + g = *pi++; + r = *pi++; + pi++; + *pz++ = m_thresholds->findZone (r, g, b); + } +} + +// Group les groupes de pixels. +void Image::group (void) +{ + // c: group courant, n: group suivant (chercheur). + ImageGroup *c, **n, *n2; + // Supprime tous les groupes. + for (c = m_groups; c; c = n2) + { + n2 = c->next; + delete c; + } + // Fait des groupes de pixels. + groupLine (); + for (c = m_groups; c; c = c->next) + { + n = &c->next; + // Recherche des groups plus bas dans l'image qui sont colle au groupe + // courant. + while (*n && (*n)->y <= c->y + c->h) + { + // Si ils sont de la même zone et qu'ils se touchent (avec au + // moins 5 pixels), on les rassemble en 1 groupe. + if (c->zone == (*n)->zone + && (*n)->x < c->x + c->w - 5 + && c->x < (*n)->x + (*n)->w - 5) + { + // Dechainage de n. + n2 = (*n); + *n = n2->next; + // Mise a jour de c. + c->h++; + c->w = max (c->x + c->w, n2->x + n2->w) - min (c->x, n2->x); + c->x = min (c->x, n2->x); + // Supression de n. + delete n2; + } + else + n = &(*n)->next; + } + } +} + +// Fait des packets de pixels sur les lignes. +void Image::groupLine (void) +{ + unsigned char zone; + int n, y, x, sx; + ImageGroup **p; + unsigned char *pz; + pz = m_zones; + p = &m_groups; + for (y = 0; y < m_height; y++) + { + // Pour chaque ligne. + n = 0; + zone = 0; + for (x = 0; x < m_width; x++) + { + if (n == 0) + { + // Nouvelle zone. + sx = x; + zone = *pz; + n = 1; + } + else + { + if (zone == *pz) + { + // Même zone. + n++; + } + else + { + // Zone differente. + if (zone && n > 5) + { + *p = new ImageGroup; + (*p)->x = sx; + (*p)->y = y; + (*p)->w = n; + (*p)->h = 1; + (*p)->zone = zone; + (*p)->type = 0; + p = &(*p)->next; + } + n = 0; + zone = *pz; + } + } + pz++; + } + // Dernier groupe, si assez grand. + if (zone && n > 5) + { + *p = new ImageGroup; + (*p)->x = sx; + (*p)->y = y; + (*p)->w = n; + (*p)->h = 1; + (*p)->zone = zone; + (*p)->type = 0; + p = &(*p)->next; + } + } + *p = 0; +} + +// Affiche les groupes qui on été trouvés. +void +Image::dumpGroups (void) +{ + ImageGroup *g; + printf ("Groups\n"); + for (g = m_groups; g; g = g->next) + { + printf ("x: %d, y: %d, w: %d, h: %d, z: %d, t: %d\n", g->x, g->y, + g->w, g->h, g->zone, g->type); + } +} + +// Filtre les packets de pixels. +void +Image::groupFilter (void) +{ + ImageGroup **g, *g2; + g = &m_groups; + while (*g) + { + g2 = *g; + // Trouve le type de palet. + g2->type = 1; // TODO : trouver le type du palet. + // Vire les groupes qu'on ne voix pas en entier ou d'une taille bizare. + if (g2->type == 0 + //|| g2->x <= 0 || g2->x + g2->w >= m_width + //|| g2->y <= 0 || g2->y + g2->h >= m_height) + ) + { + *g = g2->next; + delete g2; + } + else + { + g = &g2->next; + } + } +} + +// Enregistre les infos trouvées. +void +Image::dump (const char *filename) +{ + pixel *row; + pixel *pr; + int x, y; + FILE *fp; + unsigned char *p; + int r, g, b; + ImageGroup *pg; + // Open the file. + fp = fopen (filename, "w"); + if (!fp) throw "Image::dump: fopen failled"; + // Allocate memory... + row = ppm_allocrow (m_width); + // Sauve l'image. + ppm_writeppminit (fp, m_width, m_height * 3, 255, 0); + // image. + p = m_image; + for (y = 0; y < m_height; y++) + { + pr = row; + for (x = 0; x < m_width; x++) + { + b = *p++; + g = *p++; + r = *p++; + p++; + PPM_ASSIGN (*pr, r, g, b); + pr++; + } + ppm_writeppmrow (fp, row, m_width, 255, 0); + } + // Sauve les zones. + p = m_zones; + for (y = 0; y < m_height; y++) + { + pr = row; + for (x = 0; x < m_width; x++) + { + r = *p == 1 ? 255 : 0; + g = *p == 2 ? 255 : 0; + b = 0; + p++; + PPM_ASSIGN (*pr, r, g, b); + pr++; + } + ppm_writeppmrow (fp, row, m_width, 255, 0); + } + // Sauve les groupes. + for (y = 0; y < m_height; y++) + { + pr = row; + for (x = 0; x < m_width; x++) + { + r = g = b = 0; + for (pg = m_groups; pg; pg = pg->next) + { + if (x >= pg->x && x < pg->x + pg->w + && y >= pg->y && y < pg->y + pg->h) + { + if (pg->zone == 1) + r = 255; + else if (pg->zone == 2) + g = 255; + else + b = 255; + } + } + PPM_ASSIGN (*pr, r, g, b); + pr++; + } + ppm_writeppmrow (fp, row, m_width, 255, 0); + } + // Free memory. + ppm_freerow (row); + // Close it. + fclose (fp); +} + +// Relit les données de la camera et fait les analyses. +bool +Image::reread (void) +{ + // Lit l'image. + if (m_camera->read (m_image) == 0) + return false; + filter (); + group (); + groupFilter (); + return true; +} + diff --git a/2004/i/nono/src/vision/image.h b/2004/i/nono/src/vision/image.h new file mode 100644 index 0000000..4177f81 --- /dev/null +++ b/2004/i/nono/src/vision/image.h @@ -0,0 +1,48 @@ +#ifndef image_h +#define image_h +// image.h - Image class +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet + +#include "thresholds.h" +#include "camera/camera.h" + +struct ImageGroup +{ + ImageGroup *next; + int x, y, w, h; + int zone, type; +}; + +class Image +{ + int m_width, m_height; + unsigned char *m_image; + unsigned char *m_zones; + ImageGroup *m_groups; + const Thresholds *m_thresholds; + Camera *m_camera; + public: + // Constructeurs. + Image (const char *filename, const Thresholds *thresholds); + Image (Camera *camera, const Thresholds *thresholds); + // Destructeur. + ~Image (void); + // Filtre l'image pour trouver les zones. + void filter (void); + // Fait des packets de pixels. + void group (void); + // Affiche les groupes qui on été trouvés. + void dumpGroups (void); + // Enregistre les infos trouvées. + void dump (const char *filename); + // Filtre les packets de pixels. + void groupFilter (void); + // Relit les données de la camera et fait les analyses. + bool reread (void); + protected: + // Fait des packets de pixels sur les lignes. + void groupLine (void); +}; + +#endif // image_h diff --git a/2004/i/nono/src/vision/rgbyuv.h b/2004/i/nono/src/vision/rgbyuv.h new file mode 100644 index 0000000..104c309 --- /dev/null +++ b/2004/i/nono/src/vision/rgbyuv.h @@ -0,0 +1,57 @@ +#ifndef rgbyuv_h +#define rgbyuv_h +// rgbyuv.h - Convertions RGB <-> YUV +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet + +// TODO: Implementer une version plus rapide avec des tableaux ??? +// Pas pour le momment, on utilise pas YUV. + +class RgbYuv +{ + public: + // Convertie un pixel RGB en YUV. + static void rgbToYuv (unsigned char r, unsigned char g, unsigned char b, + unsigned char &y, unsigned char &u, unsigned char + &v); + // Convertie un pixel YUV en RGB. + static void yuvToRgb (unsigned char y, unsigned char u, unsigned char v, + unsigned char &r, unsigned char &g, unsigned char + &b); +}; + +// Convertie un pixel RGB en YUV. +inline void RgbYuv::rgbToYuv (unsigned char r, unsigned char g, unsigned char + b, unsigned char &y, unsigned char &u, unsigned + char &v) +{ + y = (( 66 * r + 129 * g + 25 * b + 128) >> 8) + 16; + u = (( -38 * r - 74 * g + 112 * b + 128) >> 8) + 128; + v = (( 112 * r - 94 * g - 18 * b + 128) >> 8) + 128; +} + +// Convertie un pixel YUV en RGB. +inline void RgbYuv::yuvToRgb (unsigned char y, unsigned char u, unsigned char + v, unsigned char &r, unsigned char &g, unsigned + char &b) +{ + int c, d, e; + int _r, _g, _b; + c = (int) (y & 0xff) - 16; + d = (int) (u & 0xff) - 128; + e = (int) (v & 0xff) - 128; + _r = (298 * c + 409 * e + 128) >> 8; + _g = (298 * c - 100 * d - 208 * e + 128) >> 8; + _b = (298 * c + 516 * d + 128) >> 8; + if (_r > 255) _r = 255; + if (_g > 255) _g = 255; + if (_b > 255) _b = 255; + if (_r < 0) _r = 0; + if (_g < 0) _g = 0; + if (_b < 0) _b = 0; + r = _r; + g = _g; + b = _b; +} + +#endif // rgbyuv_h diff --git a/2004/i/nono/src/vision/test_image.cc b/2004/i/nono/src/vision/test_image.cc new file mode 100644 index 0000000..006c764 --- /dev/null +++ b/2004/i/nono/src/vision/test_image.cc @@ -0,0 +1,34 @@ +// test_image.cc - Teste la classe Image. +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet +#include "image.h" + +extern "C" { +#include +}; + +int +main (int argc, char **argv) +{ + Image *i; + Thresholds *t; + ppm_init (&argc, argv); + try + { + t = new Thresholds ("rc/vision/palets.rgb"); + i = new Image (argc == 2 ? argv[1] : "test.ppm", t); + i->filter (); + i->group (); + i->groupFilter (); + i->dumpGroups (); + i->dump ("dump.ppm"); + delete i; + delete t; + } + catch (const char *s) + { + fprintf (stderr, "test_image: %s\n", s); + return 1; + } + return 0; +} diff --git a/2004/i/nono/src/vision/test_vision.cc b/2004/i/nono/src/vision/test_vision.cc new file mode 100644 index 0000000..9792910 --- /dev/null +++ b/2004/i/nono/src/vision/test_vision.cc @@ -0,0 +1,48 @@ +// test_vision.cc +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet +// +#include "image.h" +#include "date/date.h" + +#include +extern "C" { +#include +}; + +int +main (int argc, char **argv) +{ + Date d; + Image *i; + Camera *c; + Thresholds *t; + ppm_init (&argc, argv); + try + { + t = new Thresholds ("rc/vision/palets.rgb"); + c = new Camera (); + i = new Image (c, t); + for (int j = 0; j < 5; ++j) + { + if (i->reread ()) + { + char s[256]; + i->dumpGroups (); + sprintf (s, "dump%d.ppm", j); + i->dump (s); + } + sleep (1); + } + delete i; + delete c; + delete t; + } + catch (const char *s) + { + fprintf (stderr, "test_vision: %s\n", s); + return 1; + } + return 0; +} + diff --git a/2004/i/nono/src/vision/thresholds.cc b/2004/i/nono/src/vision/thresholds.cc new file mode 100644 index 0000000..23c37e5 --- /dev/null +++ b/2004/i/nono/src/vision/thresholds.cc @@ -0,0 +1,55 @@ +// Classe Thresholds - Chargement de seuils +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet +#include "thresholds.h" + +#include + +// Constructeur. +Thresholds::Thresholds (const char *filename) +{ + FILE *fp; + Threshold **pt; + int n; + int zone, ym, yM, um, uM, vm, vM; + // Ouvre le fichier. + fp = fopen (filename, "r"); + if (!fp) throw "Thresholds::Thresholds: fopen failled"; + // Charge chaque lignes. + pt = &m_thresholds; + while (!feof (fp)) + { + n = fscanf (fp, "%d %d %d %d %d %d %d\n", &zone, &ym, &yM, &um, + &uM, &vm, &vM); + if (n != 7) + { + fclose (fp); + throw "Thresholds::Thresholds: Bad format."; + } + // Ajoute un nouveaux seuil a la collection. + *pt = new Threshold; + (*pt)->zone = zone; + (*pt)->ym = ym; + (*pt)->yM = yM; + (*pt)->um = um; + (*pt)->uM = uM; + (*pt)->vm = vm; + (*pt)->vM = vM; + pt = &(*pt)->next; + } + // Ferme la liste de seuils. + *pt = 0; + // Ferme le fichier. + fclose (fp); +} + +// Destructeur. +Thresholds::~Thresholds (void) +{ + Threshold *p, *p2; + for (p = m_thresholds; p; p = p2) + { + p2 = p->next; + delete p; + } +} diff --git a/2004/i/nono/src/vision/thresholds.h b/2004/i/nono/src/vision/thresholds.h new file mode 100644 index 0000000..42d7313 --- /dev/null +++ b/2004/i/nono/src/vision/thresholds.h @@ -0,0 +1,42 @@ +#ifndef thresholds_h +#define thresholds_h +// thresholds.h - Chargement de seuils. +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet + +struct Threshold +{ + Threshold *next; + unsigned char ym, yM, um, uM, vm, vM; + unsigned char zone; +}; + +class Thresholds +{ + Threshold *m_thresholds; + public: + Thresholds (const char *filename); + ~Thresholds (void); + // Trouve la zone qui correspond aux composantes. + unsigned char findZone (unsigned char y, unsigned char u, unsigned char v) + const; +}; + +// Trouve la zone qui correspond aux composantes. +inline unsigned char +Thresholds::findZone (unsigned char y, unsigned char u, unsigned char v) const +{ + Threshold *t; + for (t = m_thresholds; t; t = t->next) + { + if (y >= t->ym && y <= t->yM + && u >= t->um && u <= t->uM + && v >= t->vm && v <= t->vM) + { + return t->zone; + } + } + return 0; +} + +#endif // thresholds_h -- cgit v1.2.3