summaryrefslogtreecommitdiff
path: root/2003/i/buzz/src/motor/motor.cc
diff options
context:
space:
mode:
Diffstat (limited to '2003/i/buzz/src/motor/motor.cc')
-rw-r--r--2003/i/buzz/src/motor/motor.cc155
1 files changed, 155 insertions, 0 deletions
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;
+}
+