summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--i/marvin/src/asserv/asserv.cc612
-rw-r--r--i/marvin/src/asserv/asserv.hh238
2 files changed, 352 insertions, 498 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_)));
-//}
diff --git a/i/marvin/src/asserv/asserv.hh b/i/marvin/src/asserv/asserv.hh
index b3d3183..86cbb23 100644
--- a/i/marvin/src/asserv/asserv.hh
+++ b/i/marvin/src/asserv/asserv.hh
@@ -1,11 +1,11 @@
#ifndef asserv_hh
#define asserv_hh
// asserv.hh
-// 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
//
@@ -25,135 +25,115 @@
//
// }}}
-#include "utils/non_copyable.hh"
#include "proto/proto.hh"
-#include "log/log.hh"
-/// Classe de dialogue avec la carte d'asservissement.
-class Asserv : public NonCopyable, public Proto::Receiver
+/// Dialog class with the motor control board.
+/// Units:
+/// - step: one step distance.
+/// - period: one motor control sampling period.
+class Asserv : public Proto::Receiver
{
- public:
- /// Les clients de Asserv doivent dériver de Receiver.
- class Receiver
- {
- public:
- virtual ~Receiver(void){};
- /// Recoit un packet.
- virtual void receiveCounter (double lMotor, double rMotor) = 0;
- virtual void receivePosX (double xPos) = 0;
- virtual void receivePosY (double yPos) = 0;
- virtual void receivePosA (double aPos) = 0;
- virtual void receiveSpeedStat (int leftError, int leftInt, int rightError, int rightInt) = 0;
- virtual void receivePwm (double leftPwm, double rightPwm) = 0;
- virtual void receiveTiming (int motorTimer4,
- int motorTimer3, int motorTimer2,
- int motorTimer1,
- int motorTimer0 ) = 0;
- virtual void receiveInPort (int port) = 0;
- virtual void receiveSharp (int sharp1, int sharp2, int sharp3) = 0;
- virtual void receiveTazState(int state, int subState){}; // XXX Vérifier les formats
- virtual void done (void) = 0;
- };
-
- private:
-
- // Communication avec l'AVR
- Proto proto_;
- std::string ttyName_;
- Receiver & receiver_;
- // Paramètre de l'avr
- int footing_; //truc
- double epsilon_; //mm
- int accel_; //unité AVR
- int kp_;
- int ki_;
- int kd_;
- int eSat_;
- int speedIntMax_;
- int dSample_;
- int maxSLin_;//unité AVR
- int maxSRot_;
- bool leftInvPwm_, rightInvPwm_;
- bool useTazFSM_;
- // Stat
- int pCounter_;
- int pPosition_;
- int pMotor_;
- int pPwm_;
- int pTiming_;
- int pStatInPort_;
- int pSharp_;
-
- // Unités
- double mmPpas_;
- double sPperiod_;
- int pwmMax_;
-
- // Système de log.
- Log log_;
- public:
- /// Constructeur.
- Asserv (Asserv::Receiver & receiver);
- /// Reset la carte et envois les paramètres.
- void reset (void);
- /// Essaie de purger la liste d'émission et indique si elle est vide.
- bool sync (void);
- /// Attend que toute les émissions soit terminées.
- bool wait (int timeout = -1);
- /// Récupère le File Descriptor
- int getFd(void);
- /// Commandes asservissement
- void linearMove(double distance);
- void angularMove(double angle);
- void goToPosition(double xPos, double yPos);
- void fuckTheWall(double speed);
- void fuckTheWall(int speed);
- void setSpeed(double xSpeed, double ySpeed);
- void setSpeed(int xSpeed = 0, int ySpeed = 0);
- void setPwm(int leftPwm = 0, int rightPwm = 0);
- void finishAck(void); //met le paramètre F à 0
- /// Statistiques
- void statCounter(int period = 0);
- void statPosition(int period = 0);
- void statMotor(int period = 0);
- void statPwm(int period = 0);
- void statTiming(int period = 0);
- void statInPort(int period = 0);
- void statSharp(int period = 0);
- //@{
- /// Change les paramètres de la carte.
- void setXPos(double pos);
- void setYPos(double pos);
- void setAngle(double angle);
- void setFooting(int16_t dist);
- void setEpsilon(double e);
- void setKp (int kp);
- void setKi (int ki);
- void setKd (int kd);
- void setESat (int eSat);
- void setSpeedIntMax (int maxInt);
- void setDSample(int dSample);
- void setAccel (int accel);
- double getAccel (void);
- void setMaxSpeed(int maxSLin, int maxSRot);
- int getMaxLSpeed(void);
- void setUseTazFSM(bool use);
- void setInvPwm(bool leftInvPwm, bool rightInvPwm);
- //@}
-
- /// implémentation du proto::Receiver
- void receive (char command, const Proto::Frame &frame);
-
- private:
- /// Charge les données de la classe config
- void loadConfig ();
- /// Fonctions de conversion
- int mm2pas(double dist, bool format24eme = false); //Format 24/8 ou 8
- int radTo256(double angle, bool format24eme = false); //Format 8 ou 8/24
- int mms2ppp(double vitesse); // Format 8
- int rCycl2Pwm(double rCycl); // Format 16
- int s2period(double period); // Format 8
- //int mmps2ppperiod(double accel); //Format 8 (1 vitesse par x period)
+ public:
+ /// Asserv clients must implement Receiver.
+ class Receiver
+ {
+ public:
+ virtual ~Receiver (void) { }
+ virtual void receiveAck (int seq) = 0;
+ virtual void receiveCounterStat (int l, int r) = 0;
+ virtual void receivePos (double x, double y, double a) = 0;
+ virtual void receiveSpeedStat (int t, int a) = 0;
+ virtual void receivePosStat (int te, int ti, int ae, int ai) = 0;
+ virtual void receivePwmStat (int l, int r) = 0;
+ virtual void receiveTimerStat (const int *t, int tn) = 0;
+ virtual void receiveInPort (unsigned int port) = 0;
+ };
+ private:
+ Proto proto_;
+ std::string tty_;
+ Receiver &receiver_;
+ int intervalCounterStat_, intervalPos_, intervalSpeedStat_,
+ intervalPosStat_, intervalPwmStat_, intervalTimerStat_,
+ intervalInPort_;
+ int footing_;
+ int tAccel_, aAccel_;
+ int tMaxSpeed_, aMaxSpeed_;
+ int tkp_, tki_, tkd_, akp_, aki_, akd_, esat_, isat_;
+ bool lInvertPwm_, rInvertPwm_;
+ double stepPerMm_;
+ public:
+ /// Constructor.
+ Asserv (Asserv::Receiver &receiver);
+ /// Essaie de purger la liste d'émission et indique si elle est vide.
+ bool sync (void);
+ /// Attend que toute les émissions soit terminées.
+ bool wait (int timeout = -1);
+ /// Récupère le File Descriptor
+ int getFd (void);
+ /// Reset the board and send parameters.
+ void reset (void);
+ /// Set PWM.
+ void setPwm (int l, int r);
+ /// Position consign offset.
+ void positionOffset (double t, double a);
+ /// Set speed.
+ void setSpeed (int t, int a);
+ /// Speed controlled position consign offset.
+ void setSpeedControlled (double t, double a, int seq);
+ /// Find a hole.
+ void findHole (int seq);
+ /// Acknoledge.
+ void ack (int seq);
+ /// Change counter stat interval.
+ void setIntervalCounterStat (int i);
+ /// Change position report interval.
+ void setIntervalPos (int i);
+ /// Change speed stat interval.
+ void setIntervalSpeedStat (int i);
+ /// Change position control stat interval.
+ void setIntervalPosStat (int i);
+ /// Change pwm stat interval.
+ void setIntervalPwmStat (int i);
+ /// Change timer stat interval.
+ void setIntervalTimerStat (int i);
+ /// Change input port report interval.
+ void setIntervalInPort (int i);
+ /// Set current position.
+ void setPos (double x, double y, double a);
+ /// Set current x position.
+ void setXPos (double x);
+ /// Set current y position.
+ void setYPos (double y);
+ /// Set current angle.
+ void setAPos (double a);
+ /// Set footing.
+ void setFooting (int f);
+ /// Set acceleration.
+ void setAccel (int t, int a);
+ /// Set maximum speed for automatic movements.
+ void setMaxSpeed (int t, int a);
+ /// Set motor control coeficients.
+ void setCoef (int tkp, int tki, int tkd, int akp, int aki, int akd,
+ int esat, int isat);
+ /// Set PWM direction.
+ void setPwmDir (bool invertL, bool invertR);
+ /// Store to eeprom.
+ void storeParams (void);
+ /// Clear eeprom.
+ void clearParams (void);
+ /// Implémentation du proto::Receiver.
+ void receive (char command, const Proto::Frame &frame);
+ private:
+ /// Convert mm to steps.
+ int mmToStep (double mm) const;
+ /// Convert steps to mm.
+ double stepToMm (int step) const;
+ /// Convert rad to avr angles.
+ int radToAvr (double a) const;
+ /// Convert avr angles to rad.
+ double avrToRad (int a) const;
+ /// Convert rad to steps.
+ int radToStep (double a) const;
};
#endif // asserv_hh