From d2dd07c7622f4ca6d01e6013c76d771fbb897fca Mon Sep 17 00:00:00 2001 From: haller Date: Tue, 12 Apr 2005 23:02:09 +0000 Subject: Début de l'implémentation de la classe asserv Modification de la déclaration de asserv --- 2005/i/robert/src/asserv/asserv.cc | 128 +++++++++++++++++++++++++++++++++++++ 2005/i/robert/src/asserv/asserv.hh | 84 ++++++++++++++++-------- 2 files changed, 186 insertions(+), 26 deletions(-) create mode 100644 2005/i/robert/src/asserv/asserv.cc (limited to '2005') diff --git a/2005/i/robert/src/asserv/asserv.cc b/2005/i/robert/src/asserv/asserv.cc new file mode 100644 index 0000000..2489235 --- /dev/null +++ b/2005/i/robert/src/asserv/asserv.cc @@ -0,0 +1,128 @@ +// asserv.cc +// robert - programme du robot 2005 {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#include "asserv.hh" + +/// Constructeur +Asserv::Asserv (const Config & config, Receiver & receiver) + :proto_(*this), log("asserv") +{ + // Rechargement des paramètres + loadConfig(); + //Ouverture du port série + proto.open(ttyName_); + //Initialisation de l'AVR + reset(); +} + +/// Destructeur XXX Voir si il sert à quelque chose +Ãsserv::~Asserv(void) +{ +} + +/// Reset de l'AVR +Asserv::reset(void) +{ + // Reset AVR + proto_.send() + // Envoie des données de conf sur l'AVR + setFooting(footing_); + setEpsilon(epsilon_); + setAccel(accel_); + setKp(kp_); + setKi(ki_); + setKd(kd_); + setMaxSpeed(maxSpeed_); + + // XXX Envoie un stop + setSpeed(); +} + +/// XXX Essaie de purger la liste d'émission et indique si elle est vide. +bool Asserv::sync(void) +{ + return proto_.sync(); +} + +/// Attend que toute les émissions soit terminées +bool Asserv::wait(int timeout) +{ + return proto_.wait(timeout); +} + +/// Commandes d'asservissement +void Asserv::linearMove(double distance) +{ + // Conversion mm->PasW + int distPas = mm2pasW(distance); + // On envoie la commande à l'AVR + proto_.send('l', "w", distPas); +} + +void Asserv::angularMove(double angle) +{ + // Conversion radian->256ème + int a = radTo256(angle); + // Envopie vers avr + proto_.send('a',"b", a); +} + +void Asserv::goToPosition(double xPos, double yPos) +{ + // Convertion mm->PasD + int x = mm2pasD(xPos); + int y = mm2pasD(yPos); + // Envoie vers l'AVR + proto_.send('g', "dd", x, y); +} + +void Asserv::fuckTheWall(double speed) +{ + // Conversion mm/s->Pas/Period + int v = mms2ppp(speed); + // Envoie vers l'AVR + proto_.send('f',"b", v); +} + +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); +} + +// XXX Faut mettre le format en config +void Asserv::setPwm(double xPwm, double yPwm) +{ + // Conversion rCycl->Pwm + int xp = rCycl2Pwm(xPwm); + int yp = rCycl2Pwm(yPwm); + // Envoie sur l'AVR + proto_.send('w',"ww",xPwm, yPwm); +} + + diff --git a/2005/i/robert/src/asserv/asserv.hh b/2005/i/robert/src/asserv/asserv.hh index b2cca59..08b8aa0 100644 --- a/2005/i/robert/src/asserv/asserv.hh +++ b/2005/i/robert/src/asserv/asserv.hh @@ -32,49 +32,81 @@ /// Classe de dialogue avec la carte d'asservissement. class Asserv : public NonCopyable, public Proto::Receiver { + /// Les clients de Asserv doivent dériver de Receiver. + class Receiver + { + public: + /// Recoit un packet. + virtual void receive (char command, const Frame &frame) = 0; + }; // Communication avec l'AVR Proto proto_; - // Paramètres. - int accel_, kp_, ki_, kd_; - int statMotor_; - int counter_; - bool posAsserv_; - int gpiDelay_; - bool asserv_; - int zeroEps_; - bool noSetParam_; - // Ancienne valeur des compteur. - bool firstCounter_; - int countLeft_, countRight_; - int countLeftSum_, countRightSum_; - // Valeurs GPIO. - unsigned int gpi_; - unsigned int gpo_; + std::string ttyName_; + // Paramètre de l'avr + double footing_; + double epsilon_; + double accel_; + double kp_; + double ki_; + double kd_; + double maxSpeed_; + // Système de log. Log log_; public: /// Constructeur. - Asserv (const Config & config); - /// Destructeur. XXX Y'a besoin du destructeur? + Asserv (const Config & config, Receiver & receiver); + /// Destructeur. ~Asserv (void); /// Reset la carte et envois les paramètres. void reset (void); - /// Teste si l'émission est terminée. - bool ok (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. - void waitOk (void); + 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); + /// 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); //@{ /// 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 setStatMotor (int delay); - void setCounter (int delay); - void setPosAsserv (bool fl = true); - void setGpiDelay (int delay); - void setAsserv (bool fl = true); + void setMaxSpeed(int16_t); //@} + + /// implémentation du proto::Receiver + void receive (char command, const Frame &frame); + + private: + /// Charge les données de la classe config + void loadConfig(void); + /// Fonctions de conversion + int mm2pasW(double dist); + int mm2pasD(double dist); + int radTo256(double angle); + int mms2ppp(double vitesse); + int rCycl2Pwm(double rCycl); + + + }; #endif // asserv_h -- cgit v1.2.3