// 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); }