summaryrefslogtreecommitdiff
path: root/i/marvin/src
diff options
context:
space:
mode:
Diffstat (limited to 'i/marvin/src')
-rw-r--r--i/marvin/src/asserv/test_asserv.cc13
-rw-r--r--i/marvin/src/motor/motor.cc25
-rw-r--r--i/marvin/src/motor/motor.hh3
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);