From da4a77ff094a0dc74414ec1de2cac08e8b7b60f4 Mon Sep 17 00:00:00 2001 From: haller Date: Thu, 17 Nov 2005 11:10:17 +0000 Subject: Importation brut du code récupérable de robert --- i/marvin/src/asserv/Makefile.defs | 7 + i/marvin/src/asserv/asserv.cc | 496 ++++++++++++++++++++++++++ i/marvin/src/asserv/asserv.hh | 160 +++++++++ i/marvin/src/asserv/test_asserv.cc | 348 ++++++++++++++++++ i/marvin/src/config/Makefile.defs | 16 + i/marvin/src/config/config.hh | 62 ++++ i/marvin/src/config/config_data.cc | 141 ++++++++ i/marvin/src/config/config_data.hh | 69 ++++ i/marvin/src/config/config_data.tcc | 50 +++ i/marvin/src/config/lexer.ll | 112 ++++++ i/marvin/src/config/parser.yy | 175 +++++++++ i/marvin/src/config/parser_extra.hh | 47 +++ i/marvin/src/config/test_config_data.cc | 45 +++ i/marvin/src/data/Makefile.defs | 12 + i/marvin/src/data/data_buffer.cc | 130 +++++++ i/marvin/src/data/data_buffer.hh | 81 +++++ i/marvin/src/data/data_input.cc | 37 ++ i/marvin/src/data/data_input.hh | 44 +++ i/marvin/src/data/data_input_file.cc | 46 +++ i/marvin/src/data/data_input_file.hh | 45 +++ i/marvin/src/data/data_input_zlib.cc | 84 +++++ i/marvin/src/data/data_input_zlib.hh | 48 +++ i/marvin/src/data/data_output.cc | 34 ++ i/marvin/src/data/data_output.hh | 44 +++ i/marvin/src/data/test_data.cc | 56 +++ i/marvin/src/data/test_data_buffer.cc | 77 ++++ i/marvin/src/log/Makefile.defs | 7 + i/marvin/src/log/log.cc | 102 ++++++ i/marvin/src/log/log.hh | 70 ++++ i/marvin/src/log/log_message.cc | 75 ++++ i/marvin/src/log/log_message.hh | 52 +++ i/marvin/src/log/log_message.tcc | 36 ++ i/marvin/src/log/test_log.cc | 34 ++ i/marvin/src/motor/Makefile.defs | 10 + i/marvin/src/motor/motor.cc | 306 ++++++++++++++++ i/marvin/src/motor/motor.hh | 128 +++++++ i/marvin/src/motor/test_motor.cc | 163 +++++++++ i/marvin/src/proto/Makefile.defs | 8 + i/marvin/src/proto/proto.cc | 433 ++++++++++++++++++++++ i/marvin/src/proto/proto.hh | 135 +++++++ i/marvin/src/proto/test_proto.cc | 124 +++++++ i/marvin/src/scheduler/Makefile.defs | 9 + i/marvin/src/scheduler/schedulable.hh | 50 +++ i/marvin/src/scheduler/schedulable_alarm.cc | 56 +++ i/marvin/src/scheduler/schedulable_alarm.hh | 60 ++++ i/marvin/src/scheduler/schedulable_read_fd.cc | 51 +++ i/marvin/src/scheduler/schedulable_read_fd.hh | 52 +++ i/marvin/src/scheduler/scheduler.cc | 103 ++++++ i/marvin/src/scheduler/scheduler.hh | 53 +++ i/marvin/src/scheduler/test_scheduler.cc | 68 ++++ i/marvin/src/serial/Makefile.defs | 7 + i/marvin/src/serial/serial.cc | 66 ++++ i/marvin/src/serial/serial.hh | 64 ++++ i/marvin/src/serial/serial_base.cc | 84 +++++ i/marvin/src/serial/serial_base.hh | 68 ++++ i/marvin/src/serial/serial_dev.cc | 168 +++++++++ i/marvin/src/serial/serial_dev.hh | 54 +++ i/marvin/src/serial/serial_stdio.cc | 81 +++++ i/marvin/src/serial/serial_stdio.hh | 48 +++ i/marvin/src/serial/test_serial.cc | 88 +++++ i/marvin/src/socket/Makefile.defs | 9 + i/marvin/src/socket/address.cc | 93 +++++ i/marvin/src/socket/address.hh | 76 ++++ i/marvin/src/socket/server_socket.cc | 86 +++++ i/marvin/src/socket/server_socket.hh | 47 +++ i/marvin/src/socket/socket_text.cc | 236 ++++++++++++ i/marvin/src/socket/socket_text.hh | 80 +++++ i/marvin/src/socket/test_server.cc | 91 +++++ i/marvin/src/socket/test_socket.cc | 76 ++++ i/marvin/src/timer/Makefile.defs | 7 + i/marvin/src/timer/test_timer.cc | 47 +++ i/marvin/src/timer/timer.cc | 89 +++++ i/marvin/src/timer/timer.hh | 51 +++ i/marvin/src/utils/Makefile.defs | 15 + i/marvin/src/utils/any.hh | 117 ++++++ i/marvin/src/utils/any.tcc | 169 +++++++++ i/marvin/src/utils/bind.hh | 91 +++++ i/marvin/src/utils/callback.hh | 95 +++++ i/marvin/src/utils/callback.tcc | 136 +++++++ i/marvin/src/utils/errno_exception.hh | 48 +++ i/marvin/src/utils/fd_set.cc | 53 +++ i/marvin/src/utils/fd_set.hh | 54 +++ i/marvin/src/utils/hexa.cc | 113 ++++++ i/marvin/src/utils/hexa.hh | 49 +++ i/marvin/src/utils/list_ostream_output.hh | 62 ++++ i/marvin/src/utils/mathutil.hh | 57 +++ i/marvin/src/utils/meta/Makefile.defs | 5 + i/marvin/src/utils/meta/is_string.hh | 45 +++ i/marvin/src/utils/meta/test_is_string.cc | 35 ++ i/marvin/src/utils/non_copyable.hh | 41 +++ i/marvin/src/utils/test_any.cc | 50 +++ i/marvin/src/utils/test_bind.cc | 57 +++ i/marvin/src/utils/test_callback.cc | 84 +++++ 93 files changed, 7743 insertions(+) create mode 100644 i/marvin/src/asserv/Makefile.defs create mode 100644 i/marvin/src/asserv/asserv.cc create mode 100644 i/marvin/src/asserv/asserv.hh create mode 100644 i/marvin/src/asserv/test_asserv.cc create mode 100644 i/marvin/src/config/Makefile.defs create mode 100644 i/marvin/src/config/config.hh create mode 100644 i/marvin/src/config/config_data.cc create mode 100644 i/marvin/src/config/config_data.hh create mode 100644 i/marvin/src/config/config_data.tcc create mode 100644 i/marvin/src/config/lexer.ll create mode 100644 i/marvin/src/config/parser.yy create mode 100644 i/marvin/src/config/parser_extra.hh create mode 100644 i/marvin/src/config/test_config_data.cc create mode 100644 i/marvin/src/data/Makefile.defs create mode 100644 i/marvin/src/data/data_buffer.cc create mode 100644 i/marvin/src/data/data_buffer.hh create mode 100644 i/marvin/src/data/data_input.cc create mode 100644 i/marvin/src/data/data_input.hh create mode 100644 i/marvin/src/data/data_input_file.cc create mode 100644 i/marvin/src/data/data_input_file.hh create mode 100644 i/marvin/src/data/data_input_zlib.cc create mode 100644 i/marvin/src/data/data_input_zlib.hh create mode 100644 i/marvin/src/data/data_output.cc create mode 100644 i/marvin/src/data/data_output.hh create mode 100644 i/marvin/src/data/test_data.cc create mode 100644 i/marvin/src/data/test_data_buffer.cc create mode 100644 i/marvin/src/log/Makefile.defs create mode 100644 i/marvin/src/log/log.cc create mode 100644 i/marvin/src/log/log.hh create mode 100644 i/marvin/src/log/log_message.cc create mode 100644 i/marvin/src/log/log_message.hh create mode 100644 i/marvin/src/log/log_message.tcc create mode 100644 i/marvin/src/log/test_log.cc create mode 100644 i/marvin/src/motor/Makefile.defs create mode 100644 i/marvin/src/motor/motor.cc create mode 100644 i/marvin/src/motor/motor.hh create mode 100644 i/marvin/src/motor/test_motor.cc create mode 100644 i/marvin/src/proto/Makefile.defs create mode 100644 i/marvin/src/proto/proto.cc create mode 100644 i/marvin/src/proto/proto.hh create mode 100644 i/marvin/src/proto/test_proto.cc create mode 100644 i/marvin/src/scheduler/Makefile.defs create mode 100644 i/marvin/src/scheduler/schedulable.hh create mode 100644 i/marvin/src/scheduler/schedulable_alarm.cc create mode 100644 i/marvin/src/scheduler/schedulable_alarm.hh create mode 100644 i/marvin/src/scheduler/schedulable_read_fd.cc create mode 100644 i/marvin/src/scheduler/schedulable_read_fd.hh create mode 100644 i/marvin/src/scheduler/scheduler.cc create mode 100644 i/marvin/src/scheduler/scheduler.hh create mode 100644 i/marvin/src/scheduler/test_scheduler.cc create mode 100644 i/marvin/src/serial/Makefile.defs create mode 100644 i/marvin/src/serial/serial.cc create mode 100644 i/marvin/src/serial/serial.hh create mode 100644 i/marvin/src/serial/serial_base.cc create mode 100644 i/marvin/src/serial/serial_base.hh create mode 100644 i/marvin/src/serial/serial_dev.cc create mode 100644 i/marvin/src/serial/serial_dev.hh create mode 100644 i/marvin/src/serial/serial_stdio.cc create mode 100644 i/marvin/src/serial/serial_stdio.hh create mode 100644 i/marvin/src/serial/test_serial.cc create mode 100644 i/marvin/src/socket/Makefile.defs create mode 100644 i/marvin/src/socket/address.cc create mode 100644 i/marvin/src/socket/address.hh create mode 100644 i/marvin/src/socket/server_socket.cc create mode 100644 i/marvin/src/socket/server_socket.hh create mode 100644 i/marvin/src/socket/socket_text.cc create mode 100644 i/marvin/src/socket/socket_text.hh create mode 100644 i/marvin/src/socket/test_server.cc create mode 100644 i/marvin/src/socket/test_socket.cc create mode 100644 i/marvin/src/timer/Makefile.defs create mode 100644 i/marvin/src/timer/test_timer.cc create mode 100644 i/marvin/src/timer/timer.cc create mode 100644 i/marvin/src/timer/timer.hh create mode 100644 i/marvin/src/utils/Makefile.defs create mode 100644 i/marvin/src/utils/any.hh create mode 100644 i/marvin/src/utils/any.tcc create mode 100644 i/marvin/src/utils/bind.hh create mode 100644 i/marvin/src/utils/callback.hh create mode 100644 i/marvin/src/utils/callback.tcc create mode 100644 i/marvin/src/utils/errno_exception.hh create mode 100644 i/marvin/src/utils/fd_set.cc create mode 100644 i/marvin/src/utils/fd_set.hh create mode 100644 i/marvin/src/utils/hexa.cc create mode 100644 i/marvin/src/utils/hexa.hh create mode 100644 i/marvin/src/utils/list_ostream_output.hh create mode 100644 i/marvin/src/utils/mathutil.hh create mode 100644 i/marvin/src/utils/meta/Makefile.defs create mode 100644 i/marvin/src/utils/meta/is_string.hh create mode 100644 i/marvin/src/utils/meta/test_is_string.cc create mode 100644 i/marvin/src/utils/non_copyable.hh create mode 100644 i/marvin/src/utils/test_any.cc create mode 100644 i/marvin/src/utils/test_bind.cc create mode 100644 i/marvin/src/utils/test_callback.cc diff --git a/i/marvin/src/asserv/Makefile.defs b/i/marvin/src/asserv/Makefile.defs new file mode 100644 index 0000000..80bf8ee --- /dev/null +++ b/i/marvin/src/asserv/Makefile.defs @@ -0,0 +1,7 @@ +PROGRAMS += test_asserv + +asserv_OBJECTS = asserv.o + +test_asserv_OBJECTS = test_asserv.o $(asserv_OBJECTS) $(config_OBJECTS) $(timer_OBJECTS) $(proto_OBJECTS) $(log_OBJECTS) $(serial_OBJECTS) $(utils_OBJECTS) + +test_asserv: $(test_asserv_OBJECTS) diff --git a/i/marvin/src/asserv/asserv.cc b/i/marvin/src/asserv/asserv.cc new file mode 100644 index 0000000..082f39b --- /dev/null +++ b/i/marvin/src/asserv/asserv.cc @@ -0,0 +1,496 @@ +// asserv.cc +// robert - programme du robot 2005 {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// 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.hh" + +#include + +/// Constructeur +Asserv::Asserv (const Config & config, Receiver & receiver) + :proto_(*this), receiver_(receiver), log_("asserv") +{ + // Rechargement des paramètres + loadConfig(config); + //Ouverture du port série + proto_.open(ttyName_); +} + +/// Reset de l'AVR +void Asserv::reset(void) +{ + // Reset AVR + proto_.send('z'); + // Envoie des données de conf sur l'AVR + setFooting(footing_); + setEpsilon(epsilon_); + setKp(kp_); + setKi(ki_); + setKd(kd_); + setESat(eSat_); + setSpeedIntMax(speedIntMax_); + setDSample(dSample_); + setAccel(accel_); + setMaxSpeed(maxSLin_, maxSRot_); + setUseTazFSM(useTazFSM_); + setInvPwm(leftInvPwm_, rightInvPwm_); + statCounter(pCounter_); + statPosition(pPosition_); + statMotor(pMotor_); + statPwm(pPwm_); + statTiming(pTiming_); + statInPort(pStatInPort_); + statSharp(pStatInPort_); +} + +/// Essaie de purger la liste d'émission et indique si elle est vide. +bool Asserv::sync(void) +{ + return proto_.sync(); +} + +/// Attend que toute les émissions soit terminées +bool Asserv::wait(int timeout) +{ + return proto_.wait(timeout); +} + +/// Récupère le File Descriptor +int Asserv::getFd(void) +{ + return proto_.getFd(); +} + +/// Commandes d'asservissement +void Asserv::linearMove(double distance) +{ + // Conversion mm->PasW + int distPas = mm2pas(distance); + //log_ ("linearMove") << "avConv" << distance << "apConv" << distPas; + // On envoie la commande à l'AVR + proto_.send('l', "w", distPas); +} + +void Asserv::angularMove(double angle) +{ + // Conversion radian->256ème + int a = radTo256(angle); + //log_ ("angularMove") << "avConv" << angle << "apConv" << a; + // Envopie vers avr + proto_.send('a',"b", a); +} + +void Asserv::goToPosition(double xPos, double yPos) +{ + // Convertion mm->PasD + int x = mm2pas(xPos, true); + int y = mm2pas(yPos, true); + //log_ ("goToPosition") << "avConvX" << xPos << "apConvX" << x; + //log_ ("goToPosition") << "avConvY" << yPos << "apConvY" << y; + // Envoie vers l'AVR + proto_.send('g', "dd", x, y); +} + +// XXX Vérifier la conversion ca me parais un peu petit +void Asserv::fuckTheWall(double speed) +{ + // Conversion mm/s->Pas/Period + int v = mms2ppp(speed); + //log_ ("fuckTheWall") << "avConv" << speed << "apConv" << v; + // Envoie vers l'AVR + fuckTheWall(v); +} + +void Asserv::fuckTheWall(int speed) +{ + //log_ ("fuckTheWall") << "avConv"; + // Envoie vers l'AVR + proto_.send('f',"b", speed); +} + +void Asserv::setSpeed(double xSpeed, double ySpeed) +{ + // Conversion mm/s->Pas/Period + int vx = mms2ppp(xSpeed); + int vy = mms2ppp(ySpeed); + //log_ ("setSpeed") << "avConvX" << xSpeed << "apConvX" << vx; + //log_ ("setSpeed") << "avConvY" << ySpeed << "apConvY" << vy; + // Envoie vers l'AVR + setSpeed(vx,vy); +} + +void Asserv::setSpeed(int xSpeed, int ySpeed) +{ + //log_ ("setSpeed") << "avConvX" << xSpeed ; + //log_ ("setSpeed") << "avConvY" << ySpeed ; + // Envoie vers l'AVR + proto_.send('s',"bb", xSpeed, ySpeed); +} + +void Asserv::setPwm(int xPwm, int yPwm) +{ + //log_ ("setPwm") << "avConvX" << xPwm; + //log_ ("setPwm") << "avConvY" << yPwm; + // Envoie sur l'AVR + proto_.send('w',"ww" ,xPwm, yPwm); +} + +/// Informe l'AVR d'arréter de dire "j'ai fini" +void Asserv::finishAck(void) +{ + proto_.send('F', "b", 2); +} + +/// XXX Valeurs bizarres +/// Statistiques +void Asserv::statCounter(int period) +{ + + pCounter_ = period; + // Convertion s->Period + //int p = s2period(period); + //log_ ("statCounter") << "avConv" << period << "apConv" << p; + // Envoie sur l'AVR + proto_.send('C', "b", period); +} + +void Asserv::statPosition(int period) +{ + pPosition_ = period; + // Convertion s->Period + //int p = s2period(period); + // Envoie sur l'AVR + proto_.send('X', "b", period); +} + +void Asserv::statMotor(int period) +{ + pMotor_ = period; + // Convertion s->Period + //int p = s2period(period); + // Envoie sur l'AVR + proto_.send('S', "b", period); +} + +void Asserv::statPwm(int period) +{ + pPwm_ = period; + // Convertion s->Period + // int p = s2period(period); + // Envoie sur l'AVR + proto_.send('W', "b", period); +} + +void Asserv::statTiming(int period) +{ + pTiming_ = period; + // Convertion s->Period + //int p = s2period(period); + // Envoie sur l'AVR + proto_.send('T', "b", period); +} + +void Asserv::statInPort(int period) +{ + pStatInPort_ = period; + // Convertion s->Period + //int p = s2period(period); + // Envoie sur l'AVR + proto_.send('P', "b", period); +} + +void Asserv::statSharp(int period) +{ + pSharp_ = period; + // Conversion s->period + //int p = s2period(period); + // Envoie sur l'AVR + proto_.send('H', "b", period); +} + +/// Change les paramètres +void Asserv::setXPos(double xPos) +{ + // Conversion mm->pas + int p = mm2pas(xPos, true); + // Envoie à l'AVR + proto_.send('p',"bd", 'x', p); +} + +void Asserv::setYPos(double yPos) +{ + // Conversion mm->pas + int p = mm2pas(yPos, true); + // Envoie à l'AVR + proto_.send('p',"bd", 'y', p); +} + +void Asserv::setAngle(double angle) +{ + // Envoie à l'AVR + proto_.send('p',"bd", 'a', static_cast(angle / (2 * M_PI) * (1 << 24))); +} + +void Asserv::setFooting(int16_t footing) +{ + footing_ = footing; + // Envoie à l'AVR + proto_.send('p',"bw", 'f', footing); +} + +void Asserv::setEpsilon(double epsilon) +{ + epsilon_ = epsilon; + // Envoie à l'AVR + proto_.send('p',"bd", 'e', static_cast(epsilon * 256)); +} + +void Asserv::setKp(int Kp) +{ + kp_ = Kp; + // Envoie à l'AVR + proto_.send('p',"bw", 'p', Kp); +} + +void Asserv::setKi(int Ki) +{ + ki_ = Ki; + // Envoie à l'AVR + proto_.send('p',"bw", 'i', Ki); +} + +void Asserv::setKd(int Kd) +{ + kd_ = Kd; + // Envoie à l'AVR + proto_.send('p',"bw", 'd', Kd); +} + +void Asserv::setESat(int eSat) +{ + eSat_ = eSat; + // Envoie à l'AVR + proto_.send('p', "bw", 'E', eSat); +} + +void Asserv::setSpeedIntMax(int maxInt) +{ + speedIntMax_ = maxInt; + proto_.send('p', "bW", 'I', maxInt); +} + +void Asserv::setDSample (int dSample) +{ + dSample_ = dSample; + proto_.send('p', "bb", 's', dSample); +} + +void Asserv::setAccel(int accel) +{ + accel_ = accel; + //log_ ("setAccel") << "avConv" << accel; + // envoie vers l'AVR + proto_.send('p',"bb", 'a', accel_); +} + +/// Renvoie l'accélération en unité international (mm/s²) +double Asserv::getAccel(void) +{ + return mmPpas_ / (sPperiod_ * sPperiod_) / accel_; // XXX Vérifier ca +} + +/// Règle les vitesses maximum(lineaire et rotation) +void Asserv::setMaxSpeed(int maxSLin, int maxSRot) +{ + maxSLin_ = (maxSLin > 0 ? maxSLin : maxSLin_); + maxSRot_ = (maxSRot > 0 ? maxSRot : maxSRot_); + // Envoie à l'AVR + proto_.send('p',"bbb", 'm', maxSLin_, maxSRot_); +} + +int Asserv::getMaxLSpeed(void) +{ + return maxSLin_; +} + +void Asserv::setUseTazFSM(bool use) +{ + useTazFSM_ = use; + // Envoie la commande à l'AVR + proto_.send('t', "b", use ? 1 : 0); +} + +void Asserv::setInvPwm(bool leftInvPwm, bool rightInvPwm) +{ + leftInvPwm_ = leftInvPwm; + rightInvPwm_ = rightInvPwm; + // Envoie la commande à l'AVR + proto_.send('p', "bbb", 'w', leftInvPwm , rightInvPwm); +} + +/// implémentation du proto::Receiver +void Asserv::receive(char command, const Proto::Frame &frame) +{ + switch(command) + { + case 'C': //Compteurs des moteurs (uint16) + int lMotor; + int rMotor; + proto_.decode(frame, "ww", lMotor, rMotor); + receiver_.receiveCounter(lMotor, rMotor); + break; + case 'X': + int xPos; + proto_.decode(frame, "D", xPos); + // Convertion Pas->mm + receiver_.receivePosX(xPos * mmPpas_ / 256); + break; + case 'Y': + int yPos; + proto_.decode(frame, "D", yPos); + // Convertion Pas->mm + receiver_.receivePosY(yPos * mmPpas_ / 256); + break; + case 'A': + { + int aPos; + proto_.decode(frame, "D", aPos); + double aPosD = (aPos * (2 * M_PI) / (1 << 24)); + receiver_.receivePosA(aPosD); + break; + } + case 'S': + int oldLSpeed, LSpeed, oldRSpeed, RSpeed; + proto_.decode(frame, "WWWW", oldLSpeed, LSpeed, oldRSpeed, RSpeed); + receiver_.receiveSpeedStat(oldLSpeed, LSpeed, oldRSpeed, RSpeed); + break; + case 'W': + int leftPwm, rightPwm; + proto_.decode(frame, "WW", leftPwm, rightPwm); + receiver_.receivePwm(leftPwm, rightPwm); + break; + case 'T': + { + int motorTimer4 = static_cast(frame.args[1]); + int motorTimer3 = static_cast(frame.args[2]); + int motorTimer2 = static_cast(frame.args[3]); + int motorTimer1 = static_cast(frame.args[4]); + int motorTimer0 = static_cast(frame.args[5]); + receiver_.receiveTiming(motorTimer4, motorTimer3, motorTimer2, + motorTimer1, motorTimer0); + break; + } + case 'P': + { + int statePIn; + proto_.decode(frame, "b", statePIn); + receiver_.receiveInPort(statePIn); + break; + } + case 'H': + { + int stateSharp0, stateSharp1, stateSharp2; + proto_.decode(frame, "www", stateSharp0, stateSharp1, stateSharp2); + receiver_.receiveSharp(stateSharp0, stateSharp1, stateSharp2); + break; + } + case 't': + { + int state, subState; + proto_.decode(frame, "bb", state, subState); + receiver_.receiveTazState(state, subState); + } + case 'F': + receiver_.done(); + break; + } +} + +/// Charge les données de la classe config +void Asserv::loadConfig(const Config & config) +{ + ttyName_ = config.get("asserv.tty"); + footing_ = config.get("asserv.footing"); + epsilon_ = config.get("asserv.epsilon"); + accel_ = config.get("asserv.accel"); + kp_ = config.get("asserv.kp"); + ki_ = config.get("asserv.ki"); + kd_ = config.get("asserv.kd"); + eSat_ = config.get("asserv.eSat"); + speedIntMax_ = config.get("asserv.speedIntMax"); + dSample_ = config.get("asserv.dSample"); + maxSLin_ = config.get("asserv.maxSLin"); + maxSRot_ = config.get("asserv.maxSRot"); + useTazFSM_ = config.get("asserv.useTazFSM"); + mmPpas_ = config.get("asserv.mmPpas"); + sPperiod_ = config.get("asserv.sPperiod"); + pwmMax_ = config.get("asserv.pwmMax"); + leftInvPwm_ = config.get("asserv.leftInvPwm"); + rightInvPwm_ = config.get("asserv.rightInvPwm"); + pCounter_ = 0; + pPosition_ = 0; + pMotor_ = 0; + pPwm_ = 0; + pTiming_ = 0; + pStatInPort_ = 32; + pSharp_ = 0; +} + +int Asserv::mm2pas(double dist, bool format24eme) +{ + if(format24eme) + return static_cast((dist/mmPpas_)*256); + else + return static_cast(dist/mmPpas_); +} + +int Asserv::radTo256(double angle, bool format24eme) +{ + if(angle < 0) + angle += 2 * M_PI; + if(format24eme) + return static_cast((angle / (2 * M_PI)) * 256 * (1 << 24)); + else + return static_cast((angle / (2 * M_PI))* 256); +} + +int Asserv::mms2ppp(double vitesse) +{ + return static_cast(vitesse * sPperiod_ / mmPpas_); +} + +int Asserv::rCycl2Pwm(double rCycl) +{ + return static_cast(rCycl*pwmMax_); +} + +int Asserv::s2period(double period) +{ + return static_cast(period/sPperiod_); +} + +//int Asserv::mmps2ppperiod(double accel) +//{ +// return static_cast(1/(accel*(sPperiod_*sPperiod_/mmPpas_))); +//} diff --git a/i/marvin/src/asserv/asserv.hh b/i/marvin/src/asserv/asserv.hh new file mode 100644 index 0000000..4064537 --- /dev/null +++ b/i/marvin/src/asserv/asserv.hh @@ -0,0 +1,160 @@ +#ifndef asserv_hh +#define asserv_hh +// asserv.hh +// robert - programme du robot 2005 {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// 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 "utils/non_copyable.hh" +#include "config/config.hh" +#include "proto/proto.hh" +#include "log/log.hh" + +/// Classe de dialogue avec la carte d'asservissement. +class Asserv : public NonCopyable, public Proto::Receiver +{ + public: + /// Les clients de Asserv doivent dériver de Receiver. + class Receiver + { + public: + virtual ~Receiver(void){}; + /// Recoit un packet. + virtual void receiveCounter (double lMotor, double rMotor) = 0; + virtual void receivePosX (double xPos) = 0; + virtual void receivePosY (double yPos) = 0; + virtual void receivePosA (double aPos) = 0; + virtual void receiveSpeedStat (int leftError, int leftInt, int rightError, int rightInt) = 0; + virtual void receivePwm (double leftPwm, double rightPwm) = 0; + virtual void receiveTiming (int motorTimer4, + int motorTimer3, int motorTimer2, + int motorTimer1, + int motorTimer0 ) = 0; + virtual void receiveInPort (int port) = 0; + virtual void receiveSharp (int sharp1, int sharp2, int sharp3) = 0; + virtual void receiveTazState(int state, int subState){}; // XXX Vérifier les formats + virtual void done (void) = 0; + }; + + private: + + // Communication avec l'AVR + Proto proto_; + std::string ttyName_; + Receiver & receiver_; + // Paramètre de l'avr + int footing_; //truc + double epsilon_; //mm + int accel_; //unité AVR + int kp_; + int ki_; + int kd_; + int eSat_; + int speedIntMax_; + int dSample_; + int maxSLin_;//unité AVR + int maxSRot_; + bool leftInvPwm_, rightInvPwm_; + bool useTazFSM_; + // Stat + int pCounter_; + int pPosition_; + int pMotor_; + int pPwm_; + int pTiming_; + int pStatInPort_; + int pSharp_; + + // Unités + double mmPpas_; + double sPperiod_; + int pwmMax_; + + // Système de log. + Log log_; + public: + /// Constructeur. + Asserv (const Config & config, Asserv::Receiver & receiver); + /// Reset la carte et envois les paramètres. + void reset (void); + /// Essaie de purger la liste d'émission et indique si elle est vide. + bool sync (void); + /// Attend que toute les émissions soit terminées. + bool wait (int timeout = -1); + /// Récupère le File Descriptor + int getFd(void); + /// Commandes asservissement + void linearMove(double distance); + void angularMove(double angle); + void goToPosition(double xPos, double yPos); + void fuckTheWall(double speed); + void fuckTheWall(int speed); + void setSpeed(double xSpeed, double ySpeed); + void setSpeed(int xSpeed = 0, int ySpeed = 0); + void setPwm(int leftPwm = 0, int rightPwm = 0); + void finishAck(void); //met le paramètre F à 0 + /// Statistiques + void statCounter(int period = 0); + void statPosition(int period = 0); + void statMotor(int period = 0); + void statPwm(int period = 0); + void statTiming(int period = 0); + void statInPort(int period = 0); + void statSharp(int period = 0); + //@{ + /// Change les paramètres de la carte. + void setXPos(double pos); + void setYPos(double pos); + void setAngle(double angle); + void setFooting(int16_t dist); + void setEpsilon(double e); + void setKp (int kp); + void setKi (int ki); + void setKd (int kd); + void setESat (int eSat); + void setSpeedIntMax (int maxInt); + void setDSample(int dSample); + void setAccel (int accel); + double getAccel (void); + void setMaxSpeed(int maxSLin, int maxSRot); + int getMaxLSpeed(void); + void setUseTazFSM(bool use); + void setInvPwm(bool leftInvPwm, bool rightInvPwm); + //@} + + /// implémentation du proto::Receiver + void receive (char command, const Proto::Frame &frame); + + private: + /// Charge les données de la classe config + void loadConfig(const Config & config); + /// Fonctions de conversion + int mm2pas(double dist, bool format24eme = false); //Format 24/8 ou 8 + int radTo256(double angle, bool format24eme = false); //Format 8 ou 8/24 + int mms2ppp(double vitesse); // Format 8 + int rCycl2Pwm(double rCycl); // Format 16 + int s2period(double period); // Format 8 + //int mmps2ppperiod(double accel); //Format 8 (1 vitesse par x period) +}; + +#endif // asserv_hh diff --git a/i/marvin/src/asserv/test_asserv.cc b/i/marvin/src/asserv/test_asserv.cc new file mode 100644 index 0000000..228748c --- /dev/null +++ b/i/marvin/src/asserv/test_asserv.cc @@ -0,0 +1,348 @@ +// test_asserv.cc +// robert - programme du robot 2005 {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// 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.hh" +#include "config/config.hh" +#include "timer/timer.hh" + +#include +#include +#include + +/// Classe de test pour asserv. +class TestAsserv : public Asserv::Receiver +{ + void receiveCounter (double lMotor, double rMotor) + { + std::cout << "C:" << "lMotor:" << lMotor << " rMotor:" << rMotor << std::endl; + } + + void receivePosX (double xPos) + { + std::cout << "X:" << "XPos:" << xPos << std::endl; + } + + void receivePosY (double yPos) + { + std::cout << "Y:" << "yPos:" << yPos << std::endl; + } + + void receivePosA (double aPos) + { + std::cout << "A:" << " aPosR: " << aPos << " aPosD: " << aPos / (2 * M_PI) * 360 << std::endl; + } + + void receiveSpeedStat (int leftError, int leftInt, int rightError, int rightInt) + { + std::cout << "S:" << " leftError: " << leftError << "leftInt: " << + leftInt << " rightError: " << rightError << + " rightInt: " << rightInt << std::endl; + } + + void receivePwm (double leftPwm, double rightPwm) + { + std::cout << "W:" << "leftPwm" << leftPwm << " rightPwm:" << rightPwm << std::endl; + } + + void receiveTiming (int motorTimer4, int motorTimer3, + int motorTimer2, int motorTimer1, + int motorTimer0 ) + { + std::cout << "T:" << " motorTimer4: " << motorTimer4 << " motorTimer3: " + << motorTimer3 << " motorTimer2: " << motorTimer2 + << " motorTimer1: " << motorTimer1 << " motorTimer0: " + << motorTimer0 << std::endl; + } + + void receiveInPort (int port) + { + std::cout << "P:" << "state:" << port << std::endl; + } + + void receiveSharp (int sharp0, int sharp1, int sharp2) + { + std::cout << "t:" << "sharp0:" << sharp0 << " sharp1:" << sharp1 + << " sharp2:" << sharp2 << std::endl; + } + + void done (void) + { + std::cout << "F:" << "J'ai fini!!" << std::endl; + } + +}; + +/// Affiche un memo de suntaxe. + void +syntax (void) +{ + std::cout << "test_asserv - test la classe de protocol asserv.\n" + "Syntaxe : test_asserv <...>\n" + " s envois une commande\n" + " w attend pendant un nombre de millisecondes\n" + " ? affiche cet écran d'aide\n" + << std::endl; +} + + int +main (int argc, char **argv) +{ + try + { + int i; + if (argc < 2) + { + syntax (); + return 1; + } + Config config(argc, argv); + TestAsserv testAsserv; + Asserv asserv(config, testAsserv); + i = 1; + while (i < argc) + { + switch (argv[i][0]) + { + case 's': + { + switch(argv[++i][0]) + { + case 'z': + asserv.reset(); + break; + case 'l': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.linearMove(strtod(argv[i], 0)); + break; + case 'a': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.angularMove(strtod(argv[i], 0)); + break; + case 'g': + if(++i + 1 > argc) + throw std::runtime_error("syntax error"); + asserv.goToPosition(strtod(argv[i], 0), + strtod(argv[i + 1], 0)); + i++; + break; + case 'f': + if(++i + 1 > argc) + throw std::runtime_error("syntax error"); + switch(argv[i][0]) + { + case 'd': + asserv.fuckTheWall(strtod(argv[i + 1], 0)); + break; + case 'i': + asserv.fuckTheWall(atoi(argv[i + 1])); + break; + default: + throw std::runtime_error("syntax error"); + break; + } + i++; + break; + case 's': + if(++i + 2 > argc) + throw std::runtime_error("syntax error"); + switch(argv[i][0]) + { + case 'd': + i++; + asserv.setSpeed(strtod(argv[i], 0), strtod(argv[i + 1], 0)); + break; + case 'i': + i++; + asserv.setSpeed(atoi(argv[i]), atoi(argv[i + 1])); + break; + default: + throw std::runtime_error("syntax error"); + break; + } + i++; + break; + case 'w': + if(++i + 1 > argc) + throw std::runtime_error("syntax error"); + asserv.setPwm(strtol(argv[i], 0, 10), strtol(argv[i + 1], 0, 10)); + i++; + break; + case 'F': + asserv.finishAck(); + break; + case 'C': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.statCounter(strtol(argv[i], 0, 10)); + break; + case 'X': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.statPosition(strtol(argv[i], 0, 10)); + break; + case 'S': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.statMotor(strtol(argv[i], 0, 10)); + break; + case 'W': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.statPwm(strtol(argv[i], 0, 10)); + break; + case 'T': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.statTiming(strtol(argv[i], 0, 10)); + break; + case 'P': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.statInPort(strtol(argv[i], 0, 10)); + break; + + case 'H': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.statSharp(atoi(argv[i])); + break; + + case 'p': + if(++i > argc) + throw std::runtime_error("syntax error"); + switch(argv[i][0]) + { + case 'x': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setXPos(strtod(argv[i], 0)); + break; + case 'y': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setYPos(strtod(argv[i], 0)); + break; + case 'a': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setAngle(strtod(argv[i], 0)); + break; + case 'f': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setFooting(strtol(argv[i], 0, 10)); + break; + case 'e': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setEpsilon(strtod(argv[i], 0)); + break; + case 'p': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setKp(strtol(argv[i], 0, 10)); + break; + case 'i': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setKi(strtol(argv[i], 0, 10)); + break; + case 'd': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setKd(strtol(argv[i], 0, 10)); + break; + case 'E': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setESat(strtol(argv[i], 0, 10)); + break; + case 'I': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setSpeedIntMax(strtol(argv[i], 0, 10)); + break; + case 's': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setDSample(strtol(argv[i], 0, 10)); + break; + case 'A': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setAccel(strtol(argv[i], 0, 10)); + break; + case 'm': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setMaxSpeed(strtol(argv[i], 0, 10), + strtol(argv[i + 1], 0, 10)); + i++; + break; + case 'W': + if (++i + 1 > argc) + throw std::runtime_error("syntax error"); + asserv.setInvPwm(atoi(argv[i]), atoi(argv[i + 1])); + i++; + break; + } + } + asserv.sync(); + break; + } + case 'w': + { + int stop, t; + if (i + 1 >= argc) + throw std::runtime_error ("syntax error"); + stop = atoi (argv[++i]) + Timer::getProgramTime (); + t = Timer::getProgramTime (); + while (t < stop) + { + asserv.wait (stop - t); + t = Timer::getProgramTime (); + asserv.sync(); + } + break; + } + case '?': + syntax (); + return 0; + default: + throw std::runtime_error ("syntax error"); + } + i++; + } + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + syntax (); + return 1; + } + return 0; +} diff --git a/i/marvin/src/config/Makefile.defs b/i/marvin/src/config/Makefile.defs new file mode 100644 index 0000000..7683a6c --- /dev/null +++ b/i/marvin/src/config/Makefile.defs @@ -0,0 +1,16 @@ +PROGRAMS += test_config_data + +config_OBJECTS = lexer.o parser.o config_data.o + +test_config_data_OBJECTS = $(config_OBJECTS) test_config_data.o + +EXTRA_CLEAN += parser.hh parser.cc lexer.hh lexer.cc + +test_config_data: $(test_config_data_OBJECTS) + +parser.hh: parser.cc +lexer.o: parser.hh + +lexer.hh: lexer.cc +parser.o: lexer.hh +config_data.o: lexer.hh diff --git a/i/marvin/src/config/config.hh b/i/marvin/src/config/config.hh new file mode 100644 index 0000000..c88ec98 --- /dev/null +++ b/i/marvin/src/config/config.hh @@ -0,0 +1,62 @@ +#ifndef config_hh +#define config_hh +// config.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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_data.hh" + +#include +#include + +/// Classe d'accés à la configuration. +class Config +{ + ConfigData configData_; + public: + typedef std::list IntList; + typedef std::list FloatList; + typedef std::list StringList; + public: + /// Boa constricteur, voir ConfigData. + Config (int &argc, char **&argv) + : configData_ (argc, argv) + { } + /// Boa constricteur, voir ConfigData. + Config (int &argc, char **&argv, const std::string &file) + : configData_ (argc, argv, file) + { } + /// Récupère une valeur de configuration, fonction générique. + template + const T &get (const std::string &id) const + { + return configData_.get (id); + } + /// Récupère une valeur de configuration. + const any &get (const std::string &id) const + { + return configData_.get (id); + } +}; + +#endif // config_hh diff --git a/i/marvin/src/config/config_data.cc b/i/marvin/src/config/config_data.cc new file mode 100644 index 0000000..33a78c0 --- /dev/null +++ b/i/marvin/src/config/config_data.cc @@ -0,0 +1,141 @@ +// config_data.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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.hh" +#include "config_data.hh" +#include "parser_extra.hh" +#include "parser.hh" +#include "lexer.hh" + +#include + +/// Constructeur. Traite les arguments de la ligne de commande et lit le +/// fichier de configuration. Les arguments traités sont retiré de la +/// ligne de commande. +ConfigData::ConfigData (int &argc, char **&argv) +{ + init (argc, argv, "rc/config"); +} + +/// Constructeur. Fourni en plus un fichier de configuration différent de +/// celui par défaut. +ConfigData::ConfigData (int &argc, char **&argv, const std::string &file) +{ + init (argc, argv, file); +} + +/// Récupère une valeur de configuration. +const any & +ConfigData::get (const std::string &id) const +{ + Data::const_iterator i = data_.find (id); + if (i == data_.end ()) + throw std::runtime_error (id + ": config item not found"); + return i->second; +} + +/// Ajoute une valeur de configuration. VAL prend l'ancienne valeur ou un any +/// vide. +void +ConfigData::add (const std::string &id, any &val) +{ + any &a = data_[id]; + a.swap (val); +} + +/// Initialise (lit la ligne de commande et les fichiers de config. +void +ConfigData::init (int &argc, char **&argv, const std::string &file) +{ + parseFile (file); +} + +int yyparse (void *); + +/// Lance le parseur sur un fichier. +void +ConfigData::parseFile (const std::string &file) +{ + FILE *f; + f = fopen (file.c_str (), "r"); + if (!f) + throw std::runtime_error ("can not open config file \"" + file + "\""); + try + { + // Crée un scanner, initialise son tampon d'entré, puis parse. + yyscan_t scanner; + YY_BUFFER_STATE buf; + ParserExtra pe (*this); + yylex_init (&scanner); + yyset_extra (&pe, scanner); + buf = yy_create_buffer (f, YY_READ_BUF_SIZE, scanner); + yy_switch_to_buffer (buf, scanner); + int ret = yyparse (scanner); + yy_delete_buffer (buf, scanner); + yylex_destroy (scanner); + if (ret) + throw std::runtime_error ("parse error"); + } + catch (const std::runtime_error &e) + { + fclose (f); + throw std::runtime_error ("in config file \"" + file + "\": " + + e.what ()); + } + fclose (f); +} + +/// Lance le parseur sur une chaîne. +void +ConfigData::parseString (const std::string &s) +{ + try + { + // Crée un scanner, initialise son tampon d'entré, puis parse. + yyscan_t scanner; + YY_BUFFER_STATE buf; + ParserExtra pe (*this); + yylex_init (&scanner); + yyset_extra (&pe, scanner); + buf = yy_scan_bytes (s.data (), s.size (), scanner); + int ret = yyparse (scanner); + yy_delete_buffer (buf, scanner); + yylex_destroy (scanner); + if (ret) + throw std::runtime_error ("parse error"); + } + catch (const std::runtime_error &e) + { + throw std::runtime_error ("in config string \"" + s + "\": " + + e.what ()); + } +} + +/* Shut up warning for this wrongly declared static function. */ +static int +yy_init_globals (yyscan_t yyscanner) +{ + return yy_init_globals (yyscanner); +} + diff --git a/i/marvin/src/config/config_data.hh b/i/marvin/src/config/config_data.hh new file mode 100644 index 0000000..0d0944e --- /dev/null +++ b/i/marvin/src/config/config_data.hh @@ -0,0 +1,69 @@ +#ifndef config_data_hh +#define config_data_hh +// config_data.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "utils/any.hh" + +#include +#include + +/// Classe contenant les informations de configuration. +class ConfigData +{ + typedef std::map Data; + Data data_; + public: + /// Constructeur. Traite les arguments de la ligne de commande et lit le + /// fichier de configuration. Les arguments traités sont retiré de la + /// ligne de commande. + ConfigData (int &argc, char **&argv); + /// Constructeur. Fourni en plus un fichier de configuration différent de + /// celui par défaut. + ConfigData (int &argc, char **&argv, const std::string &file); + /// Récupère une valeur de configuration, fonction générique. + template + const T &get (const std::string &id) const; + /// Récupère une valeur de configuration. + const any &get (const std::string &id) const; + /// Ajoute une valeur de configuration. VAL prend l'ancienne valeur ou un + /// any vide. + void add (const std::string &id, any &val); + /// Ajoute une valeur de configuration, fonction générique. Attention, + /// cette fonction est plus couteuse car elle fait une copie de + /// la valeur. + template + void add (const std::string &id, const T &val); + private: + /// Initialise (lit la ligne de commande et les fichiers de config. + void init (int &argc, char **&argv, const std::string &file); + /// Lance le parseur sur un fichier. + void parseFile (const std::string &file); + /// Lance le parseur sur une chaîne. + void parseString (const std::string &s); +}; + +#include "config_data.tcc" + +#endif // config_data_hh diff --git a/i/marvin/src/config/config_data.tcc b/i/marvin/src/config/config_data.tcc new file mode 100644 index 0000000..60f4910 --- /dev/null +++ b/i/marvin/src/config/config_data.tcc @@ -0,0 +1,50 @@ +// config_data.tcc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 + +/// Récupère une valeur de configuration, fonction générique. +template +const T & +ConfigData::get (const std::string &id) const +{ + const any &a = get (id); + const T *v = any_cast (&a); + if (!v) + throw std::runtime_error (id + ": config item with bad type"); + return *v; +} + +/// Ajoute une valeur de configuration, fonction générique. Attention, +/// cette fonction est plus couteuse car elle fait une copie de +/// la valeur. +template +void +ConfigData::add (const std::string &id, const T &val) +{ + any a (val); + add (id, a); +} + diff --git a/i/marvin/src/config/lexer.ll b/i/marvin/src/config/lexer.ll new file mode 100644 index 0000000..96a90b3 --- /dev/null +++ b/i/marvin/src/config/lexer.ll @@ -0,0 +1,112 @@ +%{ +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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.hh" +#include "config/parser_extra.hh" +#include "utils/any.hh" +#include "parser.hh" +%} + +%option reentrant +%option header-file="lexer.hh" +%option outfile="lexer.cc" +%option bison-bridge +%option noyywrap nodefault nounput + +%x strst + +INTDEC [+-]?[0-9]+ +INTHEX "0x"[0-9a-fA-F]+ +INTNUM {INTDEC}|{INTHEX} +FLOAT1 [+-]?\.[0-9]+ +FLOAT2 [+-]?[0-9]+\.[0-9]* +FLOATNUM {FLOAT1}|{FLOAT2} + +%% + +"true"|"on" { + yylval->b = true; + return BOOLEAN; +} + +"false"|"off" { + yylval->b = false; + return BOOLEAN; +} + +[a-zA-Z][_a-zA-Z0-9.-]* { + yylval->s = new std::string (yytext); + return ID; +} + +{FLOATNUM} { + yylval->f = strtod (yytext, 0); + return FLOAT; +} + +{INTNUM} { + yylval->i = strtol (yytext, 0, 0); + return INT; +} + +\" { + BEGIN(strst); + yyextra->tmp.clear (); +} + +[=\n()] return yytext[0]; + +"img:" return IMG; +"data:" return DATA; + +[ \t]+ /* Skip. */ +#.* /* Skip comments. */ + +. { + yylval->c = yytext[0]; + std::cout << "UNKNOWN " << yytext[0] << std::endl; + return UNKNOWN; +} + +\" { + BEGIN (INITIAL); + yylval->s = new std::string (yyextra->tmp); + return STRING; +} + +\\n yyextra->tmp += '\n'; +\\r yyextra->tmp += '\r'; +\\t yyextra->tmp += '\t'; +\\(.|\n) yyextra->tmp += yytext[1]; +. yyextra->tmp += yytext[0]; +\n { + yylval->c = yytext[0]; + return UNKNOWN; +} + + +%% + +/* vim:ft=lex: +*/ diff --git a/i/marvin/src/config/parser.yy b/i/marvin/src/config/parser.yy new file mode 100644 index 0000000..1aeb986 --- /dev/null +++ b/i/marvin/src/config/parser.yy @@ -0,0 +1,175 @@ +%{ +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 YYPARSE_PARAM scanner +#define YYLEX_PARAM scanner + +#include "config/config.hh" +#include "config/config_data.hh" +#include "config/parser_extra.hh" + +// Fichiers d'en-tête générés. +#include "parser.hh" +#include "lexer.hh" + +#include // XXX à virer. + +void yyerror (const char *); + +#undef yyextra +#define yyextra (yyget_extra (scanner)) + +%} + +%error-verbose +%pure-parser +%defines +%union { + char c; + int i; + double f; + bool b; + std::string *s; + struct { + any *a; + Config::IntList *il; + Config::FloatList *fl; + Config::StringList *sl; + } a; +} + +%token UNKNOWN +%token INT +%token FLOAT +%token ID STRING +%token BOOLEAN +%token IMG DATA + +%type int_list +%type float_list +%type string_list + +%destructor { delete $$; } ID STRING +%destructor { delete $$.a; } int_list +%destructor { delete $$.a; } float_list +%destructor { delete $$.a; } string_list + +%% + +input: + /* Nothing. */ + | confitem + | input '\n' confitem + | input '\n' +; + +confitem: + ID '=' BOOLEAN { + yyextra->configData.add (*$1, $3); + delete $1; + } + | ID '=' INT { + yyextra->configData.add (*$1, $3); + delete $1; + } + | ID '=' FLOAT { + yyextra->configData.add (*$1, $3); + delete $1; + } + | ID '=' STRING { + yyextra->configData.add (*$1, *$3); + delete $1; + delete $3; + } + | ID '=' '(' int_list ')' { + yyextra->configData.add (*$1, *$4.a); + delete $1; + delete $4.a; + } + | ID '=' '(' float_list ')' { + yyextra->configData.add (*$1, *$4.a); + delete $1; + delete $4.a; + } + | ID '=' '(' string_list ')' { + yyextra->configData.add (*$1, *$4.a); + delete $1; + delete $4.a; + } +; + +int_list: + INT { + $$.a = new any (Config::IntList ()); + $$.il = any_cast ($$.a); + $$.il->push_back ($1); + } + | int_list INT { + $1.il->push_back ($2); + $$ = $1; + } +; + +float_list: + FLOAT { + $$.a = new any (Config::FloatList ()); + $$.fl = any_cast ($$.a); + $$.fl->push_back ($1); + } + | float_list FLOAT { + $1.fl->push_back ($2); + $$ = $1; + } +; + +string_list: + STRING { + $$.a = new any (Config::StringList ()); + $$.sl = any_cast ($$.a); + $$.sl->push_back (*$1); + delete $1; + } + | string_list STRING { + $1.sl->push_back (*$2); + $$ = $1; + delete $2; + } +; + +%% + +void yyerror (const char *e) +{ + std::cerr << e << std::endl; +} + +/* Shut up warning for this wrongly declared static function. */ +static int +yy_init_globals (yyscan_t yyscanner) +{ + return yy_init_globals (yyscanner); +} + +/* vim:ft=yacc: +*/ diff --git a/i/marvin/src/config/parser_extra.hh b/i/marvin/src/config/parser_extra.hh new file mode 100644 index 0000000..aa5f80f --- /dev/null +++ b/i/marvin/src/config/parser_extra.hh @@ -0,0 +1,47 @@ +#ifndef parser_extra_hh +#define parser_extra_hh +// parser_extra.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include + +class ConfigData; + +/// Structure d'information passée au parser. +struct ParserExtra +{ + /// Chaîne temporaire utilisée pendant l'analyse lexicale. + std::string tmp; + /// Référence vers le ConfigData qui doit recevoir la configuration. + ConfigData &configData; + public: + /// Constructeur pour initialiser la référence. + ParserExtra (ConfigData &configData_) + : configData (configData_) + { } +}; + +#define YY_EXTRA_TYPE ParserExtra * + +#endif // parser_extra_hh diff --git a/i/marvin/src/config/test_config_data.cc b/i/marvin/src/config/test_config_data.cc new file mode 100644 index 0000000..052a4c8 --- /dev/null +++ b/i/marvin/src/config/test_config_data.cc @@ -0,0 +1,45 @@ +// test_config_data.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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.hh" + +#include +#include + +int +main (int argc, char **argv) +{ + try + { + Config cd (argc, argv); + for (int i = 1; i < argc; ++i) + std::cout << cd.get (argv[i]) << std::endl; + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + return 1; + } + return 0; +} diff --git a/i/marvin/src/data/Makefile.defs b/i/marvin/src/data/Makefile.defs new file mode 100644 index 0000000..31698f1 --- /dev/null +++ b/i/marvin/src/data/Makefile.defs @@ -0,0 +1,12 @@ +PROGRAMS += test_data test_data_buffer + +data_OBJECTS = data_input.o data_input_file.o data_input_zlib.o \ + data_buffer.o data_output.o -lz + +test_data_OBJECTS = test_data.o $(data_OBJECTS) + +test_data_buffer_OBJECTS = test_data_buffer.o data_buffer.o + +test_data: $(test_data_OBJECTS) + +test_data_buffer: $(test_data_buffer_OBJECTS) diff --git a/i/marvin/src/data/data_buffer.cc b/i/marvin/src/data/data_buffer.cc new file mode 100644 index 0000000..c0d8cef --- /dev/null +++ b/i/marvin/src/data/data_buffer.cc @@ -0,0 +1,130 @@ +// data_buffer.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Dufour Jérémy +// +// 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 "data_buffer.hh" + +#include // std::memcpy +#include // std::runtime_error +#include // std::swap + +static const unsigned min_size = 10; + +/// Constructeur par defaut +DataBuffer::DataBuffer (void) + : size_ (min_size), headPos_ (0), tailPos_ (0), type_ (NoType) +{ + buffer_ = new uint8_t [size_]; +} + +/// Constructeur par recopie. +DataBuffer::DataBuffer (const DataBuffer &d) + : headPos_ (0), type_ (NoType) +{ + unsigned sizeUse = d.tailPos_ - d.headPos_; + size_ = tailPos_ = sizeUse; + type_ = d.type_; + buffer_ = new uint8_t [size_]; + std::memcpy (buffer_, &d.buffer_[headPos_], sizeUse); +} + +/// Constructeur avec données. +DataBuffer::DataBuffer (uint8_t *data, unsigned size, unsigned sizeAllocated, + dataType_e type) + : size_ (sizeAllocated), headPos_ (0), tailPos_ (size), type_ (type) +{ + if (!data || size > sizeAllocated) + throw std::invalid_argument ("Paramêtres invalides"); + buffer_ = new uint8_t [size_]; + std::memcpy (buffer_, data, tailPos_); +} + +/// Surcharge de l'opérateur d'affectation +DataBuffer & +DataBuffer::operator= (const DataBuffer &d) +{ + DataBuffer b (d); + b.swap (*this); + return *this; +} + +/// Lit des données. Même convention que le read de la libc, mais lance +/// une exception en cas d'erreur. +unsigned +DataBuffer::read (uint8_t *buf, unsigned size) +{ + if (!buf) + throw std::invalid_argument ("Paramêtres invalides"); + unsigned sizeTmp = tailPos_ - headPos_; + // On regarde combien de données peuvent être lues + if (size < sizeTmp) + sizeTmp = size; + std::memcpy (buf, &buffer_[headPos_], sizeTmp); + headPos_ += sizeTmp; + // Si il n'y a plus de données dans le buffer, on recommence de puis 0 + if (headPos_ == tailPos_) + headPos_ = tailPos_ = 0; + return sizeTmp; +} + +/// Écrit des données. Même convention que le write de la libc, mais lance +/// une exception en cas d'erreur. +void +DataBuffer::write (const uint8_t *buf, unsigned size) +{ + // Vérification des paramêtres + if (!buf) + throw std::invalid_argument ("Parametres invalides"); + // Agrandissement du buffer + grow (size); + // Ecriture des données + std::memcpy (&buffer_[tailPos_], buf, size); + tailPos_ += size; +} + +/// Augmente la taille du buffer pour correspondre à size. +void +DataBuffer::grow (const unsigned size) +{ + // Taille restant non utilisé dans le buffer + unsigned tailleLibre = size_ - tailPos_; + // Si la taille du buffer n'est pas suffisante + if (tailleLibre < size) + { + // On augmente de deux fois la taille + DataBuffer d (&buffer_[headPos_], tailPos_ - headPos_, + 2 * size + size_, type_); + d.swap (*this); + } +} + +/// Echange les buffers. +void +DataBuffer::swap (DataBuffer &d) +{ + std::swap (buffer_, d.buffer_); + std::swap (size_, d.size_); + std::swap (headPos_, d.headPos_); + std::swap (tailPos_, d.tailPos_); + std::swap (type_, d.type_); +} diff --git a/i/marvin/src/data/data_buffer.hh b/i/marvin/src/data/data_buffer.hh new file mode 100644 index 0000000..3d2201d --- /dev/null +++ b/i/marvin/src/data/data_buffer.hh @@ -0,0 +1,81 @@ +#ifndef data_buffer_hh +#define data_buffer_hh +// data_buffer.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Dufour Jérémy +// +// 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. +// +// Contact : +// Web: %WEB% +// Email: +// }}} + +#include "data_input.hh" +#include "data_output.hh" + +#include // uint8_t + +/// Classe contenant un buffer de données accessible en lecture et en +/// écriture. +class DataBuffer : public DataInput, public DataOutput +{ + public: + /// Type de données possible du buffer. + enum dataType_e { + NoType, + Image, + AskImage + }; + private: + /// Buffer de données. + uint8_t *buffer_; + /// Taille alloué en mémoire pour le buffer. + unsigned size_; + /// Position du début et de la fin des données dans le buffer. + unsigned headPos_, tailPos_; + /// Type de données du buffer. + dataType_e type_; + /// Augmente la taille du buffer pour correspondre à size. + void grow (const unsigned size); + /// Echange les buffers. + void swap (DataBuffer &d); + public: + /// Constructeur par défaut. + DataBuffer (void); + /// Constructeur par recopie. + DataBuffer (const DataBuffer &d); + /// Constructeur avec données. + DataBuffer (uint8_t *data, unsigned size, unsigned sizeAllocated, + dataType_e type = NoType); + /// Destructeur par défaut. + ~DataBuffer (void) { delete [] buffer_; } + /// Surcharge de l'opérateur d'affectation + DataBuffer &operator= (const DataBuffer &d); + /// Lit des données. Même convention que le read de la libc, mais lance + /// une exception en cas d'erreur. + unsigned read (uint8_t *buf, unsigned size); + /// Écrit des données. Même convention que le write de la libc, mais lance + /// une exception en cas d'erreur. + void write (const uint8_t *buf, unsigned size); + /// Taille utile du buffer. + unsigned size (void) const { return tailPos_ - headPos_; } + /// Type de données du buffer. + dataType_e type (void) const { return type_; } + /// Change le type de données du buffer. + void setType (const dataType_e type) { type_ = type; } +}; +#endif // data_buffer_hh diff --git a/i/marvin/src/data/data_input.cc b/i/marvin/src/data/data_input.cc new file mode 100644 index 0000000..cfc9b2a --- /dev/null +++ b/i/marvin/src/data/data_input.cc @@ -0,0 +1,37 @@ +// data_input.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "data_input.hh" + +/// Lit un caractère. Renvois -1 en cas de fin de fichier. +int +DataInput::getc (void) +{ + uint8_t c; + if (read (&c, 1) == 1) + return c & 0xff; + else + return -1; +} + diff --git a/i/marvin/src/data/data_input.hh b/i/marvin/src/data/data_input.hh new file mode 100644 index 0000000..ae59366 --- /dev/null +++ b/i/marvin/src/data/data_input.hh @@ -0,0 +1,44 @@ +#ifndef data_input_hh +#define data_input_hh +// data_input.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 + +/// Classe d'entrée de données. C'est une sorte de istream simplifié +/// permettant d'accéder à des données en lecture. +class DataInput +{ + public: + /// Destructeur. + virtual ~DataInput (void) { } + /// Lit des données. Même convention que le read de la libc, mais lance + /// une exception en cas d'erreur. + virtual unsigned read (uint8_t *buf, unsigned size) = 0; + /// Lit un caractère. Renvois -1 en cas de fin de fichier. + int getc (void); +}; + +#endif // data_input_hh diff --git a/i/marvin/src/data/data_input_file.cc b/i/marvin/src/data/data_input_file.cc new file mode 100644 index 0000000..75b7500 --- /dev/null +++ b/i/marvin/src/data/data_input_file.cc @@ -0,0 +1,46 @@ +// data_input_file.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "data_input_file.hh" +#include "utils/errno_exception.hh" + +/// Constructeur. +DataInputFile::DataInputFile (const std::string &name) + : name_ (name), file_ (name.c_str ()) +{ + if (!file_) + throw errno_exception (name_, errno); +} + +/// Lit des données. Même convention que le read de la libc, mais lance +/// une exception en cas d'erreur. +unsigned +DataInputFile::read (uint8_t *buf, unsigned size) +{ + file_.read (reinterpret_cast (buf), size); + if (file_.bad ()) + throw errno_exception (name_, errno); + return file_.gcount (); +} + diff --git a/i/marvin/src/data/data_input_file.hh b/i/marvin/src/data/data_input_file.hh new file mode 100644 index 0000000..29d22ca --- /dev/null +++ b/i/marvin/src/data/data_input_file.hh @@ -0,0 +1,45 @@ +#ifndef data_input_file_hh +#define data_input_file_hh +// data_input_file.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "data_input.hh" + +#include +#include + +/// Data depuis un fichier standard. +class DataInputFile : public DataInput +{ + std::string name_; + std::ifstream file_; + public: + /// Constructeur. + DataInputFile (const std::string &name); + /// Lit des données. Même convention que le read de la libc, mais lance + /// une exception en cas d'erreur. + unsigned read (uint8_t *buf, unsigned size); +}; + +#endif // data_input_file_hh diff --git a/i/marvin/src/data/data_input_zlib.cc b/i/marvin/src/data/data_input_zlib.cc new file mode 100644 index 0000000..f8f5322 --- /dev/null +++ b/i/marvin/src/data/data_input_zlib.cc @@ -0,0 +1,84 @@ +// data_input_zlib.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "data_input_zlib.hh" + +#include + +/// Constructeur. +DataInputZlib::DataInputZlib (DataInput &data) + : data_ (data), inBuf_ (0) +{ + // Allocate input buffer. + inBuf_ = new uint8_t[inBufSize_]; + // Initialise the zlib structure. + zStream_.next_in = 0; + zStream_.avail_in = 0; + zStream_.next_out = 0; + zStream_.avail_out = 0; + zStream_.zalloc = 0; + zStream_.zfree = 0; + zStream_.opaque = 0; + if (inflateInit2 (&zStream_, 32 + 15) != Z_OK) + { + delete[] inBuf_; + throw std::runtime_error (zStream_.msg); + } +} + +/// Destructeur. +DataInputZlib::~DataInputZlib (void) +{ + inflateEnd (&zStream_); + delete[] inBuf_; +} + +/// Lit des données. Même convention que le read de la libc, mais lance +/// une exception en cas d'erreur. +unsigned +DataInputZlib::read (uint8_t *buf, unsigned size) +{ + // Set output buffer. + zStream_.next_out = buf; + zStream_.avail_out = size; + // While not satisfied. + while (zStream_.avail_out) + { + // Feed more data. + if (zStream_.avail_in == 0) + { + zStream_.next_in = inBuf_; + zStream_.avail_in = data_.read (inBuf_, inBufSize_); + } + // Uncompress. + int r = inflate (&zStream_, Z_SYNC_FLUSH); + // Check result. + if (r == Z_STREAM_END) + return size - zStream_.avail_out; + else if (r != Z_OK) + throw std::runtime_error (zStream_.msg); + } + return size; +} + diff --git a/i/marvin/src/data/data_input_zlib.hh b/i/marvin/src/data/data_input_zlib.hh new file mode 100644 index 0000000..0b7cf5f --- /dev/null +++ b/i/marvin/src/data/data_input_zlib.hh @@ -0,0 +1,48 @@ +#ifndef data_input_zlib_hh +#define data_input_zlib_hh +// data_input_zlib.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "data_input.hh" + +#include + +/// Data depuis un data compressé en zlib. +class DataInputZlib : public DataInput +{ + DataInput &data_; + uint8_t *inBuf_; + static const unsigned inBufSize_ = 1024; + z_stream zStream_; + public: + /// Constructeur. + DataInputZlib (DataInput &data); + /// Destructeur. + ~DataInputZlib (void); + /// Lit des données. Même convention que le read de la libc, mais lance + /// une exception en cas d'erreur. + unsigned read (uint8_t *buf, unsigned size); +}; + +#endif // data_input_zlib_hh diff --git a/i/marvin/src/data/data_output.cc b/i/marvin/src/data/data_output.cc new file mode 100644 index 0000000..ed04e97 --- /dev/null +++ b/i/marvin/src/data/data_output.cc @@ -0,0 +1,34 @@ +// data_output.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "data_output.hh" + +/// Écrit un caractère. +void +DataOutput::putc (int c) +{ + uint8_t uc = c; + write (&uc, 1); +} + diff --git a/i/marvin/src/data/data_output.hh b/i/marvin/src/data/data_output.hh new file mode 100644 index 0000000..7141892 --- /dev/null +++ b/i/marvin/src/data/data_output.hh @@ -0,0 +1,44 @@ +#ifndef data_output_hh +#define data_output_hh +// data_output.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 + +/// Classe de sortie de données. C'est une sorte de ostream simplifié +/// permettant d'accéder à des données en écriture. +class DataOutput +{ + public: + /// Destructeur. + virtual ~DataOutput (void) { } + /// Écrit des données. Même convention que le write de la libc, mais lance + /// une exception en cas d'erreur. + virtual void write (const uint8_t *buf, unsigned size) = 0; + /// Écrit un caractère. + void putc (int c); +}; + +#endif // data_output_hh diff --git a/i/marvin/src/data/test_data.cc b/i/marvin/src/data/test_data.cc new file mode 100644 index 0000000..acc57ac --- /dev/null +++ b/i/marvin/src/data/test_data.cc @@ -0,0 +1,56 @@ +// test_data.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "data_input_file.hh" +#include "data_input_zlib.hh" + +#include +#include +#include + +int +main (int argc, char **argv) +{ + try + { + if (argc != 2) + throw std::runtime_error ("Syntax: test_data file"); + DataInputFile f (argv[1]); + DataInputZlib z (f); + uint8_t buf[1024]; + int r = z.read (buf, 1024); + while (r != 0) + { + write (1, buf, r); + r = z.read (buf, r); + } + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + return 1; + } + return 0; +} + diff --git a/i/marvin/src/data/test_data_buffer.cc b/i/marvin/src/data/test_data_buffer.cc new file mode 100644 index 0000000..36095ab --- /dev/null +++ b/i/marvin/src/data/test_data_buffer.cc @@ -0,0 +1,77 @@ +// test_data_buffer.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Dufour Jérémy +// +// 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. +// +// Contact : +// Web: %WEB% +// Email: +// }}} + +#include "data_buffer.hh" + +#include +#include +#include + +void +check_data (uint8_t *b1, uint8_t *b2, unsigned size) +{ + for (unsigned i = 0; i < size; i ++) + if (b1[i] != b2[i]) + throw std::runtime_error ("Erreur, buffers invalides"); +} + +int +main (int argc, char **argv) +{ + try + { + DataBuffer d; + uint8_t bufW[100]; + uint8_t bufR[150]; + // Remplissage des buffers de test + memset (bufW, 42, 100); + memset (bufR, 1, 150); + // On en écrit 50 + d.write (bufW, 50); + // On en lit 42 + if (d.read (bufR, 42) == 42) + check_data (bufW, bufR, 42); + else + throw std::runtime_error ("Erreur de taille de données"); + // On en re écrit 50 + d.write (&bufW[50], 50); + unsigned pos = 42; + unsigned status; + // On lit jusqu'à la fin + do + { + status = d.read (&bufR[pos], 10); + if (status) + check_data (&bufW[pos], &bufR[pos], status); + pos += status; + } while (status); + // On vérifie que tout a été bien lue + } + catch (const std::runtime_error &r) + { + std::cerr << r.what () << std::endl; + return 1; + } + return 0; +} diff --git a/i/marvin/src/log/Makefile.defs b/i/marvin/src/log/Makefile.defs new file mode 100644 index 0000000..8462793 --- /dev/null +++ b/i/marvin/src/log/Makefile.defs @@ -0,0 +1,7 @@ +PROGRAMS += test_log + +log_OBJECTS = log.o log_message.o + +test_log_OBJECTS = test_log.o $(log_OBJECTS) + +test_log: $(test_log_OBJECTS) diff --git a/i/marvin/src/log/log.cc b/i/marvin/src/log/log.cc new file mode 100644 index 0000000..1818b99 --- /dev/null +++ b/i/marvin/src/log/log.cc @@ -0,0 +1,102 @@ +// log.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "log.hh" + +/// Constructeur. +Log::Log (const char *module) + : module_ (module), instance_ (0) +{ +} + +/// Constructeur. +Log::Log (const char *module, const char *instance) + : module_ (module), instance_ (instance) +{ +} + +/// Crée un nouveau LogMessage. +LogMessage +Log::operator() (const char *msg, Level level/*info*/) const +{ + return LogMessage (*this, msg, level); +} + +/// Traduit le niveau de log. +std::string +Log::toString (Level level) +{ + switch (level) + { + case none: + return "none"; + case fatal: + return "fatal"; + case error: + return "error"; + case warning: + return "warning"; + case info: + return "info"; + case debug: + return "debug"; + case verydebug: + return "verydebug"; + default: + return "logunknown"; + } +} + +/// Traduit le niveau de log. +Log::Level +Log::toLevel (const std::string &level) +{ + switch (level[0]) + { + case 'n': + return none; + case 'f': + return fatal; + case 'e': + return error; + case 'w': + return warning; + case 'i': + return info; + case 'd': + return debug; + case 'v': + return verydebug; + default: + return levelReserved; + } +} + +/// Change un niveau de log en masque. +Log::Level +Log::toLevelMask (Log::Level level) +{ + return static_cast ((level - 1) | level); +} + diff --git a/i/marvin/src/log/log.hh b/i/marvin/src/log/log.hh new file mode 100644 index 0000000..387a2c4 --- /dev/null +++ b/i/marvin/src/log/log.hh @@ -0,0 +1,70 @@ +#ifndef log_hh +#define log_hh +// log.h +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include + +class LogMessage; + +/// Classe de log. Permet de construire des LogMessage. +class Log +{ + public: + enum Level + { + none = 0x00, + fatal = 0x01, + error = 0x02, + warning = 0x04, + info = 0x08, + debug = 0x10, + verydebug = 0x20, + levelReserved = 0xffff + }; + private: + const char *module_; + const char *instance_; + public: + /// Constructeur. + Log (const char *module); + /// Constructeur. + Log (const char *module, const char *instance); + /// Crée un nouveau LogMessage. + LogMessage operator() (const char *msg, Level level = info) const; + /// Récupère le module. + const char *getModule (void) const { return module_; } + /// Récupère l'instance. + const char *getInstance (void) const { return instance_; } + /// Traduit le niveau de log. + static std::string toString (Level level); + /// Traduit le niveau de log. + static Level toLevel (const std::string &level); + /// Change un niveau de log en masque. + static Level toLevelMask (Level level); +}; + +#include "log_message.hh" + +#endif // log_hh diff --git a/i/marvin/src/log/log_message.cc b/i/marvin/src/log/log_message.cc new file mode 100644 index 0000000..4a3af9a --- /dev/null +++ b/i/marvin/src/log/log_message.cc @@ -0,0 +1,75 @@ +// log_message.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "log_message.hh" + +#include + +/// Constructeur. +LogMessage::LogMessage (const Log &log, const char *msg, Log::Level level) +{ + std::cout << log.getModule () << ':'; + if (log.getInstance ()) + std::cout << ' ' << log.getInstance () << ':'; + std::cout << ' ' << msg; +} + +/// Destructeur. +LogMessage::~LogMessage (void) +{ + std::cout << std::endl; +} + +/// Output a string or a variable name. +LogMessage & +LogMessage::operator<< (const char *s) +{ + std::cout << ' ' << s; + return *this; +} + +/// Output a string or a variable name. +LogMessage & +LogMessage::operator<< (const std::string &s) +{ + std::cout << ' ' << s; + return *this; +} + +/// Output a integer. +LogMessage & +LogMessage::operator<< (int i) +{ + std::cout << ' ' << i; + return *this; +} + +/// Output a double. +LogMessage & +LogMessage::operator<< (double d) +{ + std::cout << ' ' << d; + return *this; +} + diff --git a/i/marvin/src/log/log_message.hh b/i/marvin/src/log/log_message.hh new file mode 100644 index 0000000..e5a9e20 --- /dev/null +++ b/i/marvin/src/log/log_message.hh @@ -0,0 +1,52 @@ +#ifndef log_message_hh +#define log_message_hh +// log_message.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "log.hh" + +class LogMessage +{ + public: + /// Constructeur. + LogMessage (const Log &log, const char *msg, Log::Level level); + /// Destructeur. + ~LogMessage (void); + /// Output a string or a variable name. + LogMessage &operator<< (const char *s); + /// Output a string or a variable name. + LogMessage &operator<< (const std::string &s); + /// Output a integer. + LogMessage &operator<< (int i); + /// Output a double. + LogMessage &operator<< (double d); + /// Output a OutputStreamable as a string. + template + LogMessage &operator<< (const OutputStreamable &o); +}; + +#include "log_message.tcc" + +#endif // log_message_hh diff --git a/i/marvin/src/log/log_message.tcc b/i/marvin/src/log/log_message.tcc new file mode 100644 index 0000000..98ac01b --- /dev/null +++ b/i/marvin/src/log/log_message.tcc @@ -0,0 +1,36 @@ +// log_message.tcc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 + +/// Output a OutputStreamable as a string. +template +LogMessage & +LogMessage::operator<< (const OutputStreamable &o) +{ + std::cout << ' ' << o; + return *this; +} + diff --git a/i/marvin/src/log/test_log.cc b/i/marvin/src/log/test_log.cc new file mode 100644 index 0000000..0cd7d42 --- /dev/null +++ b/i/marvin/src/log/test_log.cc @@ -0,0 +1,34 @@ +// test_log.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "log.hh" + +int +main (void) +{ + Log log ("main"); + log ("foo") << "bar" << 4 << "foobar" << 5.6; + log ("bar", Log::error) << "foo" << 5; + return 0; +} diff --git a/i/marvin/src/motor/Makefile.defs b/i/marvin/src/motor/Makefile.defs new file mode 100644 index 0000000..51c658c --- /dev/null +++ b/i/marvin/src/motor/Makefile.defs @@ -0,0 +1,10 @@ +PROGRAMS += test_motor + +motor_OBJECTS = motor.o + +test_motor_OBJECTS = test_motor.o $(motor_OBJECTS) $(timer_OBJECTS) \ + $(config_OBJECTS) $(serial_OBJECTS) \ + $(asserv_OBJECTS) $(proto_OBJECTS) $(log_OBJECTS) \ + $(utils_OBJECTS) + +test_motor: $(test_motor_OBJECTS) diff --git a/i/marvin/src/motor/motor.cc b/i/marvin/src/motor/motor.cc new file mode 100644 index 0000000..c292e9c --- /dev/null +++ b/i/marvin/src/motor/motor.cc @@ -0,0 +1,306 @@ +// motor.cc +// robert - programme du robot 2005 {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// 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.hh" + +#include + +/// Constructeur +Motor::Motor (const Config & config) + :asserv_(config, *this), log_("Motor"),idle_(true), doneDone_(false), + f0Sended_(false), pinUpdated_(false) +{ + // XXX Quant est-il des positions de départ du robot??? + pStatPosition_ = config.get("motor.pStatPosition"); + pStatMotor_ = config.get("motor.pStatMotor"); + posX_ = 65; + posY_ = -225; + posA_ = 0; + asserv_.setXPos(posX_); + asserv_.setYPos(posY_); + asserv_.setAngle(posA_); +} + +/// Initialise les moteurs +void Motor::init (void) +{ + // on reset la carte + asserv_.reset(); + // on règle le rafraichissement des positions + asserv_.statPosition(pStatPosition_); + asserv_.statMotor(pStatMotor_); + //log_ ("initialisation") << "Etat" << "Terminée"; +} + +// Arrête les moteurs +void Motor::stop(void) +{ + asserv_.setSpeed(); + idle_ = true; +} + +double Motor::getX(void) +{ + return posX_; +} + +double Motor::getY(void) +{ + return posY_; +} + +double Motor::getA(void) +{ + return posA_; +} + +void Motor::setPosition(double x, double y, double a) //XXX Faire trois setteur +{ + asserv_.setXPos(x); + asserv_.setYPos(y); + asserv_.setAngle(a); + posX_ = x; + posY_ = y; + posA_ = a; +} + +void +Motor::setPwm(int leftPwm, int rightPwm) +{ + asserv_.setPwm (leftPwm, rightPwm); +} + +void Motor::goTo(double x, double y, double a) +{ + + //Détermination de la distance à parcourir + double dist = sqrt((x - posX_) * (x- posX_) + (y - posY_) * (y - posY_)); + //Détermination de l'angle qui va bien + double angle = acos ((x - posX_) / dist); + if((y - posY_) < 0) + angle = -angle; + + // On remplie la file + fOrdre_.push(ordre(true, dist)); + fOrdre_.push(ordre(false, a)); + + // On execute la première commande + idle_ = false; + rotation(angle); +} + +void Motor::recalage(void) +{ + asserv_.fuckTheWall(-3); // XXX Mettre l'arg en conf + //log_ ("recalage") << "Vitesse" << "-3"; + idle_ = false; +} + +bool Motor::idle(void) +{ + return idle_; +} + +void Motor::linearMove(double d) +{ + asserv_.linearMove(d); + //log_ ("linearMove") << "Valeur" << d; + idle_ = false; +} + +void Motor::rotation(double newA) +{ + asserv_.angularMove(newA); + //log_ ("rotation") << "Valeur" << newA; + idle_ = false; +} + +void Motor::setMaxSpeed(int maxLspeed, int maxRspeed) +{ + asserv_.setMaxSpeed(maxLspeed, maxRspeed); +} + +void Motor::setAccel(int accel) +{ + asserv_.setAccel(accel); +} + +int Motor::getMaxLSpeed(void) +{ + return asserv_.getMaxLSpeed(); +} + +bool Motor::sync(void) +{ + //log_("sync") << "Entré dans sync" << ""; + + // On regarde si toutes les commandes ont été envoyées et aquittées + if (asserv_.sync()) + { + //log_("sync") << "Toutes les commandes ont été envoyés et aquitté" << ""; + + if(doneDone_) // Si on a reçu des F depuis le dernier sync + { + //log_("sync") << "On a des F" << ""; + if(!f0Sended_) // Si on avait pas envoyé de f0 + { + //log_("sync") << "On envoie le F0" << ""; + asserv_.finishAck(); + f0Sended_ = true; + } + else // Si l'AVR vient d'aquitter le F0 + { + //log_("sync") << "On a déjà envoyé le F0, on s'en fout" << ""; + doneDone_ = false; + } + } + else // Si on a pas reçu de F + { + //log_("sync") << "On a pas reçu de F" << ""; + if(f0Sended_) // On peut envoyer une nouvelle commande + { + //log_("sync") << "Pret pour un nouveau travail" << ""; + if(fOrdre_.empty()) //Si la pile est vide + { + //log_("sync") << "On repasse en Idle" << ""; + idle_ = true; // Les moteurs sont idle + } + else + { + //log_("sync") << "Prochaine commande de la pile" << ""; + if(fOrdre_.front().ordre_) // Linear Move + linearMove(fOrdre_.front().arg_); + else + rotation(fOrdre_.front().arg_); + fOrdre_.pop(); + } + f0Sended_ = false; + } + // else + //log_("sync") << "Le Robot est en activité/pause, on y touche pas" << ""; + } + return true; + } + return false; +} + +void Motor::wait(int timeout) +{ + asserv_.wait(timeout); +} + +/// Récupère le File Descriptor +int Motor::getFd(void) +{ + return asserv_.getFd(); +} + +bool Motor::jackState(void) +{ + pinUpdated_ = false; +/* asserv_.statInPort(8); /// XXX La période est défini comment?? (en dur, et c'est 8 période d'asserv) + while(!pinUpdated_) + { + sync(); + } + /// On fait fermer sa gueule à l'AVR sur l'epineux sujet du port d'entrée + asserv_.statInPort(); */ + int temp = pinState_ & 0x0040; // XXX Ouais à tester + if(temp == 0x0040) // si le jack est retiré + return true; + else + return false; +} + +bool Motor::colorState (void) +{ + pinUpdated_ = false; +/* asserv_.statInPort(1); /// XXX La période est défini comment?? (en dur, et c'est 8 période d'asserv) + while(!pinUpdated_) + { + sync(); + } */ + int temp = pinState_ & 0x0002; // XXX Ouais à tester + if(temp == 0x0002) // si le jack est retiré + return true; + else + return false; + /// On fait fermer sa gueule à l'AVR sur l'epineux sujet du port d'entrée + //asserv_.statInPort(); +} + +void Motor::receiveCounter (double lMotor, double rMotor) +{ +} + +void Motor::receivePosX (double xPos) +{ + posX_ = xPos; +} + +void Motor::receivePosY (double yPos) +{ + posY_ = yPos; +} + +void Motor::receivePosA (double aPos) +{ + posA_ = aPos; +} + +void Motor::receiveSpeedStat (int leftError, int leftInt, int rightError, + int rightInt) +{ +} + +void Motor::receivePwm (double leftPwm, double rightPwm) +{ +} + +void Motor::receiveTiming (int motorTimer4, + int motorTimer3, int motorTimer2, + int motorTimer1, int motorTimer0) +{ +} + +void Motor::receiveInPort (int port) +{ + pinState_ = port; + pinUpdated_ = true; +} + +void Motor::receiveSharp (int sharp1, int sharp2, int sharp3) +{ +} + +void Motor::receiveTazState(int state, int subState) +{ +} + +void Motor::done (void) +{ + //log_("done") << "Passage dans done" << ""; + doneDone_ = true; +} diff --git a/i/marvin/src/motor/motor.hh b/i/marvin/src/motor/motor.hh new file mode 100644 index 0000000..b527525 --- /dev/null +++ b/i/marvin/src/motor/motor.hh @@ -0,0 +1,128 @@ +#ifndef motor_hh +#define motor_hh +// motor.hh +// robert - programme du robot 2005 {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// 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.hh" +#include "asserv/asserv.hh" + +#include + +/// Gère les moteurs de déplacement du robot +class Motor : public Asserv::Receiver +{ + struct ordre + { + // Ordre (true pour linéaire, false pour rotation + bool ordre_; + double arg_; + // Constructeur + ordre(bool ordre, double arg) { ordre_ = ordre; arg_ = arg; } + }; + private: + /// Communication avec l'asservissement + Asserv asserv_; + /// Système de log + Log log_; + /// Pile de commande + std::queue fOrdre_; + /// position + double posX_; + double posY_; + double posA_; + double speed_; + /// Etat des commandes + bool idle_; + /// Drapeau pour quand on a reçu un F + bool doneDone_; + /// Drapeau levé quand on a envoyé un "Ta Gueule" + bool f0Sended_; + /// Paramètre de conf + int pStatPosition_; + int pStatMotor_; + /// Etat du port d'entrée + int pinState_; + bool pinUpdated_; + + public: + /// Constructeur + Motor (const Config & config); + /// Initialise les moteurs + void init(void); + ///Arrête les moteurs + void stop(void); + /// Renvoie la position X + double getX(void); + /// Renvoie la position Y + double getY(void); + /// Renvoie l'angle A + double getA(void); + /// On set les positions dans l'avr + void setPosition(double x, double y, double a); + /// Ammène le robot à la position x,y + void goTo(double x, double y, double a); + /// Recale le robot + void recalage(void); + /// Indique si les moteurs sont occupé ou non + bool idle (void); + /// Execute un déplacement linéaire + void linearMove(double d); + /// Execute une rotation(argument en radian) + void rotation(double newA); + /// set de la vitesse des moteurs + void setMaxSpeed(int maxLspeed, int maxRspeed); + /// set de l'acceleration des moteurs + void setAccel(int accel); + /// récupère la vitesse linéaire maximum + int getMaxLSpeed(void); + /// Syncronisation + bool sync(void); + /// On attend... + void wait(int timeout); + /// Récupère le File Descriptor + int getFd(void); + /// Retoure l'état du jack (false entrée et true sortie) + bool jackState(void); + /// Retourne la couleur selectionné + bool colorState (void); + /// déclaration des fonctions de receiver + void receiveCounter (double lMotor, double rMotor); + void receivePosX (double xPos); + void receivePosY (double yPos); + void receivePosA (double aPos); + void receiveSpeedStat (int leftError, int leftInt, int rightError, + int rightInt); + void receivePwm (double leftPwm, double rightPwm); + // Défini les valeurs de la PWM + void setPwm(int leftPwm = 0, int rightPwm = 0); + void receiveTiming (int motorTimer4, + int motorTimer3, int motorTimer2, + int motorTimer1, int motorTimer0); + void receiveInPort (int port); + void receiveSharp (int sharp1, int sharp2, int sharp3); + void receiveTazState(int state, int subState); // XXX Vérifier les formats + void done (void); +}; +#endif // motor.hh diff --git a/i/marvin/src/motor/test_motor.cc b/i/marvin/src/motor/test_motor.cc new file mode 100644 index 0000000..137e355 --- /dev/null +++ b/i/marvin/src/motor/test_motor.cc @@ -0,0 +1,163 @@ +// test_motor.cc +// robert - programme du robot 2005 {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// 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.hh" +#include "motor/motor.hh" +#include "timer/timer.hh" + +#include + +/// Affiche un memo de suntaxe. + void +syntax (void) +{ + std::cout << "test_motor - test la classe motor.\n" + "Syntaxe : test_motor <...>\n" + " s envois une commande\n" + " w attend pendant un nombre de millisecondes\n" + " ? affiche cet écran d'aide\n" + << std::endl; +} + + int +main (int argc, char **argv) +{ + try + { + int i; + if (argc < 2) + { + syntax (); + return 1; + } + Config config(argc, argv); + Motor motor(config); + i = 1; + while (i < argc) + { + switch (argv[i][0]) + { + case 's': + { + switch(argv[++i][0]) + { + case 'z': + motor.init(); + break; + case 's': + motor.stop(); + break; + case 'P': + std::cout << "position:\n" << + "X: " << motor.getX() << + " Y: " << motor.getY() << + " A: " << motor.getA() << std::endl; + break; + case 'g': + if(++i + 2 > argc) + throw std::runtime_error("syntax error"); + motor.goTo(strtod(argv[i], 0), + strtod(argv[i + 1], 0), + strtod(argv[i + 2], 0)); + i += 2; + break; + case 'r': + motor.recalage(); + break; + case 'I': + std::cout << "Moteur idle? " + << (motor.idle() ? "true" : "false") + << std::endl; + break; + case 'l': + if(++i > argc) + throw std::runtime_error("syntax error"); + motor.linearMove(strtod(argv[i],0)); + break; + case 'a': + if(++i > argc) + throw std::runtime_error("syntax error"); + motor.rotation(strtod(argv[i], 0)); + break; + case 'J': + std::cout << "Etat du Jack: " << (motor.jackState() ? "Retiré" : "Dedans") << std::endl; + break; + case 'C': + std::cout << "Couleur Sélectionné: " << (motor.colorState() ? "Rouge" : "Vert") << std::endl; + break; + case 'A': + if(++i > argc) + throw std::runtime_error("syntax error"); + motor.setAccel(strtol(argv[0], 0, 10)); + break; + case 'L': + while(!motor.idle()) + { + motor.wait(-1); + motor.sync(); + } + break; + } + motor.sync(); + while(!motor.idle()) + { + motor.wait(-1); + motor.sync(); + } + break; + } + + case 'w': + { + int stop, t; + if (i + 1 >= argc) + throw std::runtime_error ("syntax error"); + stop = atoi (argv[++i]) + Timer::getProgramTime (); + t = Timer::getProgramTime (); + while (t < stop) + { + motor.wait (stop - t); + t = Timer::getProgramTime (); + motor.sync(); + } + break; + } + case '?': + syntax (); + return 0; + default: + throw std::runtime_error ("syntax error"); + } + i++; + } + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + syntax (); + return 1; + } + return 0; +} diff --git a/i/marvin/src/proto/Makefile.defs b/i/marvin/src/proto/Makefile.defs new file mode 100644 index 0000000..fa984bc --- /dev/null +++ b/i/marvin/src/proto/Makefile.defs @@ -0,0 +1,8 @@ +PROGRAMS += test_proto + +proto_OBJECTS = proto.o + +test_proto_OBJECTS = test_proto.o $(proto_OBJECTS) $(utils_OBJECTS) \ + $(serial_OBJECTS) $(timer_OBJECTS) $(log_OBJECTS) + +test_proto: $(test_proto_OBJECTS) diff --git a/i/marvin/src/proto/proto.cc b/i/marvin/src/proto/proto.cc new file mode 100644 index 0000000..a7ada32 --- /dev/null +++ b/i/marvin/src/proto/proto.cc @@ -0,0 +1,433 @@ +// proto.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// 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 "timer/timer.hh" +#include "utils/hexa.hh" +#include "proto.hh" + +#include +#include +#include + +/// Constructeur. +Proto::Proto(Receiver &receiver) + : log_ ("proto"), receiver_(receiver), tLastSend_(0), revState_(0) +{ +} + +/// Ouvre le port série. +void +Proto::open(const std::string &ttyname) +{ + serial_.open(ttyname); +} + +/// Ferme le port série +void +Proto::close(void) +{ + serial_.close(); +} + +/// Teste si tout les packets ont été envoyés et aquités, sinon, essaye de +/// le faire. +bool +Proto::sync(void) +{ + bool reGet; + // Récupération de la frame + while (reGet = getFrame()) + { + //log_ ("recv") << "frame" << currentFrame_; + // Si la frame est un aquittement + if(currentFrame_ == frameQueue_.front()) + { + // on vire la commande de la queue + frameQueue_.pop(); + // Et on envoie la suivante si elle existe + if(!frameQueue_.empty()) + sendFrame(frameQueue_.front()); + } + // Si c'est une nouvelle commande, on l'envoie avec receive + else + receiver_.receive(currentFrame_.command, currentFrame_); + } + // On regarde depuis combien de temps on a envoyé une commande + if(!frameQueue_.empty()) + { + // Si on dépasse la milliseconde, on renvoie + if(Timer::getProgramTime() - tLastSend_ >= timeout_) + sendFrame(frameQueue_.front()); + } + return frameQueue_.empty(); +} + +/// Envoie un packet +void +Proto::send (const Frame & frame, bool reliable) +{ + if(reliable) + { + if(frameQueue_.empty()) + tLastSend_ = -timeout_; + frameQueue_.push(frame); + } + else + sendFrame(frame); + // XXX sync(); +} + + +/// Envois un packet. COMMAND est la commande à envoyer, FORMAT, donne le +/// format et le nombre de paramètres ('b' : 8 bits, 'w' : 16 bits, 'd' : +/// 32 bits, majuscule pour signé). +void +Proto::send (char command, const char *format, int a0, int a1, + int a2, int a3, int a4, int a5, bool reliable) +{ + // Constitution de la frame + Proto::Frame frame; + frame.command = command; + + if (format[0] != '\0') + { + newArgFrame(frame, format[0],a0); + if (format[1] != '\0') + { + newArgFrame(frame, format[1],a1); + if (format[2] != '\0') + { + newArgFrame(frame, format[2],a2); + if (format[3] != '\0') + { + newArgFrame(frame, format[3],a3); + if(format[4] != '\0') + { + newArgFrame(frame, format[4],a4); + if(format[5] != '\0') + newArgFrame(frame, format[5],a5); + } + + } + } + } +} + send(frame,reliable); +} + +/// permet d'envoyer un packet pas fiable +void +Proto::sendUnreliable (char command, const char *format, int a0, int a1, + int a2, int a3, int a4, int a5) +{ + send(command, format, a0, a1, a2, a3, a4, a5, false); +} + +/// Teste si le packet correspond au format et décode les valeurs. Utilise +/// le même format que send. +bool +Proto::decode (const Proto::Frame &frame) +{ + int dummy; + return decode(frame, "", dummy, dummy, dummy, dummy); +} + +bool +Proto::decode (const Frame &frame, const char *format, int &a0) +{ + int dummy; + return decode(frame, format, a0, dummy, dummy, dummy); +} + +bool +Proto::decode (const Frame &frame, const char *format, int &a0, int &a1) +{ + int dummy; + return decode(frame, format, a0, a1, dummy, dummy); +} + +bool +Proto::decode (const Frame &frame, const char *format, int &a0, + int &a1, int &a2) +{ + + int dummy; + return decode(frame, format, a0, a1, a2, dummy); +} + +bool +Proto::decode (const Frame &frame, const char *format, int &a0, + int &a1, int &a2, int &a3) +{ + // Teste si il y a bien le bon nombre d'argument + if (static_cast(frame.args.size()) != argsFrameSize(format)) + return false; + // On décode et on envoie + decodeArg(frame, format, a0, a1, a2, a3); + + return true; +} + +/// Attend que des caractères soit disponible pendant un temps maximum en +/// milliseconde et les traite. Renvois le résultat de sync (). Attention, +/// fonction bloquante, n'utiliser que pour les tests. +bool +Proto::wait (int timeout/*-1*/) +{ + // On apelle serial_.wait avec un timeout toujours inf ou égal à proto::timeout_ + return serial_.wait (timeout < timeout_ ? timeout : timeout_); +} + +/// Récupère le File Descriptor +int Proto::getFd(void) +{ + return serial_.getFd(); +} + +/// Récupère les infos de l'AVR pour construire une frame +bool +Proto::getFrame(void) +{ + int receivedChar, d; + bool erreur = false; + // tant que le tampon n'est pas vide, on teste + while((receivedChar = serial_.getchar()) != -1) + { + // Si on reçoit un bang + if(receivedChar == '!') + { + revState_ = 1; + currentFrame_.command = 0; + currentFrame_.args.clear(); + } + // Si on reçoit le retour chariot et que on reçevait les args + else if(receivedChar == '\r' && revState_ == 2) + { + revState_ = 0; + return true; + } + // Pour les autres charactères + // Si on attend la commande + else + switch(revState_) + { + case 1: + if (!isalpha (receivedChar) || receivedChar == '?') + erreur = true; + else + { + currentFrame_.command = receivedChar; + revState_ = 2; + } + // On vérifie que le match n'est pas fini + break; + case 2: + d = hex2digit (receivedChar); + if (d == hexInvalid) + erreur = true; + else + { + currentFrame_.args.push_back (static_cast(d) << 4); + revState_ = 3; + } + break; + case 3: + d = hex2digit (receivedChar); + if (d == hexInvalid) + erreur = true; + else + { + currentFrame_.args.back() |= static_cast(d); + revState_ = 2; + break; + } + } + // Si revState == 0 alors on jette + // Si on a reçu une erreur on renvoie + if(erreur) + { + // On renvoie en mettant le compteur à 0, la commande sera + // renvoyer de retour à sync + //log_("Erreur de reception") << "commande inconnue" << ""; + tLastSend_ = 0; + revState_ = 0; + return false; + } + } + return false; +} + +/// Envoie la frame dans l'AVR +void +Proto::sendFrame(const Frame & frame) +{ + //log_ ("send") << "frame" << frame; + // envoyer le bang + serial_.putchar('!'); + + // Envoyer la commande + serial_.putchar(frame.command); + + // Envoyer les arguments + for(std::vector::const_iterator it = frame.args.begin(); + it != frame.args.end(); it++) + { + serial_.putchar(digit2hex(*it >> 4)); + serial_.putchar(digit2hex(*it & 0x0f)); + } + + // Envoyer le retour chariot + serial_.putchar('\r'); + + // actualiser le timer + tLastSend_ = Timer::getProgramTime(); +} + +/// Remplie une frame avec un argument +void +Proto::newArgFrame(Proto::Frame & frame, char format, int arg) +{ + switch(format) + { + case 'b': + case 'B': + frame.args.push_back(static_cast(arg)); + break; + case 'w': + case 'W': + frame.args.push_back(static_cast(arg >> 8)); + frame.args.push_back(static_cast(arg)); + break; + case 'd': + case 'D': + frame.args.push_back(static_cast(arg >> 24)); + frame.args.push_back(static_cast(arg >> 16)); + frame.args.push_back(static_cast(arg >> 8)); + frame.args.push_back(static_cast(arg)); + break; + } +} + +/// Renvoie la taille necessaire du vecteur args pour un format donné +int +Proto::argsFrameSize(const char *format) +{ + int size = 0; + for(; *format != '\0'; format++) + switch(*format) + { + case 'b': + case 'B': + size += 1; + break; + case 'w': + case 'W': + size += 2; + break; + case 'd': + case 'D': + size += 4; + break; + default: + size += 0; + } + return size; +} + +/// Décode un argument +void +Proto::decodeArg(const Frame & frame, const char *format, int &a0, int &a1, int &a2, int &a3) +{ + int temp[4]; + int pos = 0; + + for(int i = 0; *format != '\0'; format++,i++) + { + switch(*format) + { + case 'b': + temp[i] = static_cast(frame.args[pos]); + pos++; + break; + case 'B': + { + int8_t t = static_cast(frame.args[pos]); + temp[i] = static_cast(t); + pos++; + break; + } + case 'w': + temp[i] = static_cast(frame.args[pos]) << 8 + |static_cast(frame.args[pos + 1]); + pos += 2; + break; + case 'W': + { + int8_t t = static_cast(frame.args[pos]); + temp[i] = static_cast(t) << 8 + |static_cast(frame.args[pos + 1]); + pos += 2; + break; + } + case 'd': + temp[i] = static_cast(frame.args[pos]) << 24 + |static_cast(frame.args[pos + 1]) << 16 + |static_cast(frame.args[pos + 2]) << 8 + |static_cast(frame.args[pos + 3]); + pos += 4; + break; + + case 'D': + int8_t t = static_cast(frame.args[pos]); + temp[i] = static_cast(t) << 24 + |static_cast(frame.args[pos + 1]) << 16 + |static_cast(frame.args[pos + 2]) << 8 + |static_cast(frame.args[pos + 3]); + break; + } + } + a0 = temp[0]; + a1 = temp[1]; + a2 = temp[2]; + a3 = temp[3]; +} + +bool +Proto::Frame::operator==(const Frame& frame) +{ + return this->command == frame.command && this->args == frame.args; +} + +/// Affiche une frame. +std::ostream & +operator<< (std::ostream &os, const Proto::Frame &f) +{ + os << '<' << f.command << ' '; + std::copy (f.args.begin (), f.args.end (), + std::ostream_iterator (os, " ")); + os << '>'; + return os; +} + diff --git a/i/marvin/src/proto/proto.hh b/i/marvin/src/proto/proto.hh new file mode 100644 index 0000000..11b1ae3 --- /dev/null +++ b/i/marvin/src/proto/proto.hh @@ -0,0 +1,135 @@ +#ifndef proto_hh +#define proto_hh +// proto.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "utils/non_copyable.hh" +#include "serial/serial.hh" +#include "log/log.hh" + +#include +#include + +/// Classe de dialogue avec une carte électronique par le port série. +class Proto : public NonCopyable +{ + public: + class Receiver; + /// Packet. + struct Frame + { + char command; + std::vector args; + bool operator==(const Frame& frame); + }; + private: + Log log_; + Serial serial_; + Receiver &receiver_; + + /// File d'attente des messages à aquiter. + typedef std::queue FrameQueue; + FrameQueue frameQueue_; + //Date du dernier envoie + int tLastSend_; + //Temps entre les ré-émission + static const int timeout_ = 500; + + /// Frame en cours de réception + Frame currentFrame_; + int revState_; + // Etat de la réception de la frame + // 0 - Rien reçu + // 1 - Bang reçu + // 2 - Commande reçu & nouveau argument + // 3 - char 1 argument reçu + + public: + /// Constructeur. + Proto (Receiver &receiver); + /// Ouvre le port série. + void open (const std::string &ttyname); + /// Ferme le port série. + void close (void); + /// Teste si tout les packets ont été envoyés et aquités, sinon, essaye de + /// le faire. + bool sync (void); + /// Envois un packet. + void send (const Frame &frame, bool reliable = true); + /// Envois un packet. COMMAND est la commande à envoyer, FORMAT, donne le + /// format et le nombre de paramètres ('b' : 8 bits, 'w' : 16 bits, 'd' : + /// 32 bits, majuscule pour signé). + void send (char command, const char *format = "", int a0 = 0, int a1 = 0, + int a2 = 0, int a3 = 0, int a4 = 0, int a5 = 0, bool reliable = true); + /// permet d'envoyer un packet robert + void Proto::sendUnreliable (char command, const char *format, int a0, + int a1, int a2, int a3, int a4, int a5); + //@{ + /// Teste si le packet correspond au format et décode les valeurs. Utilise + /// le même format que send. + static bool decode (const Frame &frame); + static bool decode (const Frame &frame, const char *format, int &a0); + static bool decode (const Frame &frame, const char *format, int &a0, int + &a1); + static bool decode (const Frame &frame, const char *format, int &a0, int + &a1, int &a2); + static bool decode (const Frame &frame, const char *format, int &a0, int + &a1, int &a2, int &a3); + //@} + /// Attend que des caractères soit disponible pendant un temps maximum en + /// milliseconde et les traite. Renvois le résultat de sync (). Attention, + /// fonction bloquante, n'utiliser que pour les tests. + bool wait (int timeout = -1); + /// Récupère le File Descriptor + int getFd(void); + + public: + /// Les clients de Proto doivent dériver de Receiver. + class Receiver + { + public: + virtual ~Receiver(void) {}; + /// Recoit un packet. + virtual void receive (char command, const Frame &frame) = 0; + }; + private: + /// Récupère les infos de l'AVR pour construire une frame + bool getFrame(void); + /// Envoie la frame dans l'AVR + void sendFrame(const Frame & frame); + /// Remplie une frame avec un argument + void newArgFrame(Proto::Frame & frame, char format, int arg); + /// Renvoie la taille necessaire du vecteur args pour un format donné + static int argsFrameSize(const char *format); + /// Décode un argument + static void decodeArg(const Frame & frame, const char *format, int &a0, + int &a1, int &a2, int &a3); +}; + +/// Affiche une frame. +std::ostream & +operator<< (std::ostream &os, const Proto::Frame &f); + +#endif // proto_hh diff --git a/i/marvin/src/proto/test_proto.cc b/i/marvin/src/proto/test_proto.cc new file mode 100644 index 0000000..9ec119d --- /dev/null +++ b/i/marvin/src/proto/test_proto.cc @@ -0,0 +1,124 @@ +// test_proto.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// 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 "proto.hh" +#include "timer/timer.hh" + +#include +#include + +/// Classe de test pour proto. +class TestProto : public Proto::Receiver +{ + void receive (char command, const Proto::Frame &frame) + { + std::cout << "received " << frame << std::endl; + } +}; + +/// Affiche un memo de suntaxe. +void +syntax (void) +{ + std::cout << "test_proto - test la classe de protocol série.\n" + "Syntaxe : test_proto <...>\n" + " s envois une commande\n" + " w attend pendant un nombre de millisecondes\n" + " ? affiche cet écran d'aide\n" + << std::endl; +} + +int +main (int argc, char **argv) +{ + try + { + int i; + if (argc < 2) + { + syntax (); + return 1; + } + TestProto testProto; + Proto proto (testProto); + proto.open (argv[1]); + i = 2; + while (i < argc) + { + bool reliable = true; + switch (argv[i][0]) + { + case 'S': + reliable = false; + // no break; + case 's': + { + if (i + 2 >= argc) + throw std::runtime_error ("syntax error"); + unsigned a; + int arg[4]; + char c = argv[++i][0]; + const char *format = argv[++i]; + if (i + static_cast (strlen (format)) >= argc) + throw std::runtime_error ("syntax error"); + for (a = 0; a < 4 && a < strlen (format); a++) + arg[a] = atoi (argv[++i]); + proto.send (c, format, arg[0], arg[1], arg[2], arg[3], + reliable); + while (!proto.wait (-1)) + ; + } + break; + case 'w': + { + int stop, t; + if (i + 1 >= argc) + throw std::runtime_error ("syntax error"); + stop = atoi (argv[++i]) + Timer::getProgramTime (); + t = Timer::getProgramTime (); + while (t < stop) + { + proto.wait (stop - t); + t = Timer::getProgramTime (); + } + break; + } + case '?': + proto.close (); + syntax (); + return 0; + default: + throw std::runtime_error ("syntax error"); + } + i++; + } + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + syntax (); + return 1; + } + return 0; +} diff --git a/i/marvin/src/scheduler/Makefile.defs b/i/marvin/src/scheduler/Makefile.defs new file mode 100644 index 0000000..9aff718 --- /dev/null +++ b/i/marvin/src/scheduler/Makefile.defs @@ -0,0 +1,9 @@ +PROGRAMS += test_scheduler + +scheduler_OBJECTS = scheduler.o schedulable_read_fd.o schedulable_alarm.o + +test_scheduler_OBJECTS = \ + test_scheduler.o scheduler.o schedulable_read_fd.o schedulable_alarm.o \ + $(timer_OBJECTS) + +test_scheduler: $(test_scheduler_OBJECTS) diff --git a/i/marvin/src/scheduler/schedulable.hh b/i/marvin/src/scheduler/schedulable.hh new file mode 100644 index 0000000..d8869af --- /dev/null +++ b/i/marvin/src/scheduler/schedulable.hh @@ -0,0 +1,50 @@ +#ifndef schedulable_hh +#define schedulable_hh +// schedulable.hh +// {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} + +class FdSet; + +namespace scheduler { + +class Scheduler; + +/// Classe abstraite pour un objet utilisable par le Scheduler. +class Schedulable +{ + public: + /// Destructeur. + virtual ~Schedulable (void) {} + /// Paramètre le Scheduler. + virtual void setup (Scheduler &scheduler, int time, FdSet &readSet, + int &timeout) = 0; + /// Appelé aprés l'attente du Scheduler, renvois true si un événement a + /// été traité. + virtual bool run (Scheduler &scheduler, int time, + const FdSet &readSet) = 0; +}; + +} // namespace scheduler + +#endif // schedulable_hh diff --git a/i/marvin/src/scheduler/schedulable_alarm.cc b/i/marvin/src/scheduler/schedulable_alarm.cc new file mode 100644 index 0000000..b890350 --- /dev/null +++ b/i/marvin/src/scheduler/schedulable_alarm.cc @@ -0,0 +1,56 @@ +// schedulable_alarm.cc +// {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} +#include "schedulable_alarm.hh" + +namespace scheduler { + +/// Paramètre le Scheduler. +void +SchedulableAlarm::setup (Scheduler &scheduler, int time, FdSet &readSet, int + &timeout) +{ + if (start_ != -1) + { + int to = start_ + step_ - time; + timeout = to > 0 ? to : 0; + } +} + +/// Appelé aprés l'attente du Scheduler, renvois true si un événement a +/// été traité. +bool +SchedulableAlarm::run (Scheduler &scheduler, int time, const FdSet &readSet) +{ + if (time >= start_ + step_) + { + start_ = periodic_ ? start_ + step_ : -1; + callback_ (); + return true; + } + else + return false; +} + +} // namespace scheduler + diff --git a/i/marvin/src/scheduler/schedulable_alarm.hh b/i/marvin/src/scheduler/schedulable_alarm.hh new file mode 100644 index 0000000..1e008b6 --- /dev/null +++ b/i/marvin/src/scheduler/schedulable_alarm.hh @@ -0,0 +1,60 @@ +#ifndef schedulable_alarm_hh +#define schedulable_alarm_hh +// schedulable_alarm.hh +// {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} +#include "schedulable.hh" +#include "utils/callback.hh" +#include "timer/timer.hh" + +namespace scheduler { + +/// Execute un callback à un instant donné. +class SchedulableAlarm : public Schedulable +{ + Callback callback_; + int start_, step_; + bool periodic_; + public: + /// Construit une alarm dans step milliseconde, périodique si periodic est + /// vrai (vrai par defaut), à partir de start si donné, ou de maintenant + /// sinon. + template + SchedulableAlarm (T callback, int step, bool periodic = true, + int start = -1) + : callback_ (callback), + start_ (start), step_ (step), periodic_ (periodic) + { + if (start_ == -1) + start_ = Timer::getProgramTime (); + } + /// Paramètre le Scheduler. + void setup (Scheduler &scheduler, int time, FdSet &readSet, int &timeout); + /// Appelé aprés l'attente du Scheduler, renvois true si un événement a + /// été traité. + bool run (Scheduler &scheduler, int time, const FdSet &readSet); +}; + +} // namespace scheduler + +#endif // schedulable_alarm_hh diff --git a/i/marvin/src/scheduler/schedulable_read_fd.cc b/i/marvin/src/scheduler/schedulable_read_fd.cc new file mode 100644 index 0000000..3c84c9b --- /dev/null +++ b/i/marvin/src/scheduler/schedulable_read_fd.cc @@ -0,0 +1,51 @@ +// schedulable_read_fd.cc +// {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} +#include "schedulable_read_fd.hh" +#include "utils/fd_set.hh" + +namespace scheduler { + +/// Paramètre le Scheduler. +void +SchedulableReadFd::setup (Scheduler &scheduler, int time, FdSet &readSet, int &timeout) +{ + readSet.set (fd_); +} + +/// Appelé aprés l'attente du Scheduler, renvois true si un événement a +/// été traité. +bool +SchedulableReadFd::run (Scheduler &scheduler, int time, const FdSet &readSet) +{ + if (readSet.isSet (fd_)) + { + callback_ (); + return true; + } + else + return false; +} + +} // namespace scheduler + diff --git a/i/marvin/src/scheduler/schedulable_read_fd.hh b/i/marvin/src/scheduler/schedulable_read_fd.hh new file mode 100644 index 0000000..01d7b26 --- /dev/null +++ b/i/marvin/src/scheduler/schedulable_read_fd.hh @@ -0,0 +1,52 @@ +#ifndef schedulable_read_fd_hh +#define schedulable_read_fd_hh +// schedulable_read_fd.hh +// {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} +#include "schedulable.hh" +#include "utils/callback.hh" + +namespace scheduler { + +/// Execute un callback sur une lecture possible sur un descripteur de +/// fichier. +class SchedulableReadFd : public Schedulable +{ + Callback callback_; + int fd_; + public: + /// Constructeur. + template + SchedulableReadFd (T callback, int fd) + : callback_ (callback), fd_ (fd) + { } + /// Paramètre le Scheduler. + void setup (Scheduler &scheduler, int time, FdSet &readSet, int &timeout); + /// Appelé aprés l'attente du Scheduler, renvois true si un événement a + /// été traité. + bool run (Scheduler &scheduler, int time, const FdSet &readSet); +}; + +} // namespace scheduler + +#endif // schedulable_read_fd_hh diff --git a/i/marvin/src/scheduler/scheduler.cc b/i/marvin/src/scheduler/scheduler.cc new file mode 100644 index 0000000..4ef3ade --- /dev/null +++ b/i/marvin/src/scheduler/scheduler.cc @@ -0,0 +1,103 @@ +// scheduler.cc +// {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} +#include "scheduler.hh" +#include "utils/fd_set.hh" +#include "timer/timer.hh" + +#include +#include + +namespace scheduler { + +/// Destructeur. +Scheduler::~Scheduler (void) +{ + // TODO: Test for schedulables_ empty ? +} + +/// Ajoute un élément schedulable. +void +Scheduler::insert (Schedulable &schedulable) +{ + schedulables_.insert (&schedulable); +} + +/// Enlève un élément schedulable. +void +Scheduler::erase (Schedulable &schedulable) +{ + schedulables_.erase (&schedulable); +} + +/// Lance le scheduler. Si timeout est différent de -1, c'est le temps +/// maximal que prend le scheduler avant de rendre la main. Si +/// returnOnEvent est true, le premier évenement arrète le scheduler. +/// Renvois true si au moins un événement a été traité. +bool +Scheduler::schedule (int timeout/*-1*/, bool returnOnEvent/*false*/) +{ + bool event = false; + int t = Timer::getProgramTime (); + int start = t; + do + { + // Prépare le select. + int to = timeout == -1 ? -1 : start + timeout - t; + FdSet readFds; + for (Schedulables::const_iterator i = schedulables_.begin (); + i != schedulables_.end (); + ++i) + { + int top = -1; + (*i)->setup (*this, t, readFds, top); + // Si le timeout est plus court, retient ce timeout. + if (to == -1 || top != -1 && top < to) + to = top; + } + // Select. + if (to != -1) + { + timeval tv; + tv.tv_sec = to / 1000; + tv.tv_usec = to % 1000 * 1000; + select (FD_SETSIZE, readFds.get (), 0, 0, &tv); + } + else + { + select (FD_SETSIZE, readFds.get (), 0, 0, 0); + } + // Run. + t = Timer::getProgramTime (); + for (Schedulables::const_iterator i = schedulables_.begin (); + i != schedulables_.end (); + ++i) + event = (*i)->run (*this, t, readFds) || event; + t = Timer::getProgramTime (); + } while (!(returnOnEvent && event) + && (timeout == -1 || start + timeout > t)); + return event; +} + +} // namespace scheduler + diff --git a/i/marvin/src/scheduler/scheduler.hh b/i/marvin/src/scheduler/scheduler.hh new file mode 100644 index 0000000..3bd885b --- /dev/null +++ b/i/marvin/src/scheduler/scheduler.hh @@ -0,0 +1,53 @@ +#ifndef scheduler_hh +#define scheduler_hh +// scheduler.hh +// {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} +#include "schedulable.hh" + +#include + +namespace scheduler { + +/// Scheduler, répartie le travail en fonction des événements. +class Scheduler +{ + typedef std::set Schedulables; + Schedulables schedulables_; + public: + /// Destructeur. + ~Scheduler (void); + /// Ajoute un élément schedulable. + void insert (Schedulable &schedulable); + /// Enlève un élément schedulable. + void erase (Schedulable &schedulable); + /// Lance le scheduler. Si timeout est différent de -1, c'est le temps + /// maximal que prend le scheduler avant de rendre la main. Si + /// returnOnEvent est true, le premier évenement arrète le scheduler. + /// Renvois true si au moins un événement a été traité. + bool schedule (int timeout = -1, bool returnOnEvent = false); +}; + +} // namespace scheduler + +#endif // scheduler_hh diff --git a/i/marvin/src/scheduler/test_scheduler.cc b/i/marvin/src/scheduler/test_scheduler.cc new file mode 100644 index 0000000..6cb88c0 --- /dev/null +++ b/i/marvin/src/scheduler/test_scheduler.cc @@ -0,0 +1,68 @@ +// test_scheduler.cc +// {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} +#include "scheduler.hh" +#include "schedulable_read_fd.hh" +#include "schedulable_alarm.hh" +#include "utils/bind.hh" +#include "timer/timer.hh" + +#include +#include + +void +ev (void) +{ + std::cout << "event received" << std::endl; + char buf[1024]; + read (0, buf, 1024); +} + +void +to (const char *msg) +{ + std::cout << "timeout received \"" << msg << "\" at " + << Timer::getProgramTime () << std::endl; +} + +int +main (void) +{ + using namespace scheduler; + try + { + Scheduler s; + SchedulableReadFd srf (ev, 0); + s.insert (srf); + std::cout << "start at " << Timer::getProgramTime () << std::endl; + SchedulableAlarm sa1 (bind (to, "1s"), 1000); + s.insert (sa1); + s.schedule (10000); + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + return 1; + } + return 0; +} diff --git a/i/marvin/src/serial/Makefile.defs b/i/marvin/src/serial/Makefile.defs new file mode 100644 index 0000000..0ece3d9 --- /dev/null +++ b/i/marvin/src/serial/Makefile.defs @@ -0,0 +1,7 @@ +PROGRAMS += test_serial + +serial_OBJECTS = serial.o serial_base.o serial_dev.o serial_stdio.o + +test_serial_OBJECTS = test_serial.o $(serial_OBJECTS) $(utils_OBJECTS) + +test_serial: $(test_serial_OBJECTS) diff --git a/i/marvin/src/serial/serial.cc b/i/marvin/src/serial/serial.cc new file mode 100644 index 0000000..db2a6d2 --- /dev/null +++ b/i/marvin/src/serial/serial.cc @@ -0,0 +1,66 @@ +// serial.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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.hh" +#include "serial_dev.hh" +#include "serial_stdio.hh" + +/// Constructeur. +Serial::Serial (bool blocking) + : blocking_ (blocking), sb_ (0) +{ +} + +/// Destructeur. +Serial::~Serial (void) +{ + close (); +} + +/// Ouvre et paramètre le port série. \a ttyname peut être un device +/// ("/dev/tty00,115200", voir SerialDev), un programme précédé d'un tube +/// ("|simul_asserv", voir SerialPipe) ou l'entrée/sortie standard ("-"). +void +Serial::open (const std::string &ttyname) +{ + close (); + if (ttyname == "-") + sb_ = new SerialStdio (blocking_); + else if (ttyname[0] == '|') + { + // TODO + } + else + sb_ = new SerialDev (blocking_); + sb_->open (ttyname); +} + +/// Ferme le port série, appellé automatiquement dans le destructeur. +void +Serial::close (void) +{ + delete sb_; + sb_ = 0; +} + diff --git a/i/marvin/src/serial/serial.hh b/i/marvin/src/serial/serial.hh new file mode 100644 index 0000000..23feb42 --- /dev/null +++ b/i/marvin/src/serial/serial.hh @@ -0,0 +1,64 @@ +#ifndef serial_hh +#define serial_hh +// serial.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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_base.hh" + +/// Classe générique qui utilise la sous-classe apropriée de SerialBase en +/// fonction du paramètre de Serial::open(). +class Serial +{ + bool blocking_; + SerialBase *sb_; + public: + /// Constructeur. + Serial (bool blocking = false); + /// Destructeur. + ~Serial (void); + /// Ouvre et paramètre le port série. \a ttyname peut être un device + /// ("/dev/tty00,115200", voir SerialDev), un programme précédé d'un tube + /// ("|simul_asserv", voir SerialPipe) ou l'entrée/sortie standard ("-"). + void open (const std::string &ttyname); + /// Ferme le port série, appellé automatiquement dans le destructeur. + void close (void); + /// Lit un bloc, retourne le nombre d'octets lus ou <= 0 en cas d'erreur + /// ou si aucun caractère n'est disponible. Pour le robot, préferer en + /// général getchar() pour du non-bloquant. + ssize_t read (void *buf, size_t size) { return sb_->read (buf, size); } + /// Lit un caractère, ou -1 en cas d'erreur ou si aucun caractère n'est + /// disponible. + int getchar (void) { return sb_->getchar (); } + /// Ecrit un bloc. + void write (const void *buf, size_t size) { sb_->write (buf, size); } + /// Ecrit un caractère. + void putchar (int c) { sb_->putchar (c); } + /// Attend que des caractères soient disponibles pendant un delay en + /// millisecondes. + bool wait (int timeout = -1) { return sb_->wait (timeout); } + /// Récupère le descripteur. + int getFd (void) { return sb_->getFd (); } +}; + +#endif // serial_hh diff --git a/i/marvin/src/serial/serial_base.cc b/i/marvin/src/serial/serial_base.cc new file mode 100644 index 0000000..b92a6b0 --- /dev/null +++ b/i/marvin/src/serial/serial_base.cc @@ -0,0 +1,84 @@ +// serial_base.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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_base.hh" +#include "utils/fd_set.hh" + +/// Constructeur. +SerialBase::SerialBase (void) + : fdIn_ (-1), fdOut_ (-1) +{ +} + +/// Destructeur. +SerialBase::~SerialBase (void) +{ +} + +/// Lit un bloc, retourne le nombre d'octets lus ou <= 0 en cas d'erreur +/// ou si aucun caractère n'est disponible. Pour le robot, préferer en +/// général getchar() pour du non-bloquant. +ssize_t +SerialBase::read (void *buf, size_t size) +{ + return ::read (fdIn_, buf, size); +} + +/// Lit un caractère, ou -1 en cas d'erreur ou si aucun caractère n'est +/// disponible. +int +SerialBase::getchar (void) +{ + char c; + if (read (&c, 1) == 1) + return c & 0xff; + else + return -1; +} + +/// Ecrit un bloc. +void +SerialBase::write (const void *buf, size_t size) +{ + ::write (fdOut_, buf, size); +} + +/// Ecrit un caractère. +void +SerialBase::putchar (int c) +{ + char cc = c; + write (&cc, 1); +} + +/// Attend que des caractères soient disponibles pendant un delay en +/// millisecondes. +bool +SerialBase::wait (int timeout/*-1*/) +{ + FdSet fds; + fds.set (fdIn_); + return fds.wait (timeout); +} + diff --git a/i/marvin/src/serial/serial_base.hh b/i/marvin/src/serial/serial_base.hh new file mode 100644 index 0000000..9caf27d --- /dev/null +++ b/i/marvin/src/serial/serial_base.hh @@ -0,0 +1,68 @@ +#ifndef serial_base_hh +#define serial_base_hh +// serial_base.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#include +#include + +/// Classe de base pour un port série. +class SerialBase +{ + protected: + /// Descripteur de fichier utilisé par les fonctions de lecture. + int fdIn_; + /// Descripteur de fichier utilisé par les fonctions d'écriture. + int fdOut_; + public: + /// Constructeur. + SerialBase (void); + /// Destructeur. + virtual ~SerialBase (void); + /// Ouvre et paramètre le port série. \a ttyname peut être un device + /// ("/dev/tty00,115200", voir SerialDev), un programme précédé d'un tube + /// ("|simul_asserv", voir SerialPipe) ou l'entrée/sortie standard ("-"). + virtual void open (const std::string &ttyname) = 0; + /// Ferme le port série, appellé automatiquement dans le destructeur. + virtual void close (void) = 0; + /// Lit un bloc, retourne le nombre d'octets lus ou <= 0 en cas d'erreur + /// ou si aucun caractère n'est disponible. Pour le robot, préferer en + /// général getchar() pour du non-bloquant. + ssize_t read (void *buf, size_t size); + /// Lit un caractère, ou -1 en cas d'erreur ou si aucun caractère n'est + /// disponible. + int getchar (void); + /// Ecrit un bloc. + void write (const void *buf, size_t size); + /// Ecrit un caractère. + void putchar (int c); + /// Attend que des caractères soient disponibles pendant un delay en + /// millisecondes. + bool wait (int timeout = -1); + /// Récupère le descripteur. + int getFd (void) { return fdIn_; } +}; + +#endif // serial_base_hh diff --git a/i/marvin/src/serial/serial_dev.cc b/i/marvin/src/serial/serial_dev.cc new file mode 100644 index 0000000..3e796e7 --- /dev/null +++ b/i/marvin/src/serial/serial_dev.cc @@ -0,0 +1,168 @@ +// serial_dev.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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_dev.hh" +#include "utils/errno_exception.hh" + +#include +#include +#include + +/// Constructeur. +SerialDev::SerialDev (bool blocking/*false*/) + : blocking_ (blocking) +{ +} + +/// Destructeur. +SerialDev::~SerialDev (void) +{ + close (); +} + +/// Ouvre et paramètre le port série, accepte plusieurs paramètres séparés par +/// des virgules : tty, vitesse, parité (none, odd, even). Par exemple : +/// "/dev/ttyS00,115200,even" (par defaut). +void +SerialDev::open (const std::string &ttyname) +{ + struct termios tios; + std::string tty; + int speed = 115200; + char par = 'e'; + // Décode le ttyname. + decode (ttyname, tty, speed, par); + // Ouvre le port série. + fdIn_ = ::open (tty.c_str (), O_RDWR | O_NOCTTY | O_NONBLOCK); + if (fdIn_ < 0) + throw errno_exception (ttyname, errno); + // Rendre le port série asynchrone. + //fcntl (fd_, F_SETFL, FASYNC); + // Sauver la configuration courante du port série. + tcgetattr (fdIn_, &old_); + tcgetattr (fdIn_, &tios); + // Paramètrer les options du port série. + // Vérification de la parité en entrée, remplace toute erreur de + // transmission par un caractère nul. Pas de Xon Xoff. Entrée crue. + tios.c_iflag &= ~(IGNPAR | PARMRK | ISTRIP | IGNBRK | BRKINT | IGNCR | + ICRNL | INLCR | IXON | IXOFF | IXANY | IMAXBEL); + tios.c_iflag |= INPCK; + // Sortie crue. + tios.c_oflag &= ~(OPOST); + // Pas de status line, 1 bit de stop, 8 bits de données, règle la parité. + tios.c_cflag &= ~(HUPCL | CSTOPB | PARENB | PARODD | CSIZE); + tios.c_cflag |= CS8 | CLOCAL | CREAD; + if (par != 'n') tios.c_cflag |= PARENB; + if (par == 'o') tios.c_cflag |= PARODD; + // Cru. + tios.c_lflag &= ~(ICANON | ECHO | ISIG | NOFLSH | TOSTOP); + // On n'attend jamais si on est en non-block. On attend sinon. + tios.c_cc[VTIME] = 0; + if (blocking_) + tios.c_cc[VMIN] = 1; + else + tios.c_cc[VMIN] = 0; + switch (speed) + { + case 1200: + cfsetspeed (&tios, B1200); + break; + case 9600: + cfsetspeed (&tios, B9600); + break; + case 19200: + cfsetspeed (&tios, B19200); + break; + case 38400: + cfsetspeed (&tios, B38400); + break; + case 57600: + cfsetspeed (&tios, B57600); + break; + case 115200: + cfsetspeed (&tios, B115200); + break; + case 230400: + cfsetspeed (&tios, B230400); + break; + case 460800: + cfsetspeed (&tios, B460800); + break; + default: + throw std::runtime_error ("Vitesse de port série non supportée\n"); + } + // Vider et configurer le port série. + tcflush (fdIn_, TCIFLUSH); + tcsetattr (fdIn_, TCSANOW, &tios); + fdOut_ = fdIn_; +} + +/// Ferme le port série, appellé automatiquement dans le destructeur de la +/// classe SerialBase. +void +SerialDev::close (void) +{ + if (fdIn_ != -1) + { + tcsetattr (fdIn_, TCSANOW, &old_); + ::close (fdIn_); + fdIn_ = -1; + fdOut_ = -1; + } +} + +/// Décode le ttyname, voir open. +void +SerialDev::decode (const std::string &ttyname, std::string &tty, int &speed, + char &par) const +{ + std::string::size_type a, b, d; + int i; + a = 0; + b = ttyname.find (','); + while (a != std::string::npos) + { + d = b == std::string::npos ? std::string::npos : b - a; + std::string s (ttyname, a, d); + i = atoi (s.c_str ()); + if (i) + speed = i; + else + { + if (s == "e" || s == "even") + par = 'e'; + else if (s == "o" || s == "odd") + par = 'o'; + else if (s == "n" || s == "none") + par = 'n'; + else if (tty.empty ()) + tty = s; + else + throw std::runtime_error ("error in ttyname: " + s); + } + a = b == std::string::npos ? b : b + 1; + b = ttyname.find (',', a); + } +} + diff --git a/i/marvin/src/serial/serial_dev.hh b/i/marvin/src/serial/serial_dev.hh new file mode 100644 index 0000000..31e98b6 --- /dev/null +++ b/i/marvin/src/serial/serial_dev.hh @@ -0,0 +1,54 @@ +#ifndef serial_dev_hh +#define serial_dev_hh +// serial_dev.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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_base.hh" + +#include + +/// Classe de contrôle du port serie /dev/tty*. +class SerialDev : public SerialBase +{ + bool blocking_; + struct termios old_; + public: + /// Constructeur. + SerialDev (bool blocking = false); + /// Destructeur. + ~SerialDev (void); + /// Ouvre et paramètre le port série, accepte plusieurs paramètres séparés + /// par des virgules : tty, vitesse, parité (none, odd, even). Par + /// exemple : "/dev/ttyS00,115200,even" (par defaut). + void open (const std::string &ttyname); + /// Ferme le port série, appellé automatiquement dans le destructeur de la + /// classe SerialBase. + void close (void); + private: + /// Décode le ttyname, voir open. + void decode (const std::string &ttyname, std::string &tty, int &speed, + char &par) const; +}; + +#endif // serial_dev_hh diff --git a/i/marvin/src/serial/serial_stdio.cc b/i/marvin/src/serial/serial_stdio.cc new file mode 100644 index 0000000..ff652a2 --- /dev/null +++ b/i/marvin/src/serial/serial_stdio.cc @@ -0,0 +1,81 @@ +// serial_stdio.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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_stdio.hh" +#include "utils/errno_exception.hh" + +#include + +/// Constructeur. +SerialStdio::SerialStdio (bool blocking/*false*/) + : blocking_ (blocking) +{ +} + +/// Destructeur. +SerialStdio::~SerialStdio (void) +{ + close (); +} + +/// Ouvre et paramètre le port série. +void +SerialStdio::open (const std::string &ttyname) +{ + struct termios tios; + fdIn_ = 0; + fdOut_ = 1; + // Sauver la configuration courante du port série. + tcgetattr (fdIn_, &old_); + // Paramètrer les options du port série. + // Attention, affecter des valeurs directement est une mauvaise pratique, + // regarder SerialDev pour un bon exemple. + tios.c_cflag = CS8 | CLOCAL | CREAD; + tios.c_iflag = IGNBRK | IGNPAR; + tios.c_oflag = 0; + tios.c_lflag = 0; + // On n'attend jamais si on est en non-block. On attend sinon. + tios.c_cc[VTIME] = 0; + if (blocking_) + tios.c_cc[VMIN] = 1; + else + tios.c_cc[VMIN] = 0; + // Vider et configurer le port série. + tcflush (fdIn_, TCIFLUSH); + tcsetattr (fdIn_, TCSANOW, &tios); +} + +/// Ferme le port série, appellé automatiquement dans le destructeur de la +/// classe SerialBase. +void +SerialStdio::close (void) +{ + if (fdIn_ != -1) + { + tcsetattr (fdIn_, TCSANOW, &old_); + fdIn_ = -1; + fdOut_ = -1; + } +} + diff --git a/i/marvin/src/serial/serial_stdio.hh b/i/marvin/src/serial/serial_stdio.hh new file mode 100644 index 0000000..ea10eff --- /dev/null +++ b/i/marvin/src/serial/serial_stdio.hh @@ -0,0 +1,48 @@ +#ifndef serial_stdio_hh +#define serial_stdio_hh +// serial_stdio.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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_base.hh" + +#include + +/// Classe de contrôle du port serie factice sur l'entrée/sortie standart. +class SerialStdio : public SerialBase +{ + bool blocking_; + struct termios old_; + public: + /// Constructeur. + SerialStdio (bool blocking = false); + /// Destructeur. + ~SerialStdio (void); + /// Ouvre et paramètre le port série. + void open (const std::string &ttyname); + /// Ferme le port série, appellé automatiquement dans le destructeur de la + /// classe SerialBase. + void close (void); +}; + +#endif // serial_stdio_hh diff --git a/i/marvin/src/serial/test_serial.cc b/i/marvin/src/serial/test_serial.cc new file mode 100644 index 0000000..6c0b771 --- /dev/null +++ b/i/marvin/src/serial/test_serial.cc @@ -0,0 +1,88 @@ +// test_serial.cc - Programme de test pour le port série. +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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.hh" +//#include "date/date.hh" + +#include + +void +syntax (void) +{ + std::cout << "test_serial ttyname commandes... - Test la classe Serial.\n" + " a carte d'asservissement.\n" + " e échèle de 0 à 255 puis 0...\n" + " envois une valeur." << std::endl; +} +int +main (int argc, char **argv) +{ + try + { + if (argc < 3) + { + syntax (); + return 1; + } + Serial s; + s.open (argv[1]); + switch (argv[2][0]) + { + case 'a': + s.write ("!g1\r", 3); + //Date::getInstance ().wait (1000); + s.write ("!v0303\r", 7); + //Date::getInstance ().wait (5000); + s.write ("!s\r", 3); + break; + case 'e': + while (1) + { + for (int i = 0; i < 256; ++i) + { + std::cout << i << std::endl; + s.putchar (i); + //Date::getInstance ().wait (100); + } + for (int i = 255; i >= 0; --i) + { + std::cout << i << std::endl; + s.putchar (i); + //Date::getInstance ().wait (100); + } + } + break; + default: + int i = atoi (argv[2]); + std::cout << i << std::endl; + s.putchar (i); + break; + } + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + return 1; + } +} diff --git a/i/marvin/src/socket/Makefile.defs b/i/marvin/src/socket/Makefile.defs new file mode 100644 index 0000000..c125e61 --- /dev/null +++ b/i/marvin/src/socket/Makefile.defs @@ -0,0 +1,9 @@ +PROGRAMS += test_socket test_server + +net_OBJECTS = address.o server_socket.o socket_text.o + +test_socket_OBJECTS = test_socket.o $(net_OBJECTS) $(data_OBJECTS) +test_server_OBJECTS = test_server.o $(net_OBJECTS) $(data_OBJECTS) $(image_OBJECTS) + +test_socket: $(test_socket_OBJECTS) +test_server: $(test_server_OBJECTS) diff --git a/i/marvin/src/socket/address.cc b/i/marvin/src/socket/address.cc new file mode 100644 index 0000000..4567e2e --- /dev/null +++ b/i/marvin/src/socket/address.cc @@ -0,0 +1,93 @@ +// address.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Dufour Jérémy +// +// 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. +// +// Contact : +// Web: %WEB% +// Email: +// }}} +#include "address.hh" + +#include +#include + +/// Constructeur par défaut. +Address::Address (void) + : port_ (0) +{ + resolv (); +} + +/// Constructeur avec le port (serveur) +Address::Address (const int port) + : port_ (port) +{ + resolv (); +} +/// Constructeur avec hôte et port. +Address::Address (const std::string &host, const int port) + : host_ (host), port_ (port) +{ + resolv (); +} + +/// Constructeur à partir de la strcuture d'adresse. +Address::Address (const sockaddr *sa, socklen_t sl) +{ + // On vérifie que c'est vraiment un struct sockaddr_in + if (sl != sizeof (sockaddr_in)) + throw std::invalid_argument ("Invalide Adresse"); + sa_ = * reinterpret_cast (sa); +} + +/// Résolution de la structure d'adresse depuis l'hôte/port +void +Address::resolv (void) +{ + // Cas du serveur, pas d'hôte + if (host_.empty ()) + sa_.sin_addr.s_addr = htonl (INADDR_ANY); + else + { + // Convertion de l'hôte en structure adresse + if (!inet_pton (AF_INET, host_.c_str (), &sa_.sin_addr)) + { + hostent *host_info = gethostbyname (host_.c_str ()); + if (!host_info) + throw std::runtime_error (host_ + " : Hôte inconnue"); + } + } + sa_.sin_family = AF_INET; + // Convertion pour le passage sur le réseau + sa_.sin_port = htons (port_); +} + +/// Résolution de l'hôte/port depuis la structure d'adresse +void +Address::unresolv (void) +{ + hostent *host_info; + // Recherche de l'hôte + host_info = gethostbyaddr (reinterpret_cast (&(sa_.sin_addr)), sizeof sa_.sin_addr, AF_INET); + if (!host_info) + host_ = inet_ntoa (sa_.sin_addr); + else + host_ = host_info->h_name; + // Recherche du port + port_ = ntohs (sa_.sin_port); +} diff --git a/i/marvin/src/socket/address.hh b/i/marvin/src/socket/address.hh new file mode 100644 index 0000000..53fca07 --- /dev/null +++ b/i/marvin/src/socket/address.hh @@ -0,0 +1,76 @@ +#ifndef address_hh +#define address_hh +// address.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Dufour Jérémy +// +// 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. +// +// Contact : +// Web: %WEB% +// Email: +// }}} + +#include +#include +#include +#include + +#include +#include + +/// S'occupe de gérer les adresses. +class Address { + /// La structure sockaddr_in de type réseau/AF_INET. + sockaddr_in sa_; + /// Le nom d'hôte de l'adresse. + std::string host_; + /// Le numéro de port de l'adresse. + int port_; + + public: + /// Constructeur par défaut. + Address (void); + /// Constructeur pour le serveur. (port seulement) + Address (const int port); + /// Constructeur avec hôte et port. + Address (const std::string &host, const int port); + /// Constructeur à partir de la structure d'adresse. + Address (const sockaddr *sa, socklen_t sl); + /// Geter sockaddr. + const sockaddr *getSockaddr (void) const { return + reinterpret_cast (&sa_); } + /// Geter hôte. + const std::string &getHost (void) const { return host_; } + /// Geter port. + const int getPort (void) const { return port_; } + + private: + /// Résolution adresse vers hôte/port + void resolv (void); + /// Résolution hôte/port vers adresse + void unresolv (void); + +}; + +/// Surcharge de l'opérateur << pour l'affichage. +inline std::ostream & +operator<< (std::ostream &s, const Address &a) +{ + return s << a.getHost () << ':' << a.getPort (); +} + +#endif // address_hh diff --git a/i/marvin/src/socket/server_socket.cc b/i/marvin/src/socket/server_socket.cc new file mode 100644 index 0000000..539b94e --- /dev/null +++ b/i/marvin/src/socket/server_socket.cc @@ -0,0 +1,86 @@ +// server_socket.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Dufour Jérémy +// +// 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. +// +// Contact : +// Web: %WEB% +// Email: +// }}} +#include "server_socket.hh" + +#include "address.hh" + +#include +#include +#include + +/// Constructeur par défaut. +ServerSocket::ServerSocket (int port) +{ + bind (port); +} + +/// Bind le serveur sur un port d'écoute. +void +ServerSocket::bind (int port) +{ + static const int reuse_s = 1; + // Adresse sur un port (pas d'hote, écoutera de partout) + Address a (port); + // Création du socket IPv4 - TCP + socket_ = socket (PF_INET, SOCK_STREAM, 0); + if (socket_ == -1) + throw std::runtime_error ("Erreur de création du socket"); + // Socket réutilisable en cas d'arret "brutale" + if (setsockopt (socket_, SOL_SOCKET, SO_REUSEADDR, + &reuse_s, sizeof (int)) == -1) + throw std::runtime_error ("Erreur setsockopt : reusable"); + // Bindage du socket sur le port et l'adresse + if (::bind (socket_, a.getSockaddr (), sizeof (struct sockaddr_in)) == -1) + throw std::runtime_error ("Erreur de bind du socket"); + // On met le socket en attente de connexion + if (listen (socket_, 0) == -1) + throw std::runtime_error ("Erreur de listen"); +} + +// Accepte une nouvelle connexion. +int +ServerSocket::accept (void) const +{ + int s = ::accept (socket_, 0, 0); + if (s == -1) + throw std::runtime_error ("Erreur d'accept de nouvelles connexions"); + return s; +} + +// Accepte une nouvelle connexion et remplie Address. +int +ServerSocket::accept (Address &a) const +{ + sockaddr_in sa; + socklen_t sl = sizeof sa; + int s = ::accept (socket_, reinterpret_cast (&sa), + &sl); + if (s == -1) + { + throw std::runtime_error ("Erreur d'accept nouvelle connexion"); + } + // Récupération de l'adresse + a = Address (reinterpret_cast (&sa), sl); + return s; +} diff --git a/i/marvin/src/socket/server_socket.hh b/i/marvin/src/socket/server_socket.hh new file mode 100644 index 0000000..995ad01 --- /dev/null +++ b/i/marvin/src/socket/server_socket.hh @@ -0,0 +1,47 @@ +#ifndef server_socket_hh +#define server_socket_hh +// server_socket.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Dufour Jérémy +// +// 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. +// +// Contact : +// Web: %WEB% +// Email: +// }}} + +#include +class Address; + +/// Créer un server TCP pour le transfert des données. +class ServerSocket { + private: + /// L'identificateur du socket. + int socket_; + /// Bind le serveur sur un port d'écoute. + void bind (int port); + public: + /// Constructeur par défaut. + ServerSocket (int port); + /// Destructeur. + ~ServerSocket () { close (socket_); } + /// Accepte une nouvelle connexion et remplie Address. + int accept (Address &a) const; + /// Accepte une nouvelle connexion. + int accept (void) const; +}; +#endif // server_socket_hh diff --git a/i/marvin/src/socket/socket_text.cc b/i/marvin/src/socket/socket_text.cc new file mode 100644 index 0000000..da8a23b --- /dev/null +++ b/i/marvin/src/socket/socket_text.cc @@ -0,0 +1,236 @@ +// socket_text.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Dufour Jérémy +// +// 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. +// +// Contact : +// Web: %WEB% +// Email: +// }}} + +#include "socket_text.hh" + +#include "server_socket.hh" +#include "address.hh" + +#include +#include +#include +#include + +/// Taille maximum du header +static const unsigned max_header_size = 10; + +/// Constructeur du serveur +SocketText::SocketText (ServerSocket &ss) +: recvBufPos_ (0), numMessageBuffer_ (0) +{ + headBuf_.resize (max_header_size); + socket_ = ss.accept (); +} + +/// Constructeur du serveur avec addresse +SocketText::SocketText (ServerSocket &ss, Address &a) +: recvBufPos_ (0), numMessageBuffer_ (0) +{ + headBuf_.resize (max_header_size); + socket_ = ss.accept (a); +} + +/// Constructeur du client +SocketText::SocketText (const Address &a) +: recvBufPos_ (0), numMessageBuffer_ (0) +{ + headBuf_.resize (max_header_size); + // Création du socket + socket_ = socket (AF_INET, SOCK_STREAM, 0); + if (socket_ == -1) + throw std::runtime_error ("Erreur de création de socket"); + // Connexion au serveur + if (connect (socket_, a.getSockaddr (), sizeof (sockaddr_in)) == -1) + throw std::runtime_error ("Erreur de connexion au serveur"); +} + +/// Envoie d'un buffer de données. +SocketText & +SocketText::operator<< (const DataBuffer &d) +{ + // Copie du buffer + DataBuffer db (d); + // Récupération de la taille du message + unsigned size = db.size (); + // Données du buffer + std::vector data (size); + // On y place les données dedans + db.read ((uint8_t*) &data[0], size); + // Encode les données et le places dans le buffer d'envoie + encode (&data[0], size, db.type ()); + return *this; +} + +/// Reception d'un buffer de données. +SocketText & +SocketText::operator>> (DataBuffer &d) +{ + if (numMessageBuffer_ > 0) + { + // Récupération des informations du message qu'on veut retirer + unsigned int size = messList_.begin ()->size; + d.setType (messList_.begin ()->type); + // Récupération du message depuis le buffer + std::vector v (size); + std::memcpy (&v[0], &recvBuf_[0], size); + // Ecriture des données dans le buffer + d.write ((uint8_t*) &v[0], v.size ()); + // Suppression du message du buffer + recvBuf_.erase (recvBuf_.begin (), recvBuf_.begin () + size); + messList_.erase (messList_.begin ()); + bufferOldSize_ -= size; + bufferCurSize_ -= size; + recvBufPos_ -= size; + numMessageBuffer_--; + } + return *this; +} + +/// Envoie des données du buffer. +int +SocketText::send (void) +{ + int status; + status = ::send (socket_, &sendBuf_[0], sendBuf_.size (), MSG_NOSIGNAL); + if (status == -1 && errno != EWOULDBLOCK) + { + throw std::runtime_error ("Erreur de send de socket"); + } + if (status >= 0) + { + if (status != 0) + sendBuf_.erase (sendBuf_.begin (), sendBuf_.begin () + status); + return status; + } + return -1; +} + +/// Encodage du message (rajout du header) +void +SocketText::encode (const char *data, const int size, + const DataBuffer::dataType_e type) +{ + // Création du header (1H4242H) + std::ostringstream o; + // Rajout du type + if (!(o << type)) + throw std::runtime_error ("Erreur de convertion int -> string"); + o << "H"; + if (!(o << size )) + throw std::runtime_error ("Erreur de convertion int -> string"); + o << "H"; + // Recopie du header dans le buffer + std::string s = o.str (); + std::string::const_iterator sEnd = s.end (); + unsigned int compt = 0; + std::string::const_iterator i = s.begin (); + // Ecriture d'un header de taille fixe + while (compt < max_header_size) + { + if (i != sEnd) + { + sendBuf_.push_back (*i); + i++; + } + else + sendBuf_.push_back ('H'); + compt++; + } + // Rajout des données dans le buffer + for (int i = 0; i < size; i++) + sendBuf_.push_back (data[i]); +} + +/// Décodage du header. +int +SocketText::decode (void) +{ + mess_t header; + header.type = (DataBuffer::dataType_e) std::atoi (&headBuf_[0]); + header.size = std::atoi (&headBuf_[2]); + messList_.push_back (header); + return header.size; +} + +/// Reception de données dans un vecteur. +bool +SocketText::recv () +{ + // FIXME C'est KK + int status; + static unsigned int headerPos = 0; + + // On n'a pas reçu encore de quoi faire un header + if (headerPos < max_header_size) + { + status = ::recv (socket_, &headBuf_[headerPos], + max_header_size - headerPos, MSG_NOSIGNAL); + if (status == -1 && errno != EWOULDBLOCK) + throw std::runtime_error ("Erreur de send de socket"); + if (status > 0) + headerPos += status; + } + // Décodage du header + if (headerPos == max_header_size) + { + bufferOldSize_ = recvBuf_.size (); + bufferCurSize_ = bufferOldSize_ + decode (); + recvBuf_.resize (bufferCurSize_); + headerPos ++; + } + // Récupération du message lui-même + if (headerPos > max_header_size) + { + status = ::recv (socket_, &recvBuf_[recvBufPos_], + bufferCurSize_ - recvBufPos_, MSG_NOSIGNAL); + if (status == -1 && errno != EWOULDBLOCK) + throw std::runtime_error ("Erreur de send de socket"); + if (status > 0) + recvBufPos_ += status; + if (recvBufPos_ == bufferCurSize_) + { + headerPos = 0; + numMessageBuffer_++; + } + } + return numMessageBuffer_ > 0; +} + +/// Changement du bloquant non/bloquant. +void +SocketText::nonblock (bool flag) +{ + // Récupération de l'ancien flag. + int old = fcntl (socket_, F_GETFL, 0); + if (old == -1) + throw std::runtime_error ("Erreur de fcntl"); + // Change le flag. + if (flag) + old |= O_NONBLOCK; + else + old &= ~O_NONBLOCK; + // Ecrit le nouveau flag. + if (fcntl (socket_, F_SETFL, old) == -1) + throw std::runtime_error ("Erreur de fcntl"); +} diff --git a/i/marvin/src/socket/socket_text.hh b/i/marvin/src/socket/socket_text.hh new file mode 100644 index 0000000..b734753 --- /dev/null +++ b/i/marvin/src/socket/socket_text.hh @@ -0,0 +1,80 @@ +#ifndef socket_text_hh +#define socket_text_hh +// socket_text.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Dufour Jérémy +// +// 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. +// +// Contact : +// Web: %WEB% +// Email: +// }}} + +class ServerSocket; +class Address; +#include "data/data_buffer.hh" + +#include + +class SocketText { + /// Structure pour stocker les informations d'un message. + typedef struct { + unsigned int size; + DataBuffer::dataType_e type; + } mess_t; + std::vector messList_; + /// L'identificateur du socket. + int socket_; + /// Buffer pour le header. + std::vector headBuf_; + /// Buffer de reception de données. + std::vector recvBuf_; + /// Buffer d'émission de données. + std::vector sendBuf_; + /// Taille des buffers d'E/S XXX + unsigned int recvBufPos_; + unsigned int bufferOldSize_; + unsigned int bufferCurSize_; + /// Nombre de message dans le buffer + int numMessageBuffer_; + DataBuffer::dataType_e type_; + /// Encodage du message (rajout du header) + void encode (const char *data, const int size, + const DataBuffer::dataType_e type); + /// Décodage du header. + int decode (void); + + public: + /// Constructeur du serveur. + SocketText (ServerSocket &ss); + /// Constructeur du serveur avec addresse. + SocketText (ServerSocket &ss, Address &a); + /// Constructeur du client. + SocketText (const Address &a); + /// Envoie d'un buffer de données. + SocketText &operator<< (const DataBuffer &d); + /// Reception d'un buffer de données. + SocketText &operator>> (DataBuffer &d); + /// Envoie des données du buffer. + int send (void); + /// Reception de données dans un vecteur. + bool recv (void); + /// Changement du bloquant non/bloquant. + void nonblock (bool flag = true); +}; + +#endif // socket_text_hh diff --git a/i/marvin/src/socket/test_server.cc b/i/marvin/src/socket/test_server.cc new file mode 100644 index 0000000..81399ec --- /dev/null +++ b/i/marvin/src/socket/test_server.cc @@ -0,0 +1,91 @@ +// test_socket.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Dufour Jérémy +// +// 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. +// +// Contact : +// Web: %WEB% +// Email: +// }}} +#include "server_socket.hh" +#include "address.hh" +#include "socket_text.hh" +#include "data/data_input_file.hh" +#include "data/data_buffer.hh" +#include "image/raw_reader.hh" +#include "image/image.hh" + +#include +#include +#include + +int +main (int argc, char **argv) +{ + if (argc == 3) + { + try + { + // Serveur + ServerSocket ss (atoi (argv[1])); + Address a; + SocketText st (ss, a); + st.nonblock (); + // DataBuffer de reception + DataBuffer db; + uint8_t c = 'a'; + DataBuffer dbAskImg (&c, 1, 1, DataBuffer::AskImage); + st << dbAskImg; + st.send (); + // Reception des données + while (!st.recv ()); + st >> db; + if (db.type () != DataBuffer::Image) + throw std::runtime_error ("Echec de la transmission"); + RawReader reader (db, 360, 296, Image::rgb); + Image img (360, 296, Image::rgb); + img.read (reader); + std::ofstream file (argv[2]); + file.write ((char*) img.getBuf (), img.getBufSize ()); + +// // Récupération des données du DataBuffer +// std::vector v (db.size ()); +// db.read ((uint8_t*) &v[0], db.size ()); +// // Ecriture dans le fichier +// std::ofstream file (argv[2]); +// file.write (&v[0], v.size ()); +// DataBuffer db1; +// // Reception des données +// while (!st.recv ()); +// st >> db1; +// std::cout << ">> Reçu " << db1.size () << " bits." << std::endl; +// if (db1.type () != DataBuffer::Image) +// throw std::runtime_error ("Echec de la transmission"); + } + catch (const std::runtime_error &r) + { + std::cerr << argv[0] << ": " << r.what () << std::endl; + return 1; + } + } + else + { + std::cerr << "Syntaxe: " << argv[0] << " port out_file" << std::endl; + return 1; + } + return 0; +} diff --git a/i/marvin/src/socket/test_socket.cc b/i/marvin/src/socket/test_socket.cc new file mode 100644 index 0000000..f2beb51 --- /dev/null +++ b/i/marvin/src/socket/test_socket.cc @@ -0,0 +1,76 @@ +// test_socket.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Dufour Jérémy +// +// 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. +// +// Contact : +// Web: %WEB% +// Email: +// }}} + +#include "server_socket.hh" +#include "address.hh" +#include "socket_text.hh" +#include "data/data_input_file.hh" +#include "data/data_buffer.hh" + +#include +#include +#include +#include +#include + +int +main (int argc, char **argv) +{ + if (argc == 4) + { + try + { + // Récupération des données du fichier + DataInputFile df (argv[3]); + std::vector img (319680); + df.read (&img[0], 319680); + // Ecriture des données du fichier dans le buffer + DataBuffer db (&img[0], img.size (), img.size (), + DataBuffer::Image); + DataBuffer dbAnswer; + // Création du socket + char *h = argv[1]; + SocketText st (Address (h, atoi (argv[2]))); + st.nonblock (); + st >> dbAnswer; + if (dbAnswer.type () == DataBuffer::AskImage) + { + // Envoie des données + st << db; + while (st.send () != 0); + } + } + catch (const std::runtime_error &r) + { + std::cerr << argv[0] << ": " << r.what () << std::endl; + return 1; + } + } + else + { + std::cerr << "Syntaxe: " << argv[0] << " host port in_file" << + std::endl; + return 1; + } +} diff --git a/i/marvin/src/timer/Makefile.defs b/i/marvin/src/timer/Makefile.defs new file mode 100644 index 0000000..e7b58cc --- /dev/null +++ b/i/marvin/src/timer/Makefile.defs @@ -0,0 +1,7 @@ +PROGRAMS += test_timer + +timer_OBJECTS = timer.o + +test_timer_OBJECTS = test_timer.o $(timer_OBJECTS) + +test_timer: $(test_timer_OBJECTS) diff --git a/i/marvin/src/timer/test_timer.cc b/i/marvin/src/timer/test_timer.cc new file mode 100644 index 0000000..2e8c502 --- /dev/null +++ b/i/marvin/src/timer/test_timer.cc @@ -0,0 +1,47 @@ +// test_timer.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "timer.hh" + +#include +#include + +int +main (void) +{ + using namespace std; + cout << "timer 0 " << Timer::getProgramTime () << ' ' << + Timer::getRoundTime () << endl; + sleep (1); + cout << "timer 1 " << Timer::getProgramTime () << ' ' << + Timer::getRoundTime () << endl; + Timer::startRound (); + sleep (2); + cout << "timer 3 " << Timer::getProgramTime () << ' ' << + Timer::getRoundTime () << endl; + Timer::wait (1500); + cout << "timer 4.5 " << Timer::getProgramTime () << ' ' << + Timer::getRoundTime () << endl; + return 0; +} diff --git a/i/marvin/src/timer/timer.cc b/i/marvin/src/timer/timer.cc new file mode 100644 index 0000000..662e6de --- /dev/null +++ b/i/marvin/src/timer/timer.cc @@ -0,0 +1,89 @@ +// timer.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "timer.hh" + +#include +#include + +/// Retourne le temps depuis le début du programme en millisecondes. +int +Timer::getProgramTime (void) +{ + Timer &timer = getInstance (); + return getSystemTime () - timer.programStart_; +} + +/// Retourne le temps depuis le début du match en millisecondes. +int +Timer::getRoundTime (void) +{ + Timer &timer = getInstance (); + return timer.roundStart_ != -1 ? getSystemTime () - timer.roundStart_ : 0; +} + +/// Démare le match et enregistre le temps de départ. +void +Timer::startRound (void) +{ + Timer &timer = getInstance (); + timer.roundStart_ = getSystemTime (); +} + +/// Attend un nombre de millisecondes. Attention, le robot ne fait absolument +/// rien pendant cette periode ! +void +Timer::wait (int t) +{ + timespec ts, r; + ts.tv_sec = t / 1000; + ts.tv_nsec = (t % 1000) * 1000000; + nanosleep (&ts, &r); +} + +/// Retourne une référence vers l'instance unique. +Timer & +Timer::getInstance (void) +{ + static Timer instance; + return instance; +} + +/// Retourne la date système. +long long +Timer::getSystemTime (void) +{ + timeval tv; + gettimeofday (&tv, 0); + return ((long long) tv.tv_sec % 1000000) * 1000 + + (long long) tv.tv_usec / 1000; +} + +/// Constructeur. +Timer::Timer (void) +{ + programStart_ = getSystemTime (); + roundStart_ = -1; +} + diff --git a/i/marvin/src/timer/timer.hh b/i/marvin/src/timer/timer.hh new file mode 100644 index 0000000..341f1da --- /dev/null +++ b/i/marvin/src/timer/timer.hh @@ -0,0 +1,51 @@ +#ifndef timer_hh +#define timer_hh +// timer.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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. +// +// }}} + +/// Classe de chronométrage. +class Timer +{ + long long programStart_, roundStart_; + public: + /// Retourne le temps depuis le début du programme en millisecondes. + static int getProgramTime (void); + /// Retourne le temps depuis le début du match en millisecondes. + static int getRoundTime (void); + /// Démare le match et enregistre le temps de départ. + static void startRound (void); + /// Attend un nombre de millisecondes. Attention, le robot ne fait + /// absolument rien pendant cette periode ! + static void wait (int t); + private: + /// Retourne une référence vers l'instance unique. + static Timer &getInstance (void); + /// Retourne la date système. + static long long getSystemTime (void); + /// Constructeur. + Timer (void); +}; + +#endif // timer_hh diff --git a/i/marvin/src/utils/Makefile.defs b/i/marvin/src/utils/Makefile.defs new file mode 100644 index 0000000..43711a8 --- /dev/null +++ b/i/marvin/src/utils/Makefile.defs @@ -0,0 +1,15 @@ +PROGRAMS += test_any test_callback test_bind + +utils_OBJECTS = fd_set.o hexa.o + +test_any_OBJECTS = test_any.o + +test_callback_OBJECTS = test_callback.o + +test_bind_OBJECTS = test_bind.o + +test_any: $(test_any_OBJECTS) + +test_callback: $(test_callback_OBJECTS) + +test_bind: $(test_bind_OBJECTS) diff --git a/i/marvin/src/utils/any.hh b/i/marvin/src/utils/any.hh new file mode 100644 index 0000000..eabee96 --- /dev/null +++ b/i/marvin/src/utils/any.hh @@ -0,0 +1,117 @@ +#ifndef any_hh +#define any_hh +// any.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} + +#include +#include +#include + +/// This class can contain a data of any type. +class any +{ + class AbstractHolder; + AbstractHolder *holder_; + public: + /// Default constructor. + any (void); + /// Constructor. + template + explicit any (const T &value); + /// Copy constructor. + explicit any (const any &other); + /// Destructor. + ~any (void); + /// Swap content between two any objects. + any &swap (any &other); + /// Copy another any object. + any &operator= (const any &other); + /// Copy another object into this any. + template + any &operator= (const T &value); + /// Test if the any object is empty. + bool empty (void) const; + /// Return the std::type_info of the contained object. + const std::type_info &type (void) const; + private: + template + friend T *any_cast (any *rhs); + friend std::ostream &operator<< (std::ostream &os, const any &rhs); + /// Abstract holder class. + class AbstractHolder + { + public: + /// Destructor. + virtual ~AbstractHolder (void); + virtual const std::type_info &type (void) const = 0; + virtual AbstractHolder *clone (void) const = 0; + virtual std::ostream &print (std::ostream &os) const = 0; + }; + /// Holder class, templated. + template + class Holder : public AbstractHolder + { + public: + T value_; + public: + Holder (const T &value); + virtual const std::type_info &type (void) const; + virtual AbstractHolder *clone (void) const; + virtual std::ostream &print (std::ostream &os) const; + }; +}; + +/// Object throw if a any_cast returning a reference fail. +class bad_any_cast : public std::bad_cast +{ + public: + virtual const char * what() const throw() + { + return "bad_any_cast: illegal conversion"; + } +}; + +/// Return a pointer to the contained object or 0 on faillure. +template +T * +any_cast (any *rhs); + +/// Return a const pointer to the contained object or 0 on faillure. +template +const T * +any_cast (const any *rhs); + +/// Return a const reference to the contained object or throw a bad_any_cast +/// on faillure. +template +const T & +any_cast (const any &rhs); + +/// Print the contained object. +std::ostream & +operator<< (std::ostream &os, const any &rhs); + +#include "any.tcc" + +#endif // any_hh diff --git a/i/marvin/src/utils/any.tcc b/i/marvin/src/utils/any.tcc new file mode 100644 index 0000000..fd3dca6 --- /dev/null +++ b/i/marvin/src/utils/any.tcc @@ -0,0 +1,169 @@ +// any.tcc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} +#include "list_ostream_output.hh" + +#include + +/// Default constructor. +inline +any::any (void) + : holder_ (0) +{ +} + +/// Constructor. +template +any::any (const T &value) + : holder_ (new Holder (value)) +{ +} + +/// Copy constructor. +inline +any::any (const any &other) + : holder_ (other.holder_ ? other.holder_->clone () : 0) +{ +} + +/// Destructor. +inline +any::~any (void) +{ + delete holder_; +} + +/// Swap content between two any objects. +inline +any & +any::swap (any &other) +{ + std::swap (holder_, other.holder_); + return *this; +} + +/// Copy another any object. +inline +any & +any::operator= (const any &other) +{ + any (other).swap (*this); + return *this; +} + +/// Copy another object into this any. +template +any & +any::operator= (const T &value) +{ + any (value).swap (*this); + return *this; +} + +/// Test if the any object is empty. +inline +bool +any::empty (void) const +{ + return !holder_; +} + +/// Return the std::type_info of the contained object. +inline +const std::type_info & +any::type (void) const +{ + return holder_ ? holder_->type () : typeid (void); +} + +/// Destructor. +inline +any::AbstractHolder::~AbstractHolder (void) +{ +} + +template +any::Holder::Holder (const T &value) + : value_ (value) +{ +} + +template +const std::type_info & +any::Holder::type (void) const +{ + return typeid (T); +} + +template +any::AbstractHolder * +any::Holder::clone (void) const +{ + return new Holder (value_); +} + +template +std::ostream & +any::Holder::print (std::ostream &os) const +{ + return os << value_; +} + +/// Return a pointer to the contained object or 0 on faillure. +template +T * +any_cast (any *rhs) +{ + return rhs && rhs->type () == typeid (T) + ? &static_cast *> (rhs->holder_)->value_ + : 0; +} + +/// Return a const pointer to the contained object or 0 on faillure. +template +const T * +any_cast (const any *rhs) +{ + return any_cast (const_cast (rhs)); +} + +/// Return a const reference to the contained object or throw a bad_any_cast +/// on faillure. +template +const T & +any_cast (const any &rhs) +{ + const T *value = any_cast (&rhs); + if (!value) + throw bad_any_cast (); + return *value; +} + +/// Print the contained object. +inline +std::ostream & +operator<< (std::ostream &os, const any &rhs) +{ + return rhs.holder_->print (os); +} + diff --git a/i/marvin/src/utils/bind.hh b/i/marvin/src/utils/bind.hh new file mode 100644 index 0000000..a5630fe --- /dev/null +++ b/i/marvin/src/utils/bind.hh @@ -0,0 +1,91 @@ +#ifndef bind_hh +#define bind_hh +// bind.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} + +/// Class used to bind a function taking an argument to a functor taking no +/// argument. This is not supposed to be used directly, but using the bind +/// template function. +template +class ArgBinder +{ + /// Original function. + F func_; + /// Stored argument reference. + const A &arg_; + public: + /// Return value. + typedef R result_type; + public: + ArgBinder (F func, const A &arg) + : func_ (func), arg_ (arg) + { } + result_type operator () (void) + { + return func_ (arg_); + } +}; + +/// Bind a function taking an argument to a functor taking no argument. +/// Return value must be specified, other template arguments can be implied. +template +ArgBinder +bind (F func, const A &arg) +{ + return ArgBinder (func, arg); +} + +/// Class used to bind a member function to a functor taking no argument. +/// This is not supposed to be used directly, but using the bind template +/// function. +template +class ObjBinder +{ + /// Original member function. + R (C::*func_) (void); + /// Object pointer stored. + C *obj_; + public: + /// Return value. + typedef R result_type; + public: + ObjBinder (R (C::*func) (void), C *obj) + : func_ (func), obj_ (obj) + { } + result_type operator () (void) + { + (obj_->*func_) (); + } +}; + +/// Bind a member function to a functor taking no argument. All template +/// parameters must be specified. +template +ObjBinder +bind (R (C::*func) (void), C *obj) +{ + return ObjBinder (func, obj); +} + +#endif // bind_hh diff --git a/i/marvin/src/utils/callback.hh b/i/marvin/src/utils/callback.hh new file mode 100644 index 0000000..c5dc229 --- /dev/null +++ b/i/marvin/src/utils/callback.hh @@ -0,0 +1,95 @@ +#ifndef callback_hh +#define callback_hh +// callback.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} + +#include + +/// Class to store a callback. This callback can be of any type (function or +/// fonctor). +template +class Callback +{ + class AbstractHolder; + AbstractHolder *holder_; + public: + /// Return value. + typedef R result_type; + public: + /// Default constructor. Make an empty callback. + Callback (void); + /// Constructor. + template + Callback (T callback); + /// Copy constructor. + Callback (const Callback &other); + /// Destructor. + ~Callback (void); + /// Assignement operator. + Callback &operator= (const Callback &rhs); + /// Call the contained callback. + R operator () (void); + /// Swap the callback content with another callback. + void swap (Callback &rhs); + /// Change the contained callback. + template + Callback &operator= (T rhs); + /// Test if empty. + bool empty (void) const; + /// Test if not empty. + operator bool (void) const; + private: + /// Abstract container. + class AbstractHolder + { + public: + virtual ~AbstractHolder (void); + virtual R operator () (void) = 0; + virtual AbstractHolder *clone (void) = 0; + }; + /// Templated container. + template + class Holder : public AbstractHolder + { + private: + T callback_; + public: + Holder (T callback); + virtual R operator () (void); + virtual AbstractHolder *clone (void); + }; +}; + +/// Exception thrown on call of an empty callback. +class bad_callback : public std::runtime_error +{ + public: + bad_callback (void) : std::runtime_error ("bad_callback: empty callback call") + { + } +}; + +#include "callback.tcc" + +#endif // callback_hh diff --git a/i/marvin/src/utils/callback.tcc b/i/marvin/src/utils/callback.tcc new file mode 100644 index 0000000..d10df75 --- /dev/null +++ b/i/marvin/src/utils/callback.tcc @@ -0,0 +1,136 @@ +// callback.tcc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} + +#include + +/// Default constructor. Make an empty callback. +template +Callback::Callback (void) + : holder_ (0) +{ +} + +/// Constructor. +template +template +Callback::Callback (T callback) + : holder_ (new Holder (callback)) +{ +} + +/// Copy constructor. +template +Callback::Callback (const Callback &other) + : holder_ (other.holder_ ? other.holder_->clone () : 0) +{ +} + +/// Destructor. +template +Callback::~Callback (void) +{ + delete holder_; +} + +/// Assignement operator. +template +Callback & +Callback::operator= (const Callback &rhs) +{ + Callback (rhs).swap (*this); + return *this; +} + +/// Call the contained callback. +template +R +Callback::operator () (void) +{ + if (!holder_) + throw bad_callback (); + else + return (*holder_) (); +} + +/// Swap the callback content with another callback. +template +void +Callback::swap (Callback &rhs) +{ + std::swap (holder_, rhs.holder_); +} + +/// Change the contained callback. +template +template +Callback & +Callback::operator= (T rhs) +{ + Callback (rhs).swap (*this); + return *this; +} + +/// Test if empty. +template +bool +Callback::empty (void) const +{ + return !holder_; +} + +/// Test if not empty. +template +Callback::operator bool (void) const +{ + return !empty (); +} + +template +Callback::AbstractHolder::~AbstractHolder (void) +{ +} + +template +template +Callback::Holder::Holder (T callback) + : callback_ (callback) +{ +} + +template +template +R +Callback::Holder::operator () (void) +{ + return callback_ (); +} + +template +template +typename Callback::AbstractHolder * +Callback::Holder::clone (void) +{ + return new Holder (callback_); +} + diff --git a/i/marvin/src/utils/errno_exception.hh b/i/marvin/src/utils/errno_exception.hh new file mode 100644 index 0000000..58a636e --- /dev/null +++ b/i/marvin/src/utils/errno_exception.hh @@ -0,0 +1,48 @@ +#ifndef errno_exception_hh +#define errno_exception_hh +// errno_exception.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#include +#include +#include +#include + +/// Exception lancé lorsqu'une erreur est retournée par la libc. +class errno_exception : public std::exception +{ + int errno_; + std::string what_; + public: + errno_exception (const std::string &desc, int errno__) + : errno_ (errno__), what_ (desc + ": " + strerror (errno__)) { } + errno_exception (int errno__) + : errno_ (errno__), what_ (strerror (errno__)) { } + ~errno_exception (void) throw () { } + virtual const char* what () const throw () { return what_.c_str (); } + int getErrno (void) const { return errno_; } +}; + +#endif // errno_exception_hh diff --git a/i/marvin/src/utils/fd_set.cc b/i/marvin/src/utils/fd_set.cc new file mode 100644 index 0000000..998f3f5 --- /dev/null +++ b/i/marvin/src/utils/fd_set.cc @@ -0,0 +1,53 @@ +// fd_set.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "fd_set.hh" +#include "utils/errno_exception.hh" + +/// Wait for an read event or timeout in milliseconds, return true on read +/// event. +bool +FdSet::wait (int timeout/*-1*/) +{ + int r; + if (timeout == -1) + { + // Without timeout. + r = select (FD_SETSIZE, get (), 0, 0, 0); + } + else + { + // With timeout. + struct timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + r = select (FD_SETSIZE, get (), 0, 0, &tv); + } + if (r == -1) + // Error. + throw errno_exception (errno); + else + return r != 0; +} + diff --git a/i/marvin/src/utils/fd_set.hh b/i/marvin/src/utils/fd_set.hh new file mode 100644 index 0000000..3d6d44c --- /dev/null +++ b/i/marvin/src/utils/fd_set.hh @@ -0,0 +1,54 @@ +#ifndef fd_set_hh +#define fd_set_hh +// fd_set.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} + +#include + +/// Wrapper over libc fd_set. +class FdSet +{ + fd_set set_; + public: + /// Default constructor. + FdSet (void) + { FD_ZERO (&set_); } + /// Set a fd. + void set (int fd) + { FD_SET (fd, &set_); } + /// Clear a fd. + void clear (int fd) + { FD_CLR (fd, &set_); } + /// Test if a fd is set. + bool isSet (int fd) const + { return FD_ISSET (fd, &set_); } + /// Get pointer to internal fd_set. + fd_set *get (void) + { return &set_; } + /// Wait for an read event or timeout in milliseconds, return true on read + /// event. + bool wait (int timeout = -1); +}; + +#endif // fd_set_hh diff --git a/i/marvin/src/utils/hexa.cc b/i/marvin/src/utils/hexa.cc new file mode 100644 index 0000000..102458e --- /dev/null +++ b/i/marvin/src/utils/hexa.cc @@ -0,0 +1,113 @@ +// hexa.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 "hexa.hh" + +// Table de conversion en hexa. +static const char *digit2hexTbl_ = "0123456789abcdef"; + +// Table de conversion depuis l'hexa. +#define hI hexInvalid +static const int hex2digitTbl_[] = +{ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 0-15 */ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 16-31 */ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 32-47 */ + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9,hI,hI,hI,hI,hI,hI, /* 48-63 */ + hI,10,11,12,13,14,15,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 64-79 */ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 80-95 */ + hI,10,11,12,13,14,15,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 96-111 */ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 112-127 */ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 128-143 */ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 144-159 */ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 160-175 */ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 176-191 */ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 192-207 */ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 208-223 */ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 224-239 */ + hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI,hI, /* 240-255 */ +}; + +/// Converti un caractère hexa (0-9a-f) en chiffre (0-15). +int +hex2digit (char c) +{ + return hex2digitTbl_[static_cast (c)]; +} + +/// Converti un chiffre (0-15) en hexa (0-9a-f). +char +digit2hex (int d) +{ + return digit2hexTbl_[d]; +} + +/// Décode un mot non-signé (1 octets). +unsigned int +hexUnsignedChar2int (const char *s) +{ + int h1 = hex2digit (s[0]); + int h0 = hex2digit (s[1]); + if (h1 == hexInvalid || h0 == hexInvalid) + return hexInvalid; + return (unsigned char) (h1 << 4 | h0); +} + +/// Décode un mot signé (1 octets). +int +hexSignedChar2int (const char *s) +{ + int h1 = hex2digit (s[0]); + int h0 = hex2digit (s[1]); + if (h1 == hexInvalid || h0 == hexInvalid) + return hexInvalid; + return (signed char) (h1 << 4 | h0); +} + +/// Décode un mot non-signé (2 octets). +int +hexUnsignedShort2int (const char *s) +{ + int h3 = hex2digit (s[0]); + int h2 = hex2digit (s[1]); + int h1 = hex2digit (s[2]); + int h0 = hex2digit (s[3]); + if (h3 == hexInvalid || h2 == hexInvalid || h1 == hexInvalid || h0 == hexInvalid) + return hexInvalid; + return (unsigned short) (h3 << 12 | h2 << 8 | h1 << 4 | h0); +} + +/// Décode un mot signé (2 octets). +int +hexSignedShort2int (const char *s) +{ + int h3 = hex2digit (s[0]); + int h2 = hex2digit (s[1]); + int h1 = hex2digit (s[2]); + int h0 = hex2digit (s[3]); + if (h3 == hexInvalid || h2 == hexInvalid || h1 == hexInvalid || h0 == hexInvalid) + return hexInvalid; + return (short) (h3 << 12 | h2 << 8 | h1 << 4 | h0); +} + diff --git a/i/marvin/src/utils/hexa.hh b/i/marvin/src/utils/hexa.hh new file mode 100644 index 0000000..f7d3399 --- /dev/null +++ b/i/marvin/src/utils/hexa.hh @@ -0,0 +1,49 @@ +#ifndef hexa_hh +#define hexa_hh +// hexa.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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. +// +// }}} + +/// Constante retournée pour une valeur invalide. +const int hexInvalid = 1 << 24; + +/// Converti un caractère hexa (0-9a-f) en chiffre (0-15). +int hex2digit (char c); + +/// Converti un chiffre (0-15) en hexa (0-9a-f). +char digit2hex (int d); + +/// Décode un mot non-signé (1 octets). +unsigned int hexUnsignedChar2int (const char *s); + +/// Décode un mot signé (1 octets). +int hexSignedChar2int (const char *s); + +/// Décode un mot non-signé (2 octets). +int hexUnsignedShort2int (const char *s); + +/// Décode un mot signé (2 octets). +int hexSignedShort2int (const char *s); + +#endif // hexa_hh diff --git a/i/marvin/src/utils/list_ostream_output.hh b/i/marvin/src/utils/list_ostream_output.hh new file mode 100644 index 0000000..3863a39 --- /dev/null +++ b/i/marvin/src/utils/list_ostream_output.hh @@ -0,0 +1,62 @@ +#ifndef list_ostream_output_hh +#define list_ostream_output_hh +// list_ostream_output.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#include +#include +#include +#include +#include + +/// Output any list. +template +std::ostream & +output_list (std::ostream &os, const T &list) +{ + os << "( "; + std::copy (list.begin (), list.end (), + std::ostream_iterator (os, " ")); + os << ')'; + return os; +} + +/// Output a list. +template +std::ostream & +operator<< (std::ostream &os, const std::list &list) +{ + return output_list (os, list); +} + +/// Output a vector. +template +std::ostream & +operator<< (std::ostream &os, const std::vector &list) +{ + return output_list (os, list); +} + +#endif // list_ostream_output_hh diff --git a/i/marvin/src/utils/mathutil.hh b/i/marvin/src/utils/mathutil.hh new file mode 100644 index 0000000..d58cec3 --- /dev/null +++ b/i/marvin/src/utils/mathutil.hh @@ -0,0 +1,57 @@ +#ifndef mathutil_hh +#define mathutil_hh +// mathutil.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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 + +/// Normalise l'angle entre -pi et +pi. +double +angleNorm (double a); + +/// Converti en coordonnées polaires. +void +toPolar (double x, double y, double &r, double &a); + +/// Normalise l'angle en [-pi..+pi). +inline double +angleNorm (double a) +{ + while (a >= M_PI) + a -= 2.0 * M_PI; + while (a < -M_PI) + a += 2.0 * M_PI; + return a; +} + +/// Converti en coordonnées polaires. +inline void +toPolar (double x, double y, double &r, double &a) +{ + // Rayon. + r = sqrt (x * x + y * y); + a = y > 0 ? acos (x / r) : -acos (x / r); +} + +#endif // mathutil_hh diff --git a/i/marvin/src/utils/meta/Makefile.defs b/i/marvin/src/utils/meta/Makefile.defs new file mode 100644 index 0000000..5650f40 --- /dev/null +++ b/i/marvin/src/utils/meta/Makefile.defs @@ -0,0 +1,5 @@ +PROGRAMS += test_is_string + +test_is_string_OBJECTS = test_is_string.o + +test_is_string: $(test_is_string_OBJECTS) diff --git a/i/marvin/src/utils/meta/is_string.hh b/i/marvin/src/utils/meta/is_string.hh new file mode 100644 index 0000000..99782d2 --- /dev/null +++ b/i/marvin/src/utils/meta/is_string.hh @@ -0,0 +1,45 @@ +#ifndef is_string_hh +#define is_string_hh +// is_string.hh +// {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} + +#include + +namespace meta { + +template +struct isString +{ + static const bool value = false; +}; + +template<> +struct isString +{ + static const bool value = true; +}; + +} // namespace meta + +#endif // is_string_hh diff --git a/i/marvin/src/utils/meta/test_is_string.cc b/i/marvin/src/utils/meta/test_is_string.cc new file mode 100644 index 0000000..c61483a --- /dev/null +++ b/i/marvin/src/utils/meta/test_is_string.cc @@ -0,0 +1,35 @@ +// test_is_string.cc +// {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} +#include "is_string.hh" + +#include + +int +main (void) +{ + std::cout << "std::string " << meta::isString::value << std::endl; + std::cout << "int " << meta::isString::value << std::endl; + std::cout << "char * " << meta::isString::value << std::endl; + return 0; +} diff --git a/i/marvin/src/utils/non_copyable.hh b/i/marvin/src/utils/non_copyable.hh new file mode 100644 index 0000000..e788365 --- /dev/null +++ b/i/marvin/src/utils/non_copyable.hh @@ -0,0 +1,41 @@ +#ifndef non_copyable_hh +#define non_copyable_hh +// non_copyable.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// 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. +// +// }}} + +/// Toute classe qui hérite de NonCopyable ne peut pas être copiée. Cela est +/// assuré par le fait que l'operator= et le constructeur de recopie sont +/// privé. +/// Idée originale : Boost.org. +class NonCopyable +{ + public: + NonCopyable (void) { } + private: + NonCopyable (const NonCopyable &); + const NonCopyable &operator= (const NonCopyable &); +}; + +#endif // non_copyable_hh diff --git a/i/marvin/src/utils/test_any.cc b/i/marvin/src/utils/test_any.cc new file mode 100644 index 0000000..85d2319 --- /dev/null +++ b/i/marvin/src/utils/test_any.cc @@ -0,0 +1,50 @@ +// test_any.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} +#include "any.hh" + +#include +#include +#include + +int +main (int argc, char **argv) +{ + any a (45), b (std::string ("toto")), c (5.4), d (std::string ("5")); + try + { + // Direct call to output operator. + std::cout << a << ' ' << b << ' ' << c << ' ' << d << std::endl; + // Here, it tries to extract the contained data. + std::cout << any_cast (a) << std::endl; + std::cout << any_cast (b) << std::endl; + std::cout << any_cast (c) << std::endl; + std::cout << any_cast (d) << std::endl; + } + catch (const std::exception &e) + { + std::cout << e.what () << std::endl; + return 1; + } + return 0; +} diff --git a/i/marvin/src/utils/test_bind.cc b/i/marvin/src/utils/test_bind.cc new file mode 100644 index 0000000..b883cbe --- /dev/null +++ b/i/marvin/src/utils/test_bind.cc @@ -0,0 +1,57 @@ +// test_bind.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} +#include "bind.hh" + +#include + +void +f (int i) +{ + std::cout << "f " << i << std::endl; +} + +void +g (const char *s) +{ + std::cout << "g " << s << std::endl; +} + +class H +{ + public: + void f (void) + { + std::cout << "h" << std::endl; + } +}; + +int +main (void) +{ + // Create the callback and execute it. + bind (&f, 42) (); + bind (&g, "hello world !") (); + H h; + bind (&H::f, &h) (); +} diff --git a/i/marvin/src/utils/test_callback.cc b/i/marvin/src/utils/test_callback.cc new file mode 100644 index 0000000..2a5b6f0 --- /dev/null +++ b/i/marvin/src/utils/test_callback.cc @@ -0,0 +1,84 @@ +// test_callback.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~schodet/ +// Email: +// }}} +#include "callback.hh" + +#include + +void +f (void) +{ + std::cout << " f ()" << std::endl; +} + +int +i (void) +{ + static int v = 42; + std::cout << " i () = " << v << std::endl; + return v++; +} + +struct G +{ + void operator () (void) + { + std::cout << " g ()" << std::endl; + } +}; + +int +main (void) +{ + G g; + Callback cf (f); + Callback cg (g); + Callback ch; + Callback ci (i); + try + { + std::cout << "cf" << std::endl; + cf (); + std::cout << "cg" << std::endl; + cg (); + cf = cg; + cg = f; + std::cout << "cf" << std::endl; + cf (); + std::cout << "cg" << std::endl; + cg (); + std::cout << "cf " << (cf ? "not " : "") << "empty" << std::endl; + std::cout << "ch " << (ch ? "not " : "") << "empty" << std::endl; + std::cout << "ci" << std::endl; + int r = ci (); + std::cout << "ci = " << r << std::endl; + std::cout << "ch" << std::endl; + ch (); + } + catch (const std::exception &e) + { + std::cout << e.what () << std::endl; + return 1; + } + return 0; +} -- cgit v1.2.3