// 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), receiver_(receiver), log_("asserv") { // Rechargement des paramètres loadConfig(config); //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); } 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); } /// Informe l'AVR d'arréter de dire "j'ai fini" void Asserv::finishAck(void) { proto_.send('F', "b", 0); } /// 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::setSpeedIntMax(int16_t maxInt) { proto_.send('p', "bW", 'a', maxInt); } 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) { 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 xPos *= static_cast(mmPpas_); receiver_.receivePosX(xPos); break; case 'Y': int yPos; proto_.decode(frame, "D", yPos); // Convertion Pas->mm yPos *= static_cast(mmPpas_); receiver_.receivePosY(yPos); break; case 'A': int aPos; proto_.decode(frame, "D", aPos); aPos = static_cast(aPos * (2 * M_PI)); aPos /= (1 << 24); receiver_.receivePosA(aPos); break; case 'S': int oldLSpeed, LSpeed, oldRSpeed, RSpeed; proto_.decode(frame, "WWWW", oldLSpeed, LSpeed, oldRSpeed, RSpeed); receiver_.receiveMotor(oldLSpeed, LSpeed, oldRSpeed, RSpeed); break; case 'W': int leftPwm, rightPwm; proto_.decode(frame, "WW", leftPwm, rightPwm); receiver_.receivePwm(leftPwm / pwmMax_, rightPwm / pwmMax_); break; case 'T': { int motorTimer5 = static_cast(frame.args[0]); 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(motorTimer5 * sPperiod_, motorTimer4 * sPperiod_, motorTimer3 * sPperiod_, motorTimer2 * sPperiod_, motorTimer1 * sPperiod_, motorTimer0 * sPperiod_); break; } case 'P': { int statePIn; proto_.decode(frame, "b", statePIn); receiver_.receiveInPort(statePIn); break; } case 'F': receiver_.jaifini(); break; } } /// Charge les données de la classe config void Asserv::loadConfig(const Config & config) { 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"); maxSpeed_ = config.get("asserv.maxSpeed"); pwmMax_ = config.get("asserv.maxPwm"); } 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_))); }