From 78c6eb9bba686db209f9eb0886b5f3b2c0af3a33 Mon Sep 17 00:00:00 2001 From: schodet Date: Wed, 10 Sep 2003 20:18:46 +0000 Subject: Apres 2003 --- 2003/i/buzz/src/motor/Makefile.defs | 2 +- 2003/i/buzz/src/motor/motor.cc | 300 ++++++++++++++++++++++++++++++++++-- 2003/i/buzz/src/motor/motor.h | 60 +++++++- 2003/i/buzz/src/motor/test_motor.cc | 59 +++++-- 4 files changed, 393 insertions(+), 28 deletions(-) (limited to '2003/i/buzz/src/motor') diff --git a/2003/i/buzz/src/motor/Makefile.defs b/2003/i/buzz/src/motor/Makefile.defs index 27a3470..fa3c82b 100644 --- a/2003/i/buzz/src/motor/Makefile.defs +++ b/2003/i/buzz/src/motor/Makefile.defs @@ -1,6 +1,6 @@ TARGETS += test_motor LIBS += motor.a -test_motor_SOURCES = test_motor.cc motor.a serial.a erreur.a config.a +test_motor_SOURCES = test_motor.cc motor.a serial.a erreur.a config.a date.a motor_a_SOURCES = motor.cc test_motor: $(test_motor_SOURCES:%.cc=%.o) 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 #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; +} diff --git a/2003/i/buzz/src/motor/motor.h b/2003/i/buzz/src/motor/motor.h index 330ce88..a51b140 100644 --- a/2003/i/buzz/src/motor/motor.h +++ b/2003/i/buzz/src/motor/motor.h @@ -15,21 +15,77 @@ class Motor // Paramètre vitesse/acceleration. int m_accel, m_speed; int m_raccel, m_rspeed; + int m_inf_accel, m_inf_speed; + int m_inf_raccel, m_inf_rspeed; + // Paramètres géométriques. + int m_ed2; + double m_factor, m_rfactor; // Operations en cours. - bool running; + bool m_running; + char m_last_com; + int m_last_a1, m_last_a2, m_last_a3; + // Calcul de position. + double m_x, m_y, m_dir; + // Capteurs. + char m_capteurs; + bool m_capteursFetch; + // Servo. + bool m_servoFinnish; + // Distance effectuée. + int m_doneG, m_doneD; + int m_doneGs, m_doneDs; + // Instance unique. + static Motor *m_instance; public: // Constructeur. Motor (void); // Destructeur. ~Motor (void); + // Change la position courante. + void setPos (double x, double y); + // Change la position courante. + void setPos (double x, double y, double dir); + // Récupère la position courante. TODO: valider avec une date. + void getPos (double &x, double &y, double &dir); + // Retourne une référence vers l'instance unique. + static Motor &getInstance (void) { return *m_instance; } // 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); + // Avance indéfiniment à la vitesse s. + void go (int s = 0); + // Avance indéfiniment à la vitesse sg et sd, respectivement pour le + // moteur gauche et droit. + void go (int sg, int sd); + // Tourne indéfiniment à la vitesse s. + void turn (int s = 0); + // Avance indéfiniment en tournant à la vitesse max s, de rayon r. + void goTurn (int s, int r); + // Stoppe. + void stop (void); + // Stop progressif. + void stopp (void); + // Reset. + void reset (void); // Retourne true si la commande est terminée. - bool finnish (void) { readMsg (); return !running; } + bool finnish (void) { readMsg (); return !m_running; } // Traite les message entrants. void readMsg (void); + // Traite l'information de bouclage. + void readTicks (int g, int d); + // Demande les infos capteurs. + void fetchCapteurs (void); + // Retourne les infos capteurs. + int getCapteurs (void); + // Gère les servos. + void setServo (int s); + // Attend le top départ. + void waitRound (void); + // Récupère la distance parcouru. + int getDone (void); + // Mémorise la distance parcourue pour le moment. + void setDone (void); private: // Envoie une commande. void send (char com, int a1 = -1, int a2 = -1, int a3 = -1); diff --git a/2003/i/buzz/src/motor/test_motor.cc b/2003/i/buzz/src/motor/test_motor.cc index 636d743..b9e5066 100644 --- a/2003/i/buzz/src/motor/test_motor.cc +++ b/2003/i/buzz/src/motor/test_motor.cc @@ -4,24 +4,65 @@ // #include "motor.h" #include "erreur/erreur.h" +#include "date/date.h" #include #include +#include int -main (void) +main (int argc, char **argv) { + cout << "hello" << endl; 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); + cout << "world" << endl; + int i = 1; + int c; + while (i < argc) + { + switch (argv[i][0]) + { + case 'j': + cout << "jack" << endl; + i++; + m.waitRound (); + break; + case 'h': + i++; + cout << "capteurs "; + c = m.getCapteurs (); + for (int j = 1; j < 0x100; j <<= 1) + { + cout << (c & j ? '#' : '_') << ' '; + } + cout << endl; + break; + case 't': + cout << "turn" << endl; + i++; + if (i >= argc) return 1; + m.turnAngle (atoi (argv[i++])); + break; + case 'z': + cout << "rezet" << endl; + i++; + m.reset (); + break; + case 'H': + i++; + if (i >= argc) return 1; + cout << "servo " << atoi (argv[i]); + m.setServo (atoi (argv[i++])); + break; + default: + cout << "go" << endl; + m.goDistance (atoi (argv[i++])); + break; + } + while (!m.finnish ()) Date::wait (50); + } return 0; } catch (Erreur &e) -- cgit v1.2.3