summaryrefslogtreecommitdiff
path: root/2003/i/buzz/src/busp/busp_servo.cc
blob: ae1536fd64dfd176bcc2bc7fa689f633c216dd27 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
// 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 "erreur.h"

#include <fstream>

// 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.
    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.
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 ();
}