From d201c6b9e46b79b6dabb5a7cd753b95c108c0915 Mon Sep 17 00:00:00 2001 From: schodet Date: Thu, 22 May 2003 18:02:01 +0000 Subject: Ajoute des cli/sti. Lecture du fichier de conf pour les servos. Capteurs. --- 2003/i/buzz/src/busp/busp_servo.cc | 28 +++++++++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) (limited to '2003/i/buzz/src/busp/busp_servo.cc') diff --git a/2003/i/buzz/src/busp/busp_servo.cc b/2003/i/buzz/src/busp/busp_servo.cc index 1cef0c4..ae1536f 100644 --- a/2003/i/buzz/src/busp/busp_servo.cc +++ b/2003/i/buzz/src/busp/busp_servo.cc @@ -4,12 +4,27 @@ // #include "busp_servo.h" #include "busp.h" +#include "erreur.h" + +#include // Constructeur. BuspServo::BuspServo () { - for (int i = 0; i < 8; ++i) + // Position de départ. + for (int i = 0; i < m_nbServo; ++i) m_pos[i] = 127; + // Lectures des positions préprogrammées. + ifstream rc ("rc/busp/servo"); + for (int i = 0; i < m_nbPositions; ++i) + for (int s = 0; s < m_nbServo; ++s) + { + m_ppos[i][s] = -1; + rc >> m_ppos[i][s]; + } + // Vérifie les erreurs. + if (rc.bad ()) + throw ErreurFatale ("Impossible d'ouvrir `rc/busp/servo'\n"); } // Bouge le servo N en POS. @@ -26,6 +41,15 @@ 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) @@ -34,6 +58,8 @@ BuspServo::set (int n, int pos) 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 (); } -- cgit v1.2.3