// busp_servo.cc // buzz - Programme du robot Efrei Robotique I1-I2 2003 // Copyright (C) 2003 Nicolas Schodet // #include "busp_servo.h" #include "busp.h" #include "config/config.h" // Constructeur. BuspServo::BuspServo () { // Position de départ. for (int i = 0; i < m_nbServo; ++i) m_pos[i] = 127; // Lectures des positions préprogrammées. Config rc ("rc/busp/servo"); for (int i = 0; i < m_nbPositions; ++i) for (int s = 0; s < m_nbServo; ++s) { rc >> m_ppos[i][s]; } rc.getEof (); } // Bouge le servo N en POS. void BuspServo::moveTo (int n, int pos) { set (n, pos); } // Bouge le servo N de DIFF. void BuspServo::move (int n, int diff) { set (n, m_pos[n & 0x07] + diff); } // Déplace les servos pour atteindre une position. void BuspServo::position (int p) { for (int s = 0; s < m_nbServo; ++s) if (m_ppos[p][s] != -1) set (s, m_ppos[p][s]); } // Fait le déplacement effectif. void BuspServo::set (int n, int pos) { if (pos > 255) pos = 255; else if (pos < 0) pos = 0; n &= 0x07; m_pos[n] = pos; Busp::getInstance ().cli (); Busp::getInstance ().write (m_address, n); Busp::getInstance ().write (m_address, pos); Busp::getInstance ().sti (); }