summaryrefslogtreecommitdiff
path: root/2003/i/buzz/src/busp/busp_servo.cc
diff options
context:
space:
mode:
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 ();
}