// motor.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 "motor.hh" #include /// Constructeur Motor::Motor (const Config & config) :asserv_(config, *this), log_("Motor"),idle_(true), doneDone_(false), f0Sended_(false), pinUpdated_(false) { // XXX Quant est-il des positions de départ du robot??? pStatPosition_ = config.get("motor.pStatPosition"); pStatMotor_ = config.get("motor.pStatMotor"); posX_ = 65; posY_ = -225; posA_ = 0; asserv_.setXPos(posX_); asserv_.setYPos(posY_); asserv_.setAngle(posA_); } /// Initialise les moteurs void Motor::init (void) { // on reset la carte asserv_.reset(); // on règle le rafraichissement des positions asserv_.statPosition(pStatPosition_); asserv_.statMotor(pStatMotor_); //log_ ("initialisation") << "Etat" << "Terminée"; } // Arrête les moteurs void Motor::stop(void) { asserv_.setSpeed(); idle_ = true; } double Motor::getX(void) { return posX_; } double Motor::getY(void) { return posY_; } double Motor::getA(void) { return posA_; } void Motor::setPosition(double x, double y, double a) //XXX Faire trois setteur { asserv_.setXPos(x); asserv_.setYPos(y); asserv_.setAngle(a); posX_ = x; posY_ = y; posA_ = a; } void Motor::setPwm(int leftPwm, int rightPwm) { asserv_.setPwm (leftPwm, rightPwm); } void Motor::goTo(double x, double y, double a) { //Détermination de la distance à parcourir double dist = sqrt((x - posX_) * (x- posX_) + (y - posY_) * (y - posY_)); //Détermination de l'angle qui va bien double angle = acos ((x - posX_) / dist); if((y - posY_) < 0) angle = -angle; // On remplie la file fOrdre_.push(ordre(true, dist)); fOrdre_.push(ordre(false, a)); // On execute la première commande idle_ = false; rotation(angle); } void Motor::recalage(void) { asserv_.fuckTheWall(-3); // XXX Mettre l'arg en conf //log_ ("recalage") << "Vitesse" << "-3"; idle_ = false; } bool Motor::idle(void) { return idle_; } void Motor::linearMove(double d) { asserv_.linearMove(d); //log_ ("linearMove") << "Valeur" << d; idle_ = false; } void Motor::rotation(double newA) { asserv_.angularMove(newA); //log_ ("rotation") << "Valeur" << newA; idle_ = false; } void Motor::setMaxSpeed(int maxLspeed, int maxRspeed) { asserv_.setMaxSpeed(maxLspeed, maxRspeed); } void Motor::setAccel(int accel) { asserv_.setAccel(accel); } int Motor::getMaxLSpeed(void) { return asserv_.getMaxLSpeed(); } bool Motor::sync(void) { //log_("sync") << "Entré dans sync" << ""; // On regarde si toutes les commandes ont été envoyées et aquittées if (asserv_.sync()) { //log_("sync") << "Toutes les commandes ont été envoyés et aquitté" << ""; if(doneDone_) // Si on a reçu des F depuis le dernier sync { //log_("sync") << "On a des F" << ""; if(!f0Sended_) // Si on avait pas envoyé de f0 { //log_("sync") << "On envoie le F0" << ""; asserv_.finishAck(); f0Sended_ = true; } else // Si l'AVR vient d'aquitter le F0 { //log_("sync") << "On a déjà envoyé le F0, on s'en fout" << ""; doneDone_ = false; } } else // Si on a pas reçu de F { //log_("sync") << "On a pas reçu de F" << ""; if(f0Sended_) // On peut envoyer une nouvelle commande { //log_("sync") << "Pret pour un nouveau travail" << ""; if(fOrdre_.empty()) //Si la pile est vide { //log_("sync") << "On repasse en Idle" << ""; idle_ = true; // Les moteurs sont idle } else { //log_("sync") << "Prochaine commande de la pile" << ""; if(fOrdre_.front().ordre_) // Linear Move linearMove(fOrdre_.front().arg_); else rotation(fOrdre_.front().arg_); fOrdre_.pop(); } f0Sended_ = false; } // else //log_("sync") << "Le Robot est en activité/pause, on y touche pas" << ""; } return true; } return false; } void Motor::wait(int timeout) { asserv_.wait(timeout); } /// Récupère le File Descriptor int Motor::getFd(void) { return asserv_.getFd(); } bool Motor::jackState(void) { pinUpdated_ = false; /* asserv_.statInPort(8); /// XXX La période est défini comment?? (en dur, et c'est 8 période d'asserv) while(!pinUpdated_) { sync(); } /// On fait fermer sa gueule à l'AVR sur l'epineux sujet du port d'entrée asserv_.statInPort(); */ int temp = pinState_ & 0x0040; // XXX Ouais à tester if(temp == 0x0040) // si le jack est retiré return true; else return false; } bool Motor::colorState (void) { pinUpdated_ = false; /* asserv_.statInPort(1); /// XXX La période est défini comment?? (en dur, et c'est 8 période d'asserv) while(!pinUpdated_) { sync(); } */ int temp = pinState_ & 0x0002; // XXX Ouais à tester if(temp == 0x0002) // si le jack est retiré return true; else return false; /// On fait fermer sa gueule à l'AVR sur l'epineux sujet du port d'entrée //asserv_.statInPort(); } void Motor::receiveCounter (double lMotor, double rMotor) { } void Motor::receivePosX (double xPos) { posX_ = xPos; } void Motor::receivePosY (double yPos) { posY_ = yPos; } void Motor::receivePosA (double aPos) { posA_ = aPos; } void Motor::receiveSpeedStat (int leftError, int leftInt, int rightError, int rightInt) { } void Motor::receivePwm (double leftPwm, double rightPwm) { } void Motor::receiveTiming (int motorTimer4, int motorTimer3, int motorTimer2, int motorTimer1, int motorTimer0) { } void Motor::receiveInPort (int port) { pinState_ = port; pinUpdated_ = true; } void Motor::receiveSharp (int sharp1, int sharp2, int sharp3) { } void Motor::receiveTazState(int state, int subState) { } void Motor::done (void) { //log_("done") << "Passage dans done" << ""; doneDone_ = true; }