summaryrefslogtreecommitdiff
path: root/i/marvin/src/asserv/asserv.cc
diff options
context:
space:
mode:
Diffstat (limited to 'i/marvin/src/asserv/asserv.cc')
-rw-r--r--i/marvin/src/asserv/asserv.cc612
1 files changed, 243 insertions, 369 deletions
diff --git a/i/marvin/src/asserv/asserv.cc b/i/marvin/src/asserv/asserv.cc
index 2c36887..e4e964e 100644
--- a/i/marvin/src/asserv/asserv.cc
+++ b/i/marvin/src/asserv/asserv.cc
@@ -1,9 +1,9 @@
// asserv.cc
-// robert - programme du robot 2005 {{{
+// marvin - programme du robot 2006. {{{
//
-// Copyright (C) 2005 Nicolas Haller
+// Copyright (C) 2006 Nicolas Schodet
//
-// Robot APB Team/Efrei 2005.
+// Robot APB Team/Efrei 2006.
// Web: http://assos.efrei.fr/robot/
// Email: robot AT efrei DOT fr
//
@@ -22,478 +22,352 @@
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//
// }}}
-
#include "asserv.hh"
#include "config/config.hh"
-#include<cmath>
-
-/// Constructeur
-Asserv::Asserv (Receiver & receiver)
- :proto_(*this), receiver_(receiver), log_("asserv")
-{
- // Rechargement des paramètres
- loadConfig ();
- //Ouverture du port série
- proto_.open(ttyName_);
-}
-
-/// Reset de l'AVR
-void Asserv::reset(void)
-{
- // Reset AVR
- proto_.send('z');
- // Envoie des données de conf sur l'AVR
- setFooting(footing_);
- setEpsilon(epsilon_);
- setKp(kp_);
- setKi(ki_);
- setKd(kd_);
- setESat(eSat_);
- setSpeedIntMax(speedIntMax_);
- setDSample(dSample_);
- setAccel(accel_);
- setMaxSpeed(maxSLin_, maxSRot_);
- setUseTazFSM(useTazFSM_);
- setInvPwm(leftInvPwm_, rightInvPwm_);
- statCounter(pCounter_);
- statPosition(pPosition_);
- statMotor(pMotor_);
- statPwm(pPwm_);
- statTiming(pTiming_);
- statInPort(pStatInPort_);
- statSharp(pStatInPort_);
+/// Constructor.
+Asserv::Asserv (Asserv::Receiver &receiver)
+ : proto_ (*this), receiver_ (receiver)
+{
+ // Get Config
+ Config &config = Config::getInstance ();
+ intervalCounterStat_ = config.get<int> ("asserv.counter_stat", 0);
+ intervalPos_ = config.get<int> ("asserv.pos_report");
+ intervalSpeedStat_ = config.get<int> ("asserv.speed_stat", 0);
+ intervalPosStat_ = config.get<int> ("asserv.pos_stat", 0);
+ intervalPwmStat_ = config.get<int> ("asserv.pwm_stat", 0);
+ intervalTimerStat_ = config.get<int> ("asserv.timer_stat", 0);
+ intervalInPort_ = config.get<int> ("asserv.in_port_report");
+ footing_ = config.get<int> ("asserv.footing");
+ tAccel_ = config.get<int> ("asserv.t_accel");
+ aAccel_ = config.get<int> ("asserv.a_accel");
+ tMaxSpeed_ = config.get<int> ("asserv.t_max_speed");
+ aMaxSpeed_ = config.get<int> ("asserv.a_max_speed");
+ tkp_ = config.get<int> ("asserv.tkp");
+ tki_ = config.get<int> ("asserv.tki");
+ tkd_ = config.get<int> ("asserv.tkd");
+ akp_ = config.get<int> ("asserv.akp");
+ aki_ = config.get<int> ("asserv.aki");
+ akd_ = config.get<int> ("asserv.akd");
+ esat_ = config.get<int> ("asserv.esat");
+ isat_ = config.get<int> ("asserv.isat");
+ lInvertPwm_ = config.get<bool> ("asserv.l_invert_pwm");
+ rInvertPwm_ = config.get<bool> ("asserv.r_invert_pwm");
+ stepPerMm_ = config.get<double> ("asserv.step_per_mm");
+ tty_ = config.get<std::string> ("asserv.tty");
+ proto_.open (tty_);
}
/// Essaie de purger la liste d'émission et indique si elle est vide.
-bool Asserv::sync(void)
+bool
+Asserv::sync (void)
{
- return proto_.sync();
+ return proto_.sync ();
}
-/// Attend que toute les émissions soit terminées
-bool Asserv::wait(int timeout)
+/// Attend que toute les émissions soit terminées.
+bool
+Asserv::wait (int timeout/*-1*/)
{
- return proto_.wait(timeout);
+ return proto_.wait (timeout);
}
/// Récupère le File Descriptor
-int Asserv::getFd(void)
-{
- return proto_.getFd();
-}
-
-/// Commandes d'asservissement
-void Asserv::linearMove(double distance)
-{
- // Conversion mm->PasW
- int distPas = mm2pas(distance);
- //log_ ("linearMove") << "avConv" << distance << "apConv" << distPas;
- // On envoie la commande à l'AVR
- proto_.send('l', "w", distPas);
-}
-
-void Asserv::angularMove(double angle)
-{
- // Conversion radian->256ème
- int a = radTo256(angle);
- //log_ ("angularMove") << "avConv" << angle << "apConv" << a;
- // Envopie vers avr
- proto_.send('a',"b", a);
-}
-
-void Asserv::goToPosition(double xPos, double yPos)
-{
- // Convertion mm->PasD
- int x = mm2pas(xPos, true);
- int y = mm2pas(yPos, true);
- //log_ ("goToPosition") << "avConvX" << xPos << "apConvX" << x;
- //log_ ("goToPosition") << "avConvY" << yPos << "apConvY" << y;
- // Envoie vers l'AVR
- proto_.send('g', "dd", x, y);
-}
-
-// XXX Vérifier la conversion ca me parais un peu petit
-void Asserv::fuckTheWall(double speed)
-{
- // Conversion mm/s->Pas/Period
- int v = mms2ppp(speed);
- //log_ ("fuckTheWall") << "avConv" << speed << "apConv" << v;
- // Envoie vers l'AVR
- fuckTheWall(v);
-}
-
-void Asserv::fuckTheWall(int speed)
-{
- //log_ ("fuckTheWall") << "avConv";
- // Envoie vers l'AVR
- proto_.send('f',"b", speed);
-}
-
-void Asserv::setSpeed(double xSpeed, double ySpeed)
+int
+Asserv::getFd (void)
{
- // Conversion mm/s->Pas/Period
- int vx = mms2ppp(xSpeed);
- int vy = mms2ppp(ySpeed);
- //log_ ("setSpeed") << "avConvX" << xSpeed << "apConvX" << vx;
- //log_ ("setSpeed") << "avConvY" << ySpeed << "apConvY" << vy;
- // Envoie vers l'AVR
- setSpeed(vx,vy);
+ return proto_.getFd ();
}
-void Asserv::setSpeed(int xSpeed, int ySpeed)
+/// Reset the board and send parameters.
+void
+Asserv::reset (void)
{
- //log_ ("setSpeed") << "avConvX" << xSpeed ;
- //log_ ("setSpeed") << "avConvY" << ySpeed ;
- // Envoie vers l'AVR
- proto_.send('s',"bb", xSpeed, ySpeed);
+ proto_.send ('z');
+ setIntervalCounterStat (intervalCounterStat_);
+ setIntervalPos (intervalPos_);
+ setIntervalSpeedStat (intervalSpeedStat_);
+ setIntervalPosStat (intervalPosStat_);
+ setIntervalPwmStat (intervalPwmStat_);
+ setIntervalTimerStat (intervalTimerStat_);
+ setIntervalInPort (intervalInPort_);
+ setFooting (footing_);
+ setAccel (tAccel_, aAccel_);
+ setMaxSpeed (tMaxSpeed_, aMaxSpeed_);
+ setCoef (tkp_, tki_, tkd_, akp_, akd_, akd_, esat_, isat_);
+ setPwmDir (lInvertPwm_, rInvertPwm_);
}
-void Asserv::setPwm(int xPwm, int yPwm)
+/// Set PWM.
+void
+Asserv::setPwm (int l, int r)
{
- //log_ ("setPwm") << "avConvX" << xPwm;
- //log_ ("setPwm") << "avConvY" << yPwm;
- // Envoie sur l'AVR
- proto_.send('w',"ww" ,xPwm, yPwm);
+ proto_.send ('w', "ww", l, r);
}
-/// Informe l'AVR d'arréter de dire "j'ai fini"
-void Asserv::finishAck(void)
+/// Position consign offset.
+void
+Asserv::positionOffset (double t, double a)
{
- proto_.send('F', "b", 2);
+ proto_.send ('c', "ww", mmToStep (t), mmToStep (a));
}
-/// XXX Valeurs bizarres
-/// Statistiques
-void Asserv::statCounter(int period)
+/// Set speed.
+void
+Asserv::setSpeed (int t, int a)
{
-
- pCounter_ = period;
- // Convertion s->Period
- //int p = s2period(period);
- //log_ ("statCounter") << "avConv" << period << "apConv" << p;
- // Envoie sur l'AVR
- proto_.send('C', "b", period);
+ proto_.send ('s', "bb", t, a);
}
-void Asserv::statPosition(int period)
+/// Speed controlled position consign offset.
+void
+Asserv::setSpeedControlled (double t, double a, int seq)
{
- pPosition_ = period;
- // Convertion s->Period
- //int p = s2period(period);
- // Envoie sur l'AVR
- proto_.send('X', "b", period);
+ proto_.send ('s', "ddb", mmToStep (t), mmToStep (a), seq);
}
-void Asserv::statMotor(int period)
+/// Find a hole.
+void
+Asserv::findHole (int seq)
{
- pMotor_ = period;
- // Convertion s->Period
- //int p = s2period(period);
- // Envoie sur l'AVR
- proto_.send('S', "b", period);
+ proto_.send ('h', "b", seq);
}
-void Asserv::statPwm(int period)
+/// Acknoledge.
+void
+Asserv::ack (int seq)
{
- pPwm_ = period;
- // Convertion s->Period
- // int p = s2period(period);
- // Envoie sur l'AVR
- proto_.send('W', "b", period);
+ proto_.send ('A', "b", seq);
}
-void Asserv::statTiming(int period)
+/// Change counter stat interval.
+void
+Asserv::setIntervalCounterStat (int i)
{
- pTiming_ = period;
- // Convertion s->Period
- //int p = s2period(period);
- // Envoie sur l'AVR
- proto_.send('T', "b", period);
+ intervalCounterStat_ = i;
+ proto_.send ('C', "b", i);
}
-void Asserv::statInPort(int period)
+/// Change position report interval.
+void
+Asserv::setIntervalPos (int i)
{
- pStatInPort_ = period;
- // Convertion s->Period
- //int p = s2period(period);
- // Envoie sur l'AVR
- proto_.send('P', "b", period);
+ intervalPos_ = i;
+ proto_.send ('X', "b", i);
}
-void Asserv::statSharp(int period)
+/// Change speed stat interval.
+void
+Asserv::setIntervalSpeedStat (int i)
{
- pSharp_ = period;
- // Conversion s->period
- //int p = s2period(period);
- // Envoie sur l'AVR
- proto_.send('H', "b", period);
+ intervalSpeedStat_ = i;
+ proto_.send ('S', "b", i);
}
-/// Change les paramètres
-void Asserv::setXPos(double xPos)
+/// Change position control stat interval.
+void
+Asserv::setIntervalPosStat (int i)
{
- // Conversion mm->pas
- int p = mm2pas(xPos, true);
- // Envoie à l'AVR
- proto_.send('p',"bd", 'x', p);
+ intervalPosStat_ = i;
+ proto_.send ('P', "b", i);
}
-void Asserv::setYPos(double yPos)
+/// Change pwm stat interval.
+void
+Asserv::setIntervalPwmStat (int i)
{
- // Conversion mm->pas
- int p = mm2pas(yPos, true);
- // Envoie à l'AVR
- proto_.send('p',"bd", 'y', p);
+ intervalPwmStat_ = i;
+ proto_.send ('W', "b", i);
}
-void Asserv::setAngle(double angle)
+/// Change timer stat interval.
+void
+Asserv::setIntervalTimerStat (int i)
{
- // Envoie à l'AVR
- proto_.send('p',"bd", 'a', static_cast<int>(angle / (2 * M_PI) * (1 << 24)));
+ intervalTimerStat_ = i;
+ proto_.send ('T', "b", i);
}
-void Asserv::setFooting(int16_t footing)
+/// Change input port report interval.
+void
+Asserv::setIntervalInPort (int i)
{
- footing_ = footing;
- // Envoie à l'AVR
- proto_.send('p',"bw", 'f', footing);
+ intervalInPort_ = i;
+ proto_.send ('I', "b", i);
}
-void Asserv::setEpsilon(double epsilon)
+/// Set current position.
+void
+Asserv::setPos (double x, double y, double a)
{
- epsilon_ = epsilon;
- // Envoie à l'AVR
- proto_.send('p',"bd", 'e', static_cast<int>(epsilon * 256));
+ setXPos (x);
+ setYPos (y);
+ setAPos (a);
}
-void Asserv::setKp(int Kp)
+/// Set current x position.
+void
+Asserv::setXPos (double x)
{
- kp_ = Kp;
- // Envoie à l'AVR
- proto_.send('p',"bw", 'p', Kp);
+ proto_.send ('p', "bd", 'X', mmToStep (x));
}
-void Asserv::setKi(int Ki)
+/// Set current y position.
+void
+Asserv::setYPos (double y)
{
- ki_ = Ki;
- // Envoie à l'AVR
- proto_.send('p',"bw", 'i', Ki);
+ proto_.send ('p', "bd", 'Y', mmToStep (y));
}
-void Asserv::setKd(int Kd)
+/// Set current angle.
+void
+Asserv::setAPos (double a)
{
- kd_ = Kd;
- // Envoie à l'AVR
- proto_.send('p',"bw", 'd', Kd);
+ proto_.send ('p', "bd", 'A', radToAvr (a));
}
-void Asserv::setESat(int eSat)
+/// Set footing.
+void
+Asserv::setFooting (int f)
{
- eSat_ = eSat;
- // Envoie à l'AVR
- proto_.send('p', "bw", 'E', eSat);
+ footing_ = f;
+ proto_.send ('p', "bw", 'f', f);
}
-void Asserv::setSpeedIntMax(int maxInt)
+/// Set acceleration.
+void
+Asserv::setAccel (int t, int a)
{
- speedIntMax_ = maxInt;
- proto_.send('p', "bW", 'I', maxInt);
+ tAccel_ = t;
+ aAccel_ = a;
+ proto_.send ('p', "bww", 'a', t, a);
}
-void Asserv::setDSample (int dSample)
+/// Set maximum speed for automatic movements.
+void
+Asserv::setMaxSpeed (int t, int a)
{
- dSample_ = dSample;
- proto_.send('p', "bb", 's', dSample);
+ tMaxSpeed_ = t;
+ aMaxSpeed_ = a;
+ proto_.send ('p', "bbb", 's', t, a);
}
-void Asserv::setAccel(int accel)
+/// Set motor control coeficients.
+void
+Asserv::setCoef (int tkp, int tki, int tkd, int akp, int aki, int akd,
+ int esat, int isat)
{
- accel_ = accel;
- //log_ ("setAccel") << "avConv" << accel;
- // envoie vers l'AVR
- proto_.send('p',"bb", 'a', accel_);
+ tkp_ = tkp;
+ tki_ = tki;
+ tkd_ = tkd;
+ akp_ = akp;
+ aki_ = aki;
+ akd_ = akd;
+ esat_ = esat;
+ isat_ = isat;
+ proto_.send ('p', "bww", 'p', tkp, akp);
+ proto_.send ('p', "bww", 'i', tki, aki);
+ proto_.send ('p', "bww", 'd', tkd, akd);
+ proto_.send ('p', "bw", 'E', esat);
+ proto_.send ('p', "bw", 'I', isat);
}
-/// Renvoie l'accélération en unité international (mm/s²)
-double Asserv::getAccel(void)
+/// Set PWM direction.
+void
+Asserv::setPwmDir (bool invertL, bool invertR)
{
- return mmPpas_ / (sPperiod_ * sPperiod_) / accel_; // XXX Vérifier ca
+ lInvertPwm_ = invertL;
+ rInvertPwm_ = invertR;
+ proto_.send ('p', "bbb", 'w', invertL ? 1 : 0, invertR ? 1 : 0);
}
-/// Règle les vitesses maximum(lineaire et rotation)
-void Asserv::setMaxSpeed(int maxSLin, int maxSRot)
+/// Store to eeprom.
+void
+Asserv::storeParams (void)
{
- maxSLin_ = (maxSLin > 0 ? maxSLin : maxSLin_);
- maxSRot_ = (maxSRot > 0 ? maxSRot : maxSRot_);
- // Envoie à l'AVR
- proto_.send('p',"bbb", 'm', maxSLin_, maxSRot_);
+ proto_.send ('p', "bb", 'E', 1);
}
-int Asserv::getMaxLSpeed(void)
+/// Clear eeprom.
+void
+Asserv::clearParams (void)
{
- return maxSLin_;
+ proto_.send ('p', "bb", 'E', 0);
}
-void Asserv::setUseTazFSM(bool use)
+/// Implémentation du proto::Receiver.
+void
+Asserv::receive (char command, const Proto::Frame &frame)
{
- useTazFSM_ = use;
- // Envoie la commande à l'AVR
- proto_.send('t', "b", use ? 1 : 0);
-}
-
-void Asserv::setInvPwm(bool leftInvPwm, bool rightInvPwm)
-{
- leftInvPwm_ = leftInvPwm;
- rightInvPwm_ = rightInvPwm;
- // Envoie la commande à l'AVR
- proto_.send('p', "bbb", 'w', leftInvPwm , rightInvPwm);
-}
-
-/// implémentation du proto::Receiver
-void Asserv::receive(char command, const Proto::Frame &frame)
-{
- switch(command)
- {
- case 'C': //Compteurs des moteurs (uint16)
- int lMotor;
- int rMotor;
- proto_.decode(frame, "ww", lMotor, rMotor);
- receiver_.receiveCounter(lMotor, rMotor);
- break;
- case 'X':
- int xPos;
- proto_.decode(frame, "D", xPos);
- // Convertion Pas->mm
- receiver_.receivePosX(xPos * mmPpas_ / 256);
- break;
- case 'Y':
- int yPos;
- proto_.decode(frame, "D", yPos);
- // Convertion Pas->mm
- receiver_.receivePosY(yPos * mmPpas_ / 256);
- break;
- case 'A':
- {
- int aPos;
- proto_.decode(frame, "D", aPos);
- double aPosD = (aPos * (2 * M_PI) / (1 << 24));
- receiver_.receivePosA(aPosD);
- break;
- }
- case 'S':
- int oldLSpeed, LSpeed, oldRSpeed, RSpeed;
- proto_.decode(frame, "WWWW", oldLSpeed, LSpeed, oldRSpeed, RSpeed);
- receiver_.receiveSpeedStat(oldLSpeed, LSpeed, oldRSpeed, RSpeed);
- break;
- case 'W':
- int leftPwm, rightPwm;
- proto_.decode(frame, "WW", leftPwm, rightPwm);
- receiver_.receivePwm(leftPwm, rightPwm);
- break;
- case 'T':
- {
- int motorTimer4 = static_cast<int>(frame.args[1]);
- int motorTimer3 = static_cast<int>(frame.args[2]);
- int motorTimer2 = static_cast<int>(frame.args[3]);
- int motorTimer1 = static_cast<int>(frame.args[4]);
- int motorTimer0 = static_cast<int>(frame.args[5]);
- receiver_.receiveTiming(motorTimer4, motorTimer3, motorTimer2,
- motorTimer1, motorTimer0);
- break;
- }
- case 'P':
- {
- int statePIn;
- proto_.decode(frame, "b", statePIn);
- receiver_.receiveInPort(statePIn);
- break;
- }
- case 'H':
- {
- int stateSharp0, stateSharp1, stateSharp2;
- proto_.decode(frame, "www", stateSharp0, stateSharp1, stateSharp2);
- receiver_.receiveSharp(stateSharp0, stateSharp1, stateSharp2);
- break;
- }
- case 't':
- {
- int state, subState;
- proto_.decode(frame, "bb", state, subState);
- receiver_.receiveTazState(state, subState);
- }
- case 'F':
- receiver_.done();
- break;
- }
-}
-
-/// Charge les données de la classe config
-void Asserv::loadConfig()
-{
- // Get the config instance
- Config &config = Config::getInstance ();
- ttyName_ = config.get<std::string>("asserv.tty");
- footing_ = config.get<int>("asserv.footing");
- epsilon_ = config.get<double>("asserv.epsilon");
- accel_ = config.get<int>("asserv.accel");
- kp_ = config.get<int>("asserv.kp");
- ki_ = config.get<int>("asserv.ki");
- kd_ = config.get<int>("asserv.kd");
- eSat_ = config.get<int>("asserv.eSat");
- speedIntMax_ = config.get<int>("asserv.speedIntMax");
- dSample_ = config.get<int>("asserv.dSample");
- maxSLin_ = config.get<int>("asserv.maxSLin");
- maxSRot_ = config.get<int>("asserv.maxSRot");
- useTazFSM_ = config.get<bool>("asserv.useTazFSM");
- mmPpas_ = config.get<double>("asserv.mmPpas");
- sPperiod_ = config.get<double>("asserv.sPperiod");
- pwmMax_ = config.get<int>("asserv.pwmMax");
- leftInvPwm_ = config.get<bool>("asserv.leftInvPwm");
- rightInvPwm_ = config.get<bool>("asserv.rightInvPwm");
- pCounter_ = 0;
- pPosition_ = 0;
- pMotor_ = 0;
- pPwm_ = 0;
- pTiming_ = 0;
- pStatInPort_ = 32;
- pSharp_ = 0;
+ int seq, l, r, x, y, a, t, te, ti, ae, ai, port;
+ switch (command)
+ {
+ case 'A':
+ if (proto_.decode (frame, "b", seq))
+ receiver_.receiveAck (seq);
+ break;
+ case 'C':
+ if (proto_.decode (frame, "ww", l, r))
+ receiver_.receiveCounterStat (l, r);
+ break;
+ case 'X':
+ if (proto_.decode (frame, "DDD", x, y, a))
+ receiver_.receivePos (stepToMm (x), stepToMm (y), stepToMm (a));
+ break;
+ case 'S':
+ if (proto_.decode (frame, "BB", t, a))
+ receiver_.receiveSpeedStat (t, a);
+ break;
+ case 'P':
+ if (proto_.decode (frame, "WWWW", te, ti, ae, ai))
+ receiver_.receivePosStat (te, ti, ae, ai);
+ break;
+ case 'W':
+ if (proto_.decode (frame, "WW", l, r))
+ receiver_.receivePwmStat (l, r);
+ break;
+ case 'T':
+ // TODO (si on a le temps).
+ break;
+ case 'I':
+ if (proto_.decode (frame, "w", port))
+ receiver_.receiveInPort (port);
+ break;
+ }
}
-int Asserv::mm2pas(double dist, bool format24eme)
+/// Convert mm to steps.
+int
+Asserv::mmToStep (double mm) const
{
- if(format24eme)
- return static_cast<int>((dist/mmPpas_)*256);
- else
- return static_cast<int>(dist/mmPpas_);
+ return static_cast<int> (mm * stepPerMm_);
}
-int Asserv::radTo256(double angle, bool format24eme)
+/// Convert steps to mm.
+double
+Asserv::stepToMm (int step) const
{
- if(angle < 0)
- angle += 2 * M_PI;
- if(format24eme)
- return static_cast<int>((angle / (2 * M_PI)) * 256 * (1 << 24));
- else
- return static_cast<int>((angle / (2 * M_PI))* 256);
+ return static_cast<double> (step) / stepPerMm_;
}
-int Asserv::mms2ppp(double vitesse)
+/// Convert rad to avr angles.
+int
+Asserv::radToAvr (double a) const
{
- return static_cast<int>(vitesse * sPperiod_ / mmPpas_);
+ return static_cast<int> (a * M_1_PI * 0.5 * (1 << 24)) & 0xffffff;
}
-int Asserv::rCycl2Pwm(double rCycl)
+/// Convert avr angles to rad.
+double
+Asserv::avrToRad (int a) const
{
- return static_cast<int>(rCycl*pwmMax_);
+ return static_cast<double> (a) * 2 * M_PI / (1 << 24);
}
-int Asserv::s2period(double period)
+/// Convert rad to steps.
+int
+Asserv::radToStep (double a) const
{
- return static_cast<int>(period/sPperiod_);
+ return static_cast<int> (static_cast<double> (footing_) * a * 0.5);
}
-//int Asserv::mmps2ppperiod(double accel)
-//{
-// return static_cast<int>(1/(accel*(sPperiod_*sPperiod_/mmPpas_)));
-//}