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/asserv/test_asserv.cc | 13 +++++++------ i/marvin/src/motor/motor.cc | 25 ++++++++++++++++++------- i/marvin/src/motor/motor.hh | 3 +++ 3 files changed, 28 insertions(+), 13 deletions(-) diff --git a/i/marvin/src/asserv/test_asserv.cc b/i/marvin/src/asserv/test_asserv.cc index 81a4ba9..2796e33 100644 --- a/i/marvin/src/asserv/test_asserv.cc +++ b/i/marvin/src/asserv/test_asserv.cc @@ -45,7 +45,7 @@ class TestAsserv : public Tester, public Asserv::Receiver inline void nextSeq (void) { seq_++; - if (seq_ > 250) + if (seq_ > 100) seq_ = 1; } /// Speed controlled position consign offset, handle seq. @@ -142,11 +142,12 @@ class TestAsserv : public Tester, public Asserv::Receiver } void receiveAck (int seq) { - if (seq != seq_) - return; - std::cout << "ack received" << std::endl; - asserv_.ack (seq); - ok_ = true; + if ((seq & 0x7f) == seq_) + { + std::cout << "ack received" << std::endl; + asserv_.ack (seq); + ok_ = true; + } } void receiveCounterStat (int l, int r) { 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 diff --git a/i/marvin/src/motor/motor.hh b/i/marvin/src/motor/motor.hh index 1c6f403..68da820 100644 --- a/i/marvin/src/motor/motor.hh +++ b/i/marvin/src/motor/motor.hh @@ -33,6 +33,7 @@ class Motor : public Asserv::Receiver Asserv asserv_; int seq_; bool finish_; + bool blocked_; double x_, y_, a_; public: /// Constructor. @@ -45,6 +46,8 @@ class Motor : public Asserv::Receiver Asserv &getAsserv (void) { return asserv_; } /// True if last long move is finished. bool finish (void) const { return finish_; } + /// True if blocked. + bool blocked (void) const { return blocked_; } /// Linear and angular move. T(mm) is the linear distance. A(mm) is the /// angular distance which means the arc length. void move (double t, double a); -- cgit v1.2.3