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.cc300
1 files changed, 284 insertions, 16 deletions
diff --git a/2003/i/buzz/src/motor/motor.cc b/2003/i/buzz/src/motor/motor.cc
index 93d36b2..00880bb 100644
--- a/2003/i/buzz/src/motor/motor.cc
+++ b/2003/i/buzz/src/motor/motor.cc
@@ -4,71 +4,220 @@
//
#include "motor.h"
#include "config/config.h"
+#include "date/date.h"
#include <iostream>
#define MOTOR_DEV "/dev/tty00"
+#define MOTOR_DEBUG
+
+// Pointeur vers l'instance unique.
+Motor *Motor::m_instance = 0;
+
// Constructeur.
Motor::Motor (void) : m_serial (MOTOR_DEV)
{
m_recv_n = 0;
- running = false;
+ m_running = false;
+ m_capteursFetch = false;
+ m_capteurs = 1;
+ m_last_com = 'Z';
+ m_last_a1 = m_last_a2 = m_last_a3 = -1;
// Paramètres par defaut.
m_speed = m_accel = m_raccel = m_rspeed = 10;
+ m_ed2 = 190;
+ m_x = 0.0;
+ m_y = 0.0;
+ m_dir = 90.0;
+ m_factor = m_rfactor = 1.0;
// Lit la conf.
Config rc ("rc/motor");
while (!rc.eof ())
{
- if (rc.isId ("accel"))
+ if (rc.isId ("go"))
{
rc.getId ();
- rc >> m_accel;
+ rc >> m_inf_speed;
+ rc >> m_inf_accel;
+ }
+ else if (rc.isId ("turnangle"))
+ {
+ rc.getId ();
+ rc >> m_rspeed;
+ rc >> m_raccel;
}
- else if (rc.isId ("speed"))
+ else if (rc.isId ("godistance"))
{
rc.getId ();
rc >> m_speed;
+ rc >> m_accel;
}
- else if (rc.isId ("raccel"))
+ else if (rc.isId ("turn"))
{
rc.getId ();
- rc >> m_raccel;
+ rc >> m_inf_rspeed;
+ rc >> m_inf_raccel;
}
- else if (rc.isId ("rspeed"))
+ else if (rc.isId ("entraxe"))
{
rc.getId ();
- rc >> m_rspeed;
+ rc >> m_ed2;
+ m_ed2 /= 2;
+ }
+ else if (rc.isId ("xorig"))
+ {
+ rc.getId ();
+ rc >> m_x;
+ }
+ else if (rc.isId ("yorig"))
+ {
+ rc.getId ();
+ rc >> m_y;
+ }
+ else if (rc.isId ("dirorig"))
+ {
+ rc.getId ();
+ rc >> m_dir;
+ }
+ else if (rc.isId ("factors"))
+ {
+ rc.getId ();
+ rc >> m_factor >> m_rfactor;
}
else rc.noId ();
}
+ // Mémorise le pointeur d'instance.
+ m_instance = this;
}
// Destructeur.
Motor::~Motor (void)
{
+ stop ();
+ m_instance = 0;
+}
+
+// Change la position courante.
+void
+Motor::setPos (double x, double y)
+{
+ m_x = x; m_y = y;
+}
+
+// Change la position courante.
+void
+Motor::setPos (double x, double y, double dir)
+{
+ m_x = x; m_y = y; m_dir = dir;
+}
+
+// Récupère la position courante.
+void
+Motor::getPos (double &x, double &y, double &dir)
+{
+ x = m_x; y = m_y; dir = m_dir;
}
// Avance d'une distance d, à la vitesse s.
void
Motor::goDistance (int d, int s)
{
+ d = (int) ((double) d * m_factor);
if (d > 0)
- send ('F', d, s == -1 ? m_speed : s, m_accel);
+ send ('F', d, m_accel, s == -1 ? m_speed : s);
else
- send ('R', -d, s == -1 ? m_speed : s, m_accel);
- running = true;
+ send ('R', -d, m_accel, s == -1 ? m_speed : s);
+ m_running = true;
}
// Tourne d'un angle d, à la vitesse s.
void
Motor::turnAngle (int d, int s)
{
+ d = (int) ((double) d * m_rfactor);
if (d > 0)
- send ('G', d, s == -1 ? m_speed : s, m_accel);
+ send ('G', d, m_raccel, s == -1 ? m_rspeed : s);
else
- send ('D', -d, s == -1 ? m_speed : s, m_accel);
- running = true;
+ send ('D', -d, m_raccel, s == -1 ? m_rspeed : s);
+ m_running = true;
+}
+
+// Avance indéfiniment à la vitesse s.
+void
+Motor::go (int s)
+{
+ if (s == 0)
+ send ('f', m_inf_accel, m_inf_speed);
+ else if (s > 0)
+ send ('f', m_inf_accel, s);
+ else
+ send ('r', m_inf_accel, -s);
+ m_running = true;
+}
+
+// Avance indéfiniment à la vitesse sg et sd, respectivement pour le
+// moteur gauche et droit.
+void
+Motor::go (int sg, int sd)
+{
+ // TODO.
+}
+
+// Tourne indéfiniment à la vitesse s.
+void
+Motor::turn (int s)
+{
+ if (s == 0)
+ send ('g', m_inf_raccel, m_inf_rspeed);
+ else if (s > 0)
+ send ('g', m_inf_raccel, s);
+ else
+ send ('d', m_inf_raccel, -s);
+ m_running = true;
+}
+
+// Avance indéfiniment en tournant à la vitesse max s, de rayon r.
+void
+Motor::goTurn (int s, int r)
+{
+ int s2;
+ if (r == 0)
+ go (s);
+ else
+ {
+ s2 = s * (r - m_ed2) / (r + m_ed2);
+ if (r < 0)
+ go (s2, s);
+ else
+ go (s, s2);
+ }
+}
+
+// Stoppe.
+void
+Motor::stop (void)
+{
+ send ('s');
+ m_running = true;
+}
+
+// Stop progressif.
+void
+Motor::stopp (void)
+{
+ send ('S');
+ m_running = true;
+}
+
+// Reset.
+void
+Motor::reset (void)
+{
+ send ('Z');
+ m_running = true;
+ //for (int i = 0; i < 40; ++i)
+ // send ('+');
}
// Traite les message entrants.
@@ -83,7 +232,28 @@ Motor::readMsg (void)
switch (m_recv_buf[1])
{
case 'Y':
- running = false;
+ m_running = false;
+ break;
+ case '?':
+ if (m_running || m_capteursFetch || !m_servoFinnish)
+ send (m_last_com, m_last_a1, m_last_a2, m_last_a3);
+ break;
+ case 'h':
+ m_capteurs = m_recv_buf[2];
+ m_capteursFetch = false;
+ break;
+ case 'q':
+ m_doneG = m_recv_buf[2] << 24 |
+ m_recv_buf[3] << 16 |
+ m_recv_buf[4] << 8 |
+ m_recv_buf[5];
+ m_doneD = m_recv_buf[6] << 24 |
+ m_recv_buf[7] << 16 |
+ m_recv_buf[8] << 8 |
+ m_recv_buf[9];
+ break;
+ case 'H':
+ m_servoFinnish = true;
break;
}
}
@@ -91,6 +261,12 @@ Motor::readMsg (void)
}
}
+// Traite l'information de bouclage.
+void
+Motor::readTicks (int g, int d)
+{
+}
+
// Envoie une commande.
void
Motor::send (char com, int a1, int a2, int a3)
@@ -98,6 +274,10 @@ Motor::send (char com, int a1, int a2, int a3)
char data[32];
int n = 0;
int be, le;
+ m_last_com = com;
+ m_last_a1 = a1;
+ m_last_a2 = a2;
+ m_last_a3 = a3;
data[n++] = '<';
data[n++] = com;
if (a1 != -1)
@@ -146,10 +326,98 @@ Motor::recv (void)
if (c == '>')
{
m_recv_buf[m_recv_n] = '\0';
- cout << "motor " << m_recv_buf << endl;
+#ifdef MOTOR_DEBUG
+ if (m_recv_buf[1] != 'h')
+ cout << "motor " << m_recv_buf << endl;
+#endif // MOTOR_DEBUG
return true;
}
}
return false;
}
+// Demande les infos capteurs.
+void
+Motor::fetchCapteurs (void)
+{
+ send ('h');
+ m_capteursFetch = true;
+}
+
+// Attend le top départ.
+void
+Motor::waitRound (void)
+{
+ while (!(getCapteurs () & 8))
+ {
+ Date::wait (50);
+ }
+}
+
+// Récupère la distance parcouru.
+int
+Motor::getDone (void)
+{
+ m_doneG = 0;
+ while (m_doneG == 0)
+ {
+ send ('q');
+ for (int i = 0; i < 10 && m_doneG == 0; ++i)
+ {
+ readMsg ();
+ Date::wait (50);
+ }
+ }
+ return ((m_doneG - m_doneGs) + (m_doneD - m_doneDs) / 2);
+}
+
+// Mémorise la distance parcourue pour le moment.
+void
+Motor::setDone (void)
+{
+ m_doneG = 0;
+ while (m_doneG == 0)
+ {
+ send ('q');
+ for (int i = 0; i < 10 && m_doneG == 0; ++i)
+ {
+ readMsg ();
+ Date::wait (50);
+ }
+ }
+ m_doneDs = m_doneD;
+ m_doneGs = m_doneG;
+}
+
+// Gère les servos.
+void
+Motor::setServo (int s)
+{
+ m_servoFinnish = false;
+ while (!m_servoFinnish)
+ {
+ send ('H', s);
+ for (int i = 0; i < 10 && !m_servoFinnish; ++i)
+ {
+ readMsg ();
+ Date::wait (50);
+ }
+ }
+}
+
+// Retourne les infos capteurs.
+int
+Motor::getCapteurs (void)
+{
+ m_capteursFetch = true;
+ while (m_capteursFetch)
+ {
+ send ('h');
+ for (int i = 0; i < 10 && m_capteursFetch; ++i)
+ {
+ readMsg ();
+ Date::wait (50);
+ }
+ }
+ return m_capteurs;
+}