// 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" #include /// 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(); } /// 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_); setAccel(accel_); setKp(kp_); setKi(ki_); setKd(kd_); setMaxSpeed(maxSpeed_); } /// 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 = mm2pasD(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", vx, vy); } // XXX changer rCycl2Pwm pour prendre en compte le format 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" ,xp, yp); } /// Statistiques void Asserv::statCounter(double period) { pCounter_ = period; // Convertion s->Period int p = s2period(period); // 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) { // Conversion mm->pas int p = mm2pasD(xPos); // Envoie à l'AVR proto_.send('p',"bd", 'x', p); } void Asserv::setYPos(double yPos) { // Conversion mm->pas int p = mm2pasD(yPos); // Envoie à l'AVR proto_.send('p',"bd", 'y', p); } void Asserv::setAngle(double angle) { // Conversion mm->pas int a = mm2pasD(angle); // Envoie à l'AVR proto_.send('p',"bd", 'y', 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; // Envoie à l'AVR proto_.send('p',"bd", 'e', static_cast(epsilon * 256)); } void Asserv::setKp(double Kp) { kp_ = Kp; // Envoie à l'AVR proto_.send('p',"bw", 'p', static_cast(Kp * 256)); } void Asserv::setKi(double Ki) { ki_ = Ki; // Envoie à l'AVR proto_.send('p',"bw", 'i', static_cast(Ki * 256)); } void Asserv::setKd(double Kd) { kd_ = Kd; // Envoie à l'AVR proto_.send('p',"bw", 'd', static_cast(Kd * 256)); } 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); } /// implémentation du proto::Receiver void Asserv::receive(char command, const Proto::Frame &frame) { // XXX Faire des truc ici } /// Charge les données de la classe config void Asserv::loadConfig(void) { // XXX Faire des truc ici } int Asserv::mm2pasD(double dist) { return static_cast((dist/mmPpas_)*256); } int Asserv::radTo256(double angle, bool format24eme) { if(format24eme) return static_cast((angle / (2 * M_PI)) * (1 << 24)); else return static_cast(angle / (2 * M_PI)); } int Asserv::mms2ppp(double vitesse) { return static_cast(vitesse*(sPperiod_/mmPpas_)); } int Asserv::rCycl2Pwm(double rCycl) { return static_cast(rCycl*pwmMax_); } int Asserv::s2period(double period) { return static_cast(period/sPperiod_); } int Asserv::mmps2ppperiod(double accel) { return static_cast(1/(accel*(sPperiod_*sPperiod_/mmPpas_))); }