From 04d791eb4668d32e84be22ea2ce8c221312f0e47 Mon Sep 17 00:00:00 2001 From: haller Date: Wed, 13 Apr 2005 20:42:50 +0000 Subject: Première version qui compile Elle ne marche pas, y'a encore du boulot --- 2005/i/robert/src/asserv/asserv.cc | 201 +++++++++++++++++++++++++++++++++++-- 2005/i/robert/src/asserv/asserv.hh | 68 +++++++------ 2 files changed, 233 insertions(+), 36 deletions(-) (limited to '2005/i/robert') diff --git a/2005/i/robert/src/asserv/asserv.cc b/2005/i/robert/src/asserv/asserv.cc index 2489235..9a3a2c4 100644 --- a/2005/i/robert/src/asserv/asserv.cc +++ b/2005/i/robert/src/asserv/asserv.cc @@ -25,28 +25,29 @@ #include "asserv.hh" + /// Constructeur Asserv::Asserv (const Config & config, Receiver & receiver) - :proto_(*this), log("asserv") + :proto_(*this), log_("asserv") { // Rechargement des paramètres loadConfig(); //Ouverture du port série - proto.open(ttyName_); + proto_.open(ttyName_); //Initialisation de l'AVR reset(); } /// Destructeur XXX Voir si il sert à quelque chose -Ãsserv::~Asserv(void) +Asserv::~Asserv(void) { } /// Reset de l'AVR -Asserv::reset(void) +void Asserv::reset(void) { // Reset AVR - proto_.send() + proto_.send('z'); // Envoie des données de conf sur l'AVR setFooting(footing_); setEpsilon(epsilon_); @@ -106,13 +107,13 @@ void Asserv::fuckTheWall(double speed) proto_.send('f',"b", v); } -void Asserv::setSpeed(double xSpeed, double, ySpeed) +void Asserv::setSpeed(double xSpeed, double ySpeed) { // Conversion mm/s->Pas/Period int vx = mms2ppp(xSpeed); int vy = mms2ppp(ySpeed); // Envoie vers l'AVR - proto_.send('s',"bb", xSpeed, ySpeed); + proto_.send('s',"bb", vx, vy); } // XXX Faut mettre le format en config @@ -122,7 +123,191 @@ void Asserv::setPwm(double xPwm, double yPwm) int xp = rCycl2Pwm(xPwm); int yp = rCycl2Pwm(yPwm); // Envoie sur l'AVR - proto_.send('w',"ww",xPwm, yPwm); + proto_.send('w',"ww",xp, yp); +} + +/// Statistiques +void Asserv::statCounter(double period) +{ + pCounter_ = period; + // Convertion s->Period + int p = s2period(period); // XXX manque un cast, vérifier tout les cast + // Envoie sur l'AVR + proto_.send('C', "b", p); +} + +void Asserv::statPosition(double period) +{ + pPosition_ = period; + // Convertion s->Period + int p = s2period(period); + // Envoie sur l'AVR + proto_.send('X', "b", p); +} + +void Asserv::statMotor(double period) +{ + pMotor_ = period; + // Convertion s->Period + int p = s2period(period); + // Envoie sur l'AVR + proto_.send('S', "b", p); +} + +void Asserv::statPwm(double period) +{ + pPwm_ = period; + // Convertion s->Period + int p = s2period(period); + // Envoie sur l'AVR + proto_.send('W', "b", p); +} + +void Asserv::statTiming(double period) +{ + pTiming_ = period; + // Convertion s->Period + int p = s2period(period); + // Envoie sur l'AVR + proto_.send('T', "b", p); +} + +void Asserv::statInPort(double period) +{ + pStatInPort_ = period; + // Convertion s->Period + int p = s2period(period); + // Envoie sur l'AVR + proto_.send('P', "b", p); +} + +/// Change les paramètres +void Asserv::setXPos(double xPos) +{ + // XXX Faut conserver la valeur? + // Conversion mm->pas + int p = mm2pasD(xPos); + // Envoie à l'AVR + proto_.send('p',"bd", 'x', p); // XXX C'est bon ça?? +} + +void Asserv::setYPos(double yPos) +{ + // XXX Faut conserver la valeur? + // Conversion mm->pas + int p = mm2pasD(yPos); + // Envoie à l'AVR + proto_.send('p',"bd", 'y', p); // XXX C'est bon ça?? +} + +void Asserv::setAngle(double angle) +{ + // XXX Faut conserver la valeur? + // Conversion mm->pas + int a = mm2pasW(angle); //XXX Vérifier si c'est W ou D + // Envoie à l'AVR + proto_.send('p',"bd", 'y', a); // XXX C'est bon ça?? +} + +void Asserv::setFooting(int16_t footing) +{ + footing_ = footing; + // Envoie à l'AVR + proto_.send('p',"bw", 'f', footing); +} + +void Asserv::setEpsilon(double epsilon) +{ + epsilon_ = epsilon; + // XXX Conversion ici + // Envoie à l'AVR + proto_.send('p',"bd", 'e', (int)epsilon); } +void Asserv::setKp(double Kp) +{ + kp_ = Kp; + // Envoie à l'AVR + // XXX Conversion ici + proto_.send('p',"bw", 'p', (int)Kp); // XXX Vérifier les commandes pour tout +} + +void Asserv::setKi(double Ki) +{ + ki_ = Ki; + // XXX Conversion ici + // Envoie à l'AVR + proto_.send('p',"bw", 'i',(int) Ki); +} + +void Asserv::setKd(double Kd) +{ + kd_ = Kd; + // Envoie à l'AVR + // XXX Conversion ici + proto_.send('p',"bw", 'd', (int) Kd); /// XXX Vérifier la commande +} + +void Asserv::setAccel(double accel) +{ + // Conversion mmps2ppperiod + int a = mmps2ppperiod(accel); + // envoie vers l'AVR + proto_.send('p',"bb", 'a', a); +} + +void Asserv::setMaxSpeed(double maxSpeed) +{ + // Conversion mm->ppp + int p = mms2ppp(maxSpeed); + // Envoie à l'AVR + proto_.send('p',"bb", 'm', p); // XXX C'est bon ça?? +} + +/// implémentation du proto::Receiver +void Asserv::receive(char command, const Proto::Frame &frame) +{ + // XXX Truc à mettre +} + +/// Charge les données de la classe config +void Asserv::loadConfig(void) +{ + // XXX Faire des truc ici +} +/// Fonctions de conversion +int Asserv::mm2pasW(double dist) +{ + return static_cast(dist/mmPpas_); // XXX Ca pue +} + +int Asserv::mm2pasD(double dist) +{ + return static_cast(dist/mmPpas_); // XXX Du cul +} + +int Asserv::radTo256(double angle) +{ + return static_cast((angle/2*3.14/*pi*/)*256); // XXX En fait tout pue +} + +int Asserv::mms2ppp(double vitesse) +{ + return static_cast(vitesse*(sPperiod_/mmPpas_)); // XXX Ouvrez les fenêtres +} + +int Asserv::rCycl2Pwm(double rCycl) +{ + return static_cast(rCycl*pwmMax_); // XXX Ouvrez la clim +} + +int Asserv::s2period(double period) +{ + return static_cast(period/sPperiod_); +} + +int Asserv::mmps2ppperiod(double accel) +{ + return static_cast(accel*(sPperiod_*sPperiod_/mmPpas_)); // XXX Ca y est on est tous mort +} diff --git a/2005/i/robert/src/asserv/asserv.hh b/2005/i/robert/src/asserv/asserv.hh index 08b8aa0..f587c71 100644 --- a/2005/i/robert/src/asserv/asserv.hh +++ b/2005/i/robert/src/asserv/asserv.hh @@ -26,6 +26,7 @@ // }}} #include "utils/non_copyable.hh" +#include "config/config.hh" #include "proto/proto.hh" #include "log/log.hh" @@ -37,25 +38,37 @@ class Asserv : public NonCopyable, public Proto::Receiver { public: /// Recoit un packet. - virtual void receive (char command, const Frame &frame) = 0; + virtual void receive (char command) = 0; // XXX Classe à définir }; // Communication avec l'AVR Proto proto_; std::string ttyName_; // Paramètre de l'avr - double footing_; + int16_t footing_; double epsilon_; double accel_; double kp_; double ki_; double kd_; double maxSpeed_; + // Stat + double pCounter_; + double pPosition_; + double pMotor_; + double pPwm_; + double pTiming_; + double pStatInPort_; + + // Unités + double mmPpas_; + double sPperiod_; + double pwmMax_; // Système de log. Log log_; public: /// Constructeur. - Asserv (const Config & config, Receiver & receiver); + Asserv (const Config & config, Asserv::Receiver & receiver); /// Destructeur. ~Asserv (void); /// Reset la carte et envois les paramètres. @@ -66,34 +79,34 @@ class Asserv : public NonCopyable, public Proto::Receiver bool wait (int timeout = -1); /// Commandes asservissement void linearMove(double distance); - void angularMove(int8_t angle); - void goToPosition(int32_t xPos, int32_t yPos); - void fuckTheWall(int8_t speed); - void setSpeed(int8_t xSpeed = 0, int8_t ySpeed = 0); - void setPwm(int16_t leftPwm = 0, int16_t rightPwm = 0); + void angularMove(double angle); + void goToPosition(double xPos, double yPos); + void fuckTheWall(double speed); + void setSpeed(double xSpeed = 0, double ySpeed = 0); + void setPwm(double leftPwm = 0, double rightPwm = 0); /// Statistiques - void statCounter(int8_t period = 0); - void statPosition(int8_t period = 0); - void statMotor(int8_t period = 0); - void statPwm(int8_t period = 0); - void statTimer(int8_t period = 0); - void statInPort(int8_t period = 0); + void statCounter(double period = 0); + void statPosition(double period = 0); + void statMotor(double period = 0); + void statPwm(double period = 0); + void statTiming(double period = 0); + void statInPort(double period = 0); //@{ /// Change les paramètres de la carte. - void setXPos(int32_t pos); - void setYPos(int32_t pos); - void setAngle(int32_t angle); - void setFooting(int32_t dist); - void setEpsilon(int32_t e); - void setAccel (int accel); - void setKp (int kp); - void setKi (int ki); - void setKd (int kd); - void setMaxSpeed(int16_t); + void setXPos(double pos); + void setYPos(double pos); + void setAngle(double angle); + void setFooting(int16_t dist); + void setEpsilon(double e); + void setKp (double kp); + void setKi (double ki); + void setKd (double kd); + void setAccel (double accel); + void setMaxSpeed(double maxSpeed); //@} /// implémentation du proto::Receiver - void receive (char command, const Frame &frame); + void receive (char command, const Proto::Frame &frame); private: /// Charge les données de la classe config @@ -104,9 +117,8 @@ class Asserv : public NonCopyable, public Proto::Receiver int radTo256(double angle); int mms2ppp(double vitesse); int rCycl2Pwm(double rCycl); - - - + int s2period(double period); + int mmps2ppperiod(double accel); }; #endif // asserv_h -- cgit v1.2.3