// motor.cc // buzz - Programme du robot Efrei Robotique I1-I2 2003 // Copyright (C) 2003 Nicolas Schodet // #include "motor.h" #include "config/config.h" #include #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; }