summaryrefslogtreecommitdiff
path: root/2003/i/buzz/src/busp/busp_servo.cc
diff options
context:
space:
mode:
authorschodet2003-05-22 18:02:01 +0000
committerschodet2003-05-22 18:02:01 +0000
commitd201c6b9e46b79b6dabb5a7cd753b95c108c0915 (patch)
treeb39117bb658ba4c072d97c92196f79c10ea11de0 /2003/i/buzz/src/busp/busp_servo.cc
parent524be5fa549d7c598740b53aeda1c4432d546b01 (diff)
Ajoute des cli/sti.
Lecture du fichier de conf pour les servos. Capteurs.
Diffstat (limited to '2003/i/buzz/src/busp/busp_servo.cc')
-rw-r--r--2003/i/buzz/src/busp/busp_servo.cc28
1 files changed, 27 insertions, 1 deletions
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 <fstream>
// 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 ();
}