From 6eae3fd7b71b3527b0c95fd5e0c9f9d131f327a7 Mon Sep 17 00:00:00 2001 From: schodet Date: Thu, 25 May 2006 18:09:03 +0000 Subject: Ajout de la gestion du bloquage --- i/marvin/src/motor/motor.cc | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) (limited to 'i/marvin/src/motor/motor.cc') 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 -- cgit v1.2.3