summaryrefslogtreecommitdiff
path: root/i/marvin/src/motor/motor.cc
diff options
context:
space:
mode:
Diffstat (limited to 'i/marvin/src/motor/motor.cc')
-rw-r--r--i/marvin/src/motor/motor.cc25
1 files changed, 18 insertions, 7 deletions
diff --git a/i/marvin/src/motor/motor.cc b/i/marvin/src/motor/motor.cc
index 38b917e..d2a7250 100644
--- a/i/marvin/src/motor/motor.cc
+++ b/i/marvin/src/motor/motor.cc
@@ -26,7 +26,8 @@
/// Constructor.
Motor::Motor (void)
- : asserv_ (*this), seq_ (0), finish_ (true), x_ (0.0), y_ (0.0), a_ (0.0)
+ : asserv_ (*this), seq_ (0), finish_ (true), blocked_ (false),
+ x_ (0.0), y_ (0.0), a_ (0.0)
{
}
@@ -34,7 +35,8 @@ Motor::Motor (void)
void
Motor::reset (void)
{
- finish_ = false;
+ finish_ = true;
+ blocked_ = false;
asserv_.reset ();
}
@@ -45,6 +47,7 @@ Motor::move (double t, double a)
{
nextSeq ();
finish_ = false;
+ blocked_ = false;
asserv_.speedTo (t, a, seq_);
}
@@ -54,6 +57,7 @@ Motor::rotate (double a)
{
nextSeq ();
finish_ = false;
+ blocked_ = false;
asserv_.speedAngle (a, seq_);
}
@@ -63,6 +67,7 @@ Motor::findHole (void)
{
nextSeq ();
finish_ = false;
+ blocked_ = false;
asserv_.findHole (seq_);
}
@@ -91,6 +96,7 @@ void
Motor::stop (void)
{
finish_ = true;
+ blocked_ = false;
asserv_.speed (0, 0);
}
@@ -106,7 +112,7 @@ void
Motor::nextSeq (void)
{
seq_++;
- if (seq_ > 250)
+ if (seq_ > 100)
seq_ = 1;
}
@@ -114,10 +120,15 @@ Motor::nextSeq (void)
void
Motor::receiveAck (int seq)
{
- if (seq != seq_)
- return;
- asserv_.ack (seq);
- finish_ = true;
+ if ((seq & 0x7f) == seq_)
+ {
+ asserv_.ack (seq);
+ finish_ = true;
+ if (seq & 0x80)
+ {
+ blocked_ = true;
+ }
+ }
}
void