summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor/motor.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/motor/motor.cc')
-rw-r--r--2004/i/nono/src/motor/motor.cc68
1 files changed, 44 insertions, 24 deletions
diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc
index 45c9955..ddd00ce 100644
--- a/2004/i/nono/src/motor/motor.cc
+++ b/2004/i/nono/src/motor/motor.cc
@@ -33,7 +33,7 @@ Motor *Motor::instance_ = 0;
/// Constructeur.
Motor::Motor (void)
: asserv_ (*this), log_ ("motor"),
- s_ (1.0), vs_ (1.0), as_ (1.0),
+ s_ (1.0), vs_ (1.0), a_ (1.0),
brakeDistance_ (0.0),
movement_ (0), maxSpeed_ (0x40), minSpeed_ (0x04)
{
@@ -45,14 +45,18 @@ Motor::Motor (void)
{
if (!(
rc.get ("scale", s_) ||
- rc.get ("speedscale", vs_) ||
- rc.get ("accelscale", as_) ||
- rc.get ("brakedistance", brakeDistance_) ||
rc.get ("maxspeed", maxSpeed_) ||
rc.get ("minspeed", minSpeed_)
))
rc.noId ();
}
+ // Initialise l'échelle.
+ vs_ = s_ * asserv_.getSpeedFactor ();
+ a_ = (s_ > 0 ? s_ : -s_) * asserv_.getAccelFactor ();
+ brakeDistance_ = 0.5 * (maxSpeed_ * maxSpeed_ * vs_ * vs_) / a_;
+ log_ (Log::debug) << "scale " << vs_ << ' ' << a_ << ' ' << brakeDistance_
+ << std::endl;
+ // C'est partit !
asserv_.reset ();
}
@@ -63,13 +67,15 @@ Motor::~Motor (void)
delete movement_;
}
-/// Appelée lors d'une mise à jour des compteurs (unit).
+/// Appelée lors d'une mise à jour des compteurs.
+/// l, r : mise à jour moteur gauche et droit (unit).
+/// zero : vrai si le mouvement est à considérer comme nul.
void
-Motor::updateCounter (int l, int r)
+Motor::updateCounter (int l, int r, bool zero)
{
double dL = l * s_;
double dR = r * s_;
- tracker_.update (dL, dR);
+ tracker_.update (dL, dR, zero);
if (movement_)
if (!movement_->control ())
{
@@ -127,13 +133,25 @@ Motor::waitStopped (void)
void
Motor::speed (double l, double a, double dist)
{
- speed (computeSpeed (dist) * (l - a), computeSpeed (dist) * (l + a));
+ speedLimit (l - a, l + a, computeSpeed (dist));
}
/// Paramètre la vitesse des moteurs (-1..+1).
void
Motor::speed (double l, double r)
{
+ speedLimit (l, r, maxSpeed_);
+}
+
+/// Paramètre la vitesse des moteurs (-1..+1) avec une vitesse limite
+/// (unit).
+void
+Motor::speedLimit (double l, double r, double vLimit)
+{
+ // Il faut un minimum tout de même. La politique, c'est on avance.
+ if (vLimit < minSpeed_)
+ vLimit = minSpeed_;
+ // Convertie le (-1..+1) en (unit).
int vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_ * 0.5);
int vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * 0.5);
// Teste le dépassement de vitesse minimale.
@@ -146,41 +164,43 @@ Motor::speed (double l, double r)
vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * 0.5);
}
// Teste le dépassement de vitesse maximal.
- if (vL > maxSpeed_)
+ if (vL > vLimit)
{
- vL = maxSpeed_;
+ vL = static_cast<int> (vLimit);
vR = (int) (r / l * vL);
}
- else if (vL < -maxSpeed_)
+ else if (vL < -vLimit)
{
- vL = -maxSpeed_;
+ vL = static_cast<int> (-vLimit);
vR = (int) (r / l * vL);
}
- if (vR > maxSpeed_)
+ if (vR > vLimit)
{
- vR = maxSpeed_;
+ vR = static_cast<int> (vLimit);
vL = (int) (l / r * vR);
}
- else if (vR < -maxSpeed_)
+ else if (vR < -vLimit)
{
- vR = -maxSpeed_;
+ vR = static_cast<int> (-vLimit);
vL = (int) (l / r * vR);
}
// Envois la commande.
log_ (Log::debug) << "speed " << l << ' ' << r << ' ' << vL << ' '
- << vR << std::endl;;
+ << vR << ' ' << vLimit << std::endl;;
asserv_.speed (vL, vR);
}
-/// Calcule la vitesse en ligne droit afin de pouvoir freiner au bout
+/// Calcule la vitesse (unit) en ligne droit afin de pouvoir freiner au bout
/// d'une distance (mm).
double
Motor::computeSpeed (double dist) const
{
+ if (dist <= 0.0)
+ return 0.0;
if (dist > brakeDistance_)
- return 1.0;
+ return maxSpeed_;
else
- return dist / brakeDistance_;
+ return sqrt (2.0 * dist * a_) / (vs_ > 0 ? vs_ : -vs_);
}
/// Va vers un point, en formant un arc de cercle, renvois faux si atteind.
@@ -193,13 +213,13 @@ Motor::goToArc (double x, double y, double eps/*5.0*/)
{
log_ (Log::debug) << "goto arc " << l << ' ' << r;
if (l > 0.0)
- l = sqrt (2.0 * asserv_.getAccel () * as_ * l);
+ l = sqrt (2.0 * a_ * l);
else
- l = -sqrt (2.0 * asserv_.getAccel () * as_ * -l);
+ l = -sqrt (2.0 * a_ * -l);
if (r > 0.0)
- r = sqrt (2.0 * asserv_.getAccel () * as_ * r);
+ r = sqrt (2.0 * a_ * r);
else
- r = -sqrt (2.0 * asserv_.getAccel () * as_ * -r);
+ r = -sqrt (2.0 * a_ * -r);
log_ << ' ' << l << ' ' << r << std::endl;
speed (l, r);
return true;