// motor.cc // buzz - Programme du robot Efrei Robotique I1-I2 2003 // Copyright (C) 2003 Nicolas Schodet // #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; 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 ("go")) { rc.getId (); 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 ("godistance")) { rc.getId (); rc >> m_speed; rc >> m_accel; } else if (rc.isId ("turn")) { rc.getId (); rc >> m_inf_rspeed; rc >> m_inf_raccel; } else if (rc.isId ("entraxe")) { rc.getId (); 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, m_accel, s == -1 ? m_speed : s); else 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, m_raccel, s == -1 ? m_rspeed : s); else 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. 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': 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; } } m_recv_n = 0; } } // 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) { 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) { 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'; #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; }