summaryrefslogtreecommitdiff
path: root/2003/i/buzz/src/motor
diff options
context:
space:
mode:
Diffstat (limited to '2003/i/buzz/src/motor')
-rw-r--r--2003/i/buzz/src/motor/Makefile.defs8
-rw-r--r--2003/i/buzz/src/motor/motor.cc155
-rw-r--r--2003/i/buzz/src/motor/motor.h40
-rw-r--r--2003/i/buzz/src/motor/test_motor.cc32
4 files changed, 235 insertions, 0 deletions
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 <iostream>
+
+#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 <iostream>
+#include <unistd.h>
+
+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;
+ }
+}