// 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), 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; } /// 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(); while(!idle()){} ///XXX ca pue } double Motor::getX(void) { return posX_; } double Motor::getY(void) { return posY_; } double Motor::getA(void) { return posA_; } 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(false, angle)); fOrdre_.push(ordre(true, dist)); fOrdre_.push(ordre(false, a)); // On execute la première commande idle_ = false; done (); } void Motor::recalage(void) { // XXX je ne vois pas là log_ ("recalage") << "Etat" << "A CODER!!!!"; } 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; } bool Motor::sync(void) { return asserv_.sync(); } void Motor::wait(int timeout) { asserv_.wait(timeout); } bool Motor::jackState(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_ & 0x0040; // XXX Ouais à tester if(temp == 0x0040) // 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(); } 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) { if(leftInt == 0 && rightInt == 0) // XXX c'est bon ca? idle_ = true; else idle_ = false; } 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") << "On rentre dans done" << ""; if(!idle_) { asserv_.finishAck(); if(!fOrdre_.empty()) { if(fOrdre_.front().ordre_) // Linear Move linearMove(fOrdre_.front().arg_); else rotation(fOrdre_.front().arg_); fOrdre_.pop(); } else { idle_ = true; log_ ("done") << "Action précédente" << "Terminée"; } } log_("done") << "On sort dans done" << ""; }