From 6ebeaf91a4f64658e37d64307153cc86661f3ee0 Mon Sep 17 00:00:00 2001 From: uid4009 Date: Mon, 26 May 2003 09:00:02 +0000 Subject: Ajout de motor. --- 2003/i/buzz/src/motor/Makefile.defs | 8 ++ 2003/i/buzz/src/motor/motor.cc | 155 ++++++++++++++++++++++++++++++++++++ 2003/i/buzz/src/motor/motor.h | 40 ++++++++++ 2003/i/buzz/src/motor/test_motor.cc | 32 ++++++++ 4 files changed, 235 insertions(+) create mode 100644 2003/i/buzz/src/motor/Makefile.defs create mode 100644 2003/i/buzz/src/motor/motor.cc create mode 100644 2003/i/buzz/src/motor/motor.h create mode 100644 2003/i/buzz/src/motor/test_motor.cc (limited to '2003') diff --git a/2003/i/buzz/src/motor/Makefile.defs b/2003/i/buzz/src/motor/Makefile.defs new file mode 100644 index 0000000..27a3470 --- /dev/null +++ b/2003/i/buzz/src/motor/Makefile.defs @@ -0,0 +1,8 @@ +TARGETS += test_motor +LIBS += motor.a +test_motor_SOURCES = test_motor.cc motor.a serial.a erreur.a config.a +motor_a_SOURCES = motor.cc + +test_motor: $(test_motor_SOURCES:%.cc=%.o) + +motor.a: ${motor_a_SOURCES:%.cc=motor.a(%.o)} diff --git a/2003/i/buzz/src/motor/motor.cc b/2003/i/buzz/src/motor/motor.cc new file mode 100644 index 0000000..93d36b2 --- /dev/null +++ b/2003/i/buzz/src/motor/motor.cc @@ -0,0 +1,155 @@ +// motor.cc +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet +// +#include "motor.h" +#include "config/config.h" + +#include + +#define MOTOR_DEV "/dev/tty00" + +// Constructeur. +Motor::Motor (void) : m_serial (MOTOR_DEV) +{ + m_recv_n = 0; + running = false; + // Paramètres par defaut. + m_speed = m_accel = m_raccel = m_rspeed = 10; + // Lit la conf. + Config rc ("rc/motor"); + while (!rc.eof ()) + { + if (rc.isId ("accel")) + { + rc.getId (); + rc >> m_accel; + } + else if (rc.isId ("speed")) + { + rc.getId (); + rc >> m_speed; + } + else if (rc.isId ("raccel")) + { + rc.getId (); + rc >> m_raccel; + } + else if (rc.isId ("rspeed")) + { + rc.getId (); + rc >> m_rspeed; + } + else rc.noId (); + } +} + +// Destructeur. +Motor::~Motor (void) +{ +} + +// Avance d'une distance d, à la vitesse s. +void +Motor::goDistance (int d, int s) +{ + if (d > 0) + send ('F', d, s == -1 ? m_speed : s, m_accel); + else + send ('R', -d, s == -1 ? m_speed : s, m_accel); + running = true; +} + +// Tourne d'un angle d, à la vitesse s. +void +Motor::turnAngle (int d, int s) +{ + if (d > 0) + send ('G', d, s == -1 ? m_speed : s, m_accel); + else + send ('D', -d, s == -1 ? m_speed : s, m_accel); + running = true; +} + +// Traite les message entrants. +void +Motor::readMsg (void) +{ + while (recv ()) + { + // Vérifie la validitée du message. + if (m_recv_buf[0] == '<' && m_recv_buf[m_recv_n - 1] == '>') + { + switch (m_recv_buf[1]) + { + case 'Y': + running = false; + break; + } + } + m_recv_n = 0; + } +} + +// Envoie une commande. +void +Motor::send (char com, int a1, int a2, int a3) +{ + char data[32]; + int n = 0; + int be, le; + data[n++] = '<'; + data[n++] = com; + if (a1 != -1) + { + be = (a1 >> 8) & 0xff; + le = a1 & 0xff; + data[n++] = '\\'; + data[n++] = be; + data[n++] = '\\'; + data[n++] = le; + if (a2 != -1) + { + data[n++] = ';'; + be = (a2 >> 8) & 0xff; + le = a2 & 0xff; + data[n++] = '\\'; + data[n++] = be; + data[n++] = '\\'; + data[n++] = le; + if (a3 != -1) + { + data[n++] = ';'; + be = (a3 >> 8) & 0xff; + le = a3 & 0xff; + data[n++] = '\\'; + data[n++] = be; + data[n++] = '\\'; + data[n++] = le; + } + } + } + data[n++] = '>'; + m_serial.write (data, n); +} + +// Traite des éventuels retours, retourne true si commande finie. +bool +Motor::recv (void) +{ + int c; + while ((c = m_serial.getchar ()) != -1) + { + if (c == '<') m_recv_n = 0; + if (m_recv_n >= 63) m_recv_n = 0; + m_recv_buf[m_recv_n++] = c; + if (c == '>') + { + m_recv_buf[m_recv_n] = '\0'; + cout << "motor " << m_recv_buf << endl; + return true; + } + } + return false; +} + diff --git a/2003/i/buzz/src/motor/motor.h b/2003/i/buzz/src/motor/motor.h new file mode 100644 index 0000000..330ce88 --- /dev/null +++ b/2003/i/buzz/src/motor/motor.h @@ -0,0 +1,40 @@ +#ifndef motor_h +#define motor_h +// motor.h +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet + +#include "serial/serial.h" + +class Motor +{ + Serial m_serial; + // Buffer d'entrée. + char m_recv_buf[64]; + int m_recv_n; + // Paramètre vitesse/acceleration. + int m_accel, m_speed; + int m_raccel, m_rspeed; + // Operations en cours. + bool running; + public: + // Constructeur. + Motor (void); + // Destructeur. + ~Motor (void); + // Avance d'une distance d, à la vitesse s. + void goDistance (int d, int s = -1); + // Tourne d'un angle d, à la vitesse s. + void turnAngle (int d, int s = -1); + // Retourne true si la commande est terminée. + bool finnish (void) { readMsg (); return !running; } + // Traite les message entrants. + void readMsg (void); + private: + // Envoie une commande. + void send (char com, int a1 = -1, int a2 = -1, int a3 = -1); + // Traite des éventuels retours, retourne true si commande finie. + bool recv (void); +}; + +#endif // motor_h diff --git a/2003/i/buzz/src/motor/test_motor.cc b/2003/i/buzz/src/motor/test_motor.cc new file mode 100644 index 0000000..636d743 --- /dev/null +++ b/2003/i/buzz/src/motor/test_motor.cc @@ -0,0 +1,32 @@ +// test_motor.cc +// buzz - Programme du robot Efrei Robotique I1-I2 2003 +// Copyright (C) 2003 Nicolas Schodet +// +#include "motor.h" +#include "erreur/erreur.h" + +#include +#include + +int +main (void) +{ + try + { + Motor m; + m.goDistance (2000); + while (!m.finnish ()) sleep (1); + m.turnAngle (180); + while (!m.finnish ()) sleep (1); + m.goDistance (2000); + while (!m.finnish ()) sleep (1); + m.turnAngle (-180); + while (!m.finnish ()) sleep (1); + return 0; + } + catch (Erreur &e) + { + cerr << e.what () << endl; + return 1; + } +} -- cgit v1.2.3