summaryrefslogtreecommitdiff
path: root/i/marvin/src/motor/motor.cc
blob: af3d29f176617be251da77be8f3db952e41732ef (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
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
// motor.cc
// marvin - programme du robot 2006. {{{
//
// Copyright (C) 2006 Nicolas Schodet
//
// Robot APB Team/Efrei 2005.
//        Web: http://assos.efrei.fr/robot/
//      Email: robot AT efrei DOT fr
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
// 
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//
// }}}
#include "motor.hh"

/// Constructor.
Motor::Motor (void)
    : asserv_ (*this), seq_ (0), finish_ (true), x_ (0.0), y_ (0.0), a_ (0.0)
{
}

/// Linear and angular move. T(mm) is the linear distance.  A(mm) is the
/// angular distance which means the arc length.
void
Motor::move (double t, double a)
{
    nextSeq ();
    finish_ = false;
    asserv_.speedTo (t, a, seq_);
}

/// Rotate (rad).
void
Motor::rotate (double a)
{
    nextSeq ();
    finish_ = false;
    asserv_.speedAngle (a, seq_);
}

/// Find a hole.
void
Motor::findHole (void)
{
    nextSeq ();
    finish_ = false;
    asserv_.findHole (seq_);
}

/// Stop now.
void
Motor::stop (void)
{
    finish_ = true;
    asserv_.speed (0, 0);
}

/// Next seq number.
void
Motor::nextSeq (void)
{
    seq_++;
    if (seq_ > 250)
	    seq_ = 1;
}

/// Implement Asserv::Receiver callbacks.
void
Motor::receiveAck (int seq)
{
    if (seq != seq_)
	std::cout << "spurious ack received" << std::endl;
    asserv_.ack (seq);
    finish_ = true;
}

void
Motor::receiveCounterStat (int l, int r)
{
}

void
Motor::receivePos (double x, double y, double a)
{
    x_ = x;
    y_ = y;
    a_ = a;
}

void
Motor::receiveSpeedStat (int t, int a)
{
}

void
Motor::receivePosStat (int te, int ti, int ae, int ai)
{
}

void
Motor::receivePwmStat (int l, int r)
{
}

void
Motor::receiveTimerStat (const int *t, int tn)
{
}

void
Motor::receiveInPort (unsigned int port)
{
}