summaryrefslogtreecommitdiff
path: root/2004/i/nono
diff options
context:
space:
mode:
authorschodet2004-02-07 16:55:41 +0000
committerschodet2004-02-07 16:55:41 +0000
commit9fdd6704947174fae292dfc6e14cc1029a7f7a6c (patch)
tree40a4fe775845ececdaf27e82d99d6a06f6b3803e /2004/i/nono
parentb673946d76e436c067d24e535b1e0a167b9dfc47 (diff)
Initial revision
Diffstat (limited to '2004/i/nono')
-rw-r--r--2004/i/nono/i386/GNUmakefile5
-rw-r--r--2004/i/nono/mips/GNUmakefile4
-rw-r--r--2004/i/nono/runtime/rc/asserv4
-rw-r--r--2004/i/nono/runtime/rc/asserv.i3861
-rw-r--r--2004/i/nono/runtime/rc/busp/gps6
-rw-r--r--2004/i/nono/runtime/rc/busp/servo18
-rw-r--r--2004/i/nono/runtime/rc/camera21
-rw-r--r--2004/i/nono/runtime/rc/motor6
-rw-r--r--2004/i/nono/runtime/rc/tracker1
-rw-r--r--2004/i/nono/runtime/rc/vision/palets.rgb2
-rw-r--r--2004/i/nono/src/GNUmakefile44
-rw-r--r--2004/i/nono/src/README1
-rw-r--r--2004/i/nono/src/camera/Makefile.defs11
-rw-r--r--2004/i/nono/src/camera/camera.cc173
-rw-r--r--2004/i/nono/src/camera/camera.h28
-rw-r--r--2004/i/nono/src/camera/dumpimage.cc36
-rw-r--r--2004/i/nono/src/camera/test_camera.cc64
-rw-r--r--2004/i/nono/src/config/Makefile.defs4
-rw-r--r--2004/i/nono/src/config/config.cc162
-rw-r--r--2004/i/nono/src/config/config.h64
-rw-r--r--2004/i/nono/src/config/config_lex.h60
-rw-r--r--2004/i/nono/src/config/config_lex.ll127
-rw-r--r--2004/i/nono/src/date/Makefile.defs8
-rw-r--r--2004/i/nono/src/date/date.cc74
-rw-r--r--2004/i/nono/src/date/date.h54
-rw-r--r--2004/i/nono/src/date/test_date.cc42
-rw-r--r--2004/i/nono/src/erreur/Makefile.defs4
-rw-r--r--2004/i/nono/src/erreur/erreur.cc31
-rw-r--r--2004/i/nono/src/erreur/erreur.h39
-rw-r--r--2004/i/nono/src/kernel_mips/pbus.c393
-rw-r--r--2004/i/nono/src/kernel_mips/pbus.h35
-rw-r--r--2004/i/nono/src/motor/Makefile.defs19
-rw-r--r--2004/i/nono/src/motor/asserv.cc385
-rw-r--r--2004/i/nono/src/motor/asserv.h106
-rw-r--r--2004/i/nono/src/motor/motor.cc216
-rw-r--r--2004/i/nono/src/motor/motor.h80
-rw-r--r--2004/i/nono/src/motor/movement.h51
-rw-r--r--2004/i/nono/src/motor/movement_basic.cc61
-rw-r--r--2004/i/nono/src/motor/movement_basic.h47
-rw-r--r--2004/i/nono/src/motor/movement_goto.cc68
-rw-r--r--2004/i/nono/src/motor/movement_goto.h55
-rw-r--r--2004/i/nono/src/motor/movement_types.h30
-rw-r--r--2004/i/nono/src/motor/test_asserv.cc113
-rw-r--r--2004/i/nono/src/motor/test_motor.cc113
-rw-r--r--2004/i/nono/src/motor/test_tracker.cc82
-rw-r--r--2004/i/nono/src/motor/tracker.cc230
-rw-r--r--2004/i/nono/src/motor/tracker.h66
-rw-r--r--2004/i/nono/src/serial/Makefile.defs8
-rw-r--r--2004/i/nono/src/serial/serial.cc97
-rw-r--r--2004/i/nono/src/serial/serial.h50
-rw-r--r--2004/i/nono/src/serial/test_serial.cc48
-rw-r--r--2004/i/nono/src/vision/Makefile.defs11
-rw-r--r--2004/i/nono/src/vision/image.cc344
-rw-r--r--2004/i/nono/src/vision/image.h48
-rw-r--r--2004/i/nono/src/vision/rgbyuv.h57
-rw-r--r--2004/i/nono/src/vision/test_image.cc34
-rw-r--r--2004/i/nono/src/vision/test_vision.cc48
-rw-r--r--2004/i/nono/src/vision/thresholds.cc55
-rw-r--r--2004/i/nono/src/vision/thresholds.h42
59 files changed, 4086 insertions, 0 deletions
diff --git a/2004/i/nono/i386/GNUmakefile b/2004/i/nono/i386/GNUmakefile
new file mode 100644
index 0000000..cc96535
--- /dev/null
+++ b/2004/i/nono/i386/GNUmakefile
@@ -0,0 +1,5 @@
+SRCDIR = ../src
+LIBPPM = -lppm
+DEFINES = -D"CONFIG_VARIANT=\"i386\""
+
+include $(SRCDIR)/GNUmakefile
diff --git a/2004/i/nono/mips/GNUmakefile b/2004/i/nono/mips/GNUmakefile
new file mode 100644
index 0000000..785831d
--- /dev/null
+++ b/2004/i/nono/mips/GNUmakefile
@@ -0,0 +1,4 @@
+SRCDIR = ../src
+LIBPPM = -lnetpbm
+
+include $(SRCDIR)/GNUmakefile
diff --git a/2004/i/nono/runtime/rc/asserv b/2004/i/nono/runtime/rc/asserv
new file mode 100644
index 0000000..4f7d4bb
--- /dev/null
+++ b/2004/i/nono/runtime/rc/asserv
@@ -0,0 +1,4 @@
+accel 1
+counter true
+ki 16
+tty "/dev/tty00"
diff --git a/2004/i/nono/runtime/rc/asserv.i386 b/2004/i/nono/runtime/rc/asserv.i386
new file mode 100644
index 0000000..9be0cff
--- /dev/null
+++ b/2004/i/nono/runtime/rc/asserv.i386
@@ -0,0 +1 @@
+tty "/dev/null"
diff --git a/2004/i/nono/runtime/rc/busp/gps b/2004/i/nono/runtime/rc/busp/gps
new file mode 100644
index 0000000..aa83c0b
--- /dev/null
+++ b/2004/i/nono/runtime/rc/busp/gps
@@ -0,0 +1,6 @@
+# Facteurs de mise à l'echelle.
+fl1 1.0
+fl2 1.0
+fr 1.0
+# Distance entre les deux balises.
+d 100.0
diff --git a/2004/i/nono/runtime/rc/busp/servo b/2004/i/nono/runtime/rc/busp/servo
new file mode 100644
index 0000000..4a51add
--- /dev/null
+++ b/2004/i/nono/runtime/rc/busp/servo
@@ -0,0 +1,18 @@
+# 1
+35 -1 204 -1 -1 -1 -1 -1
+# 1+
+86 -1 150 -1 -1 -1 -1 -1
+# 2
+100 -1 132 -1 -1 -1 -1 -1
+# 2+
+162 -1 77 -1 -1 -1 -1 -1
+# 3
+177 -1 57 -1 -1 -1 -1 -1
+# 3+
+221 -1 19 -1 -1 -1 -1 -1
+# v
+ -1 -1 -1 -1 -1 -1 240 26
+# r
+ -1 -1 -1 -1 -1 -1 14 252
+# t
+ 75 -1 19 -1 -1 -1 -1 -1
diff --git a/2004/i/nono/runtime/rc/camera b/2004/i/nono/runtime/rc/camera
new file mode 100644
index 0000000..4e046c5
--- /dev/null
+++ b/2004/i/nono/runtime/rc/camera
@@ -0,0 +1,21 @@
+width 352 # 16 bits
+#width 176 # 8 bits (deux fois moins de données utilisées).
+#depth 4 #
+cut 96 # 16 bits
+#cut 448 # 8 bits
+height 288
+setup 0x12 0x80 # Reset
+#setup 0x11 0x3e # Plus lent
+#setup 0x11 0x01 # Clk / 2
+setup 0x11 0x00 # Clk
+setup 0x12 0x24 # YUV
+#setup 0x12 0x2c # RGB
+setup 0x13 0x01 # 16 bits
+#setup 0x13 0x21 # 8 bits
+setup 0x3f 0x00
+setup 0x3d 0x00
+setup 0x38 0x91
+
+#setup 0x38 0x01 # COMK[7] = 0
+setup 0x15 0x41 # COMD[6] = 1
+#setup 0x39 0x40 # COML[6] = 1
diff --git a/2004/i/nono/runtime/rc/motor b/2004/i/nono/runtime/rc/motor
new file mode 100644
index 0000000..ef3fa5d
--- /dev/null
+++ b/2004/i/nono/runtime/rc/motor
@@ -0,0 +1,6 @@
+scale -.1204059790
+speedscale -30.1014947637
+#accelscale 300.7626868453
+accelscale 1000
+maxspeed 0x40
+minspeed 0x01
diff --git a/2004/i/nono/runtime/rc/tracker b/2004/i/nono/runtime/rc/tracker
new file mode 100644
index 0000000..be91f20
--- /dev/null
+++ b/2004/i/nono/runtime/rc/tracker
@@ -0,0 +1 @@
+footing 338.2388703123
diff --git a/2004/i/nono/runtime/rc/vision/palets.rgb b/2004/i/nono/runtime/rc/vision/palets.rgb
new file mode 100644
index 0000000..0bc366a
--- /dev/null
+++ b/2004/i/nono/runtime/rc/vision/palets.rgb
@@ -0,0 +1,2 @@
+1 60 255 15 40 10 30
+2 0 60 50 255 15 30
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$(<F:%.yy=%.cc) --defines=$(<F:%.yy=%.h) $<
+
+%.cc: %.ll
+ flex -o$@ $<
+
+.dep/%.d: %.cc | .dep
+ @echo "Dependency checking for $<..."
+ @set -e; $(CXX) -M $(CPPFLAGS) $< \
+ | sed 's/\($*\)\.o[ :]*/\1.o .dep\/$*.d : /g' > $@; \
+ [ -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 <fcntl.h>
+#include <unistd.h>
+#include <iostream> // Debug.
+#include <iomanip> //
+#include <fstream>
+
+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, &param);
+ }
+ 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, &param);
+ 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 <iomanip>
+#include <unistd.h>
+
+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 <fstream>
+#include <unistd.h>
+#include <stdio.h>
+
+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 <cstring>
+
+// 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<Config *> (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 <string>
+
+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 <sys/time.h>
+#include <time.h>
+
+// 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 <iostream>
+#include <unistd.h>
+
+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 <stdio.h>
+
+// 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 <exception>
+
+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 <sys/param.h>
+#include <sys/kernel.h>
+#include <sys/malloc.h>
+#include <sys/systm.h>
+#include <sys/buf.h>
+#include <sys/device.h>
+#include <sys/proc.h>
+#include <sys/conf.h>
+#include <sys/ioctl.h>
+
+#define BONITO_I2C
+#include <machine/bonito.h>
+#include <ioconf.h>
+#include <machine/bonito_io.h>
+#include <arch/bonito/dev/tests/pbus.h>
+
+/* 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 <sys/ioctl.h>
+
+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 <iostream>
+
+// 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 <string>
+#include <queue>
+
+// 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<std::string> 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 <iostream>
+#include <math.h>
+
+// 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 <iostream>
+
+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 <iostream>
+#include <unistd.h>
+#include <stdlib.h>
+
+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 <iostream>
+#include <unistd.h>
+#include <stdlib.h>
+#include <math.h>
+
+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 <stdlib.h>
+#include <iostream>
+
+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 <iostream>
+#include <math.h>
+
+// 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 <iostream>
+
+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 <fcntl.h>
+
+// 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 <unistd.h>
+#include <termios.h>
+
+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 <iostream>
+
+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 <ppm.h>
+};
+
+// 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 <ppm.h>
+};
+
+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 <unistd.h>
+extern "C" {
+#include <ppm.h>
+};
+
+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 <stdio.h>
+
+// 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