From cd40c9287d4b1624657ca784a640aa2678b52357 Mon Sep 17 00:00:00 2001 From: schodet Date: Tue, 16 May 2006 19:25:27 +0000 Subject: Ajout de la classe Asserv. Il reste à faire le test. --- i/marvin/src/asserv/asserv.cc | 612 +++++++++++++++++------------------------- i/marvin/src/asserv/asserv.hh | 238 ++++++++-------- 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 - -/// 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 ("asserv.counter_stat", 0); + intervalPos_ = config.get ("asserv.pos_report"); + intervalSpeedStat_ = config.get ("asserv.speed_stat", 0); + intervalPosStat_ = config.get ("asserv.pos_stat", 0); + intervalPwmStat_ = config.get ("asserv.pwm_stat", 0); + intervalTimerStat_ = config.get ("asserv.timer_stat", 0); + intervalInPort_ = config.get ("asserv.in_port_report"); + footing_ = config.get ("asserv.footing"); + tAccel_ = config.get ("asserv.t_accel"); + aAccel_ = config.get ("asserv.a_accel"); + tMaxSpeed_ = config.get ("asserv.t_max_speed"); + aMaxSpeed_ = config.get ("asserv.a_max_speed"); + tkp_ = config.get ("asserv.tkp"); + tki_ = config.get ("asserv.tki"); + tkd_ = config.get ("asserv.tkd"); + akp_ = config.get ("asserv.akp"); + aki_ = config.get ("asserv.aki"); + akd_ = config.get ("asserv.akd"); + esat_ = config.get ("asserv.esat"); + isat_ = config.get ("asserv.isat"); + lInvertPwm_ = config.get ("asserv.l_invert_pwm"); + rInvertPwm_ = config.get ("asserv.r_invert_pwm"); + stepPerMm_ = config.get ("asserv.step_per_mm"); + tty_ = config.get ("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(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(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(frame.args[1]); - int motorTimer3 = static_cast(frame.args[2]); - int motorTimer2 = static_cast(frame.args[3]); - int motorTimer1 = static_cast(frame.args[4]); - int motorTimer0 = static_cast(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("asserv.tty"); - footing_ = config.get("asserv.footing"); - epsilon_ = config.get("asserv.epsilon"); - accel_ = config.get("asserv.accel"); - kp_ = config.get("asserv.kp"); - ki_ = config.get("asserv.ki"); - kd_ = config.get("asserv.kd"); - eSat_ = config.get("asserv.eSat"); - speedIntMax_ = config.get("asserv.speedIntMax"); - dSample_ = config.get("asserv.dSample"); - maxSLin_ = config.get("asserv.maxSLin"); - maxSRot_ = config.get("asserv.maxSRot"); - useTazFSM_ = config.get("asserv.useTazFSM"); - mmPpas_ = config.get("asserv.mmPpas"); - sPperiod_ = config.get("asserv.sPperiod"); - pwmMax_ = config.get("asserv.pwmMax"); - leftInvPwm_ = config.get("asserv.leftInvPwm"); - rightInvPwm_ = config.get("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((dist/mmPpas_)*256); - else - return static_cast(dist/mmPpas_); + return static_cast (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((angle / (2 * M_PI)) * 256 * (1 << 24)); - else - return static_cast((angle / (2 * M_PI))* 256); + return static_cast (step) / stepPerMm_; } -int Asserv::mms2ppp(double vitesse) +/// Convert rad to avr angles. +int +Asserv::radToAvr (double a) const { - return static_cast(vitesse * sPperiod_ / mmPpas_); + return static_cast (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(rCycl*pwmMax_); + return static_cast (a) * 2 * M_PI / (1 << 24); } -int Asserv::s2period(double period) +/// Convert rad to steps. +int +Asserv::radToStep (double a) const { - return static_cast(period/sPperiod_); + return static_cast (static_cast (footing_) * a * 0.5); } -//int Asserv::mmps2ppperiod(double accel) -//{ -// return static_cast(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 -- cgit v1.2.3