// motor.cc // marvin - programme du robot 2006. {{{ // // Copyright (C) 2006 Nicolas Schodet // // 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 /// Constructor. Motor::Motor (void) : asserv_ (*this), seq_ (0), finish_ (true), blocked_ (false), x_ (0.0), y_ (0.0), a_ (0.0), log_ ("Motor") { } /// Reset. void Motor::reset (void) { finish_ = true; blocked_ = false; asserv_.reset (); } /// Linear and angular move. T(mm) is the linear distance. A(mm) is the /// angular distance which means the arc length. void Motor::move (double t, double a) { nextSeq (); finish_ = false; blocked_ = false; asserv_.speedTo (t, a, seq_); } /// Rotate (rad). void Motor::rotate (double a) { nextSeq (); finish_ = false; blocked_ = false; asserv_.speedAngle (a, seq_); } /// Find a hole. void Motor::findHole (void) { nextSeq (); finish_ = false; blocked_ = false; asserv_.findHole (seq_); } void Motor::lockGoodHole (void) { ///XXX VERIFIER LES COORDONNEES const int xGood = 2000; const int yGood = -1100; ///XXX VERIFIER LES CALCULS D'AILLEURS... double rotation = atan2((yGood - y_), (xGood - x_)); rotate(rotation); } void Motor::lockBadHole (void) { ///XXX VERIFIER LES COORDONNEES const int xGood = 600; const int yGood = -1100; ///XXX VERIFIER LES CALCULS D'AILLEURS... double rotation = atan2((yGood - y_), (xGood - x_)); rotate(rotation); } /// Stop now. void Motor::stop (void) { finish_ = true; blocked_ = false; asserv_.speed (0, 0); } /// get the file descriptor int Motor::getFd(void) const { return asserv_.getFd(); } /// get the position of robal void Motor::getPosition(double & x, double & y, double & a) const { x = x_; y = y_; a = a_; } /// Next seq number. void Motor::nextSeq (void) { seq_++; if (seq_ > 100) seq_ = 1; } /// Implement Asserv::Receiver callbacks. void Motor::receiveAck (int seq) { if ((seq & 0x7f) == seq_) { asserv_.ack (seq); finish_ = true; if (seq & 0x80) { blocked_ = true; } } } /// The Best Evite Program ;p void Motor::avoidrobot(void) { rotate(3.14116/2);//faire calcul ailleur move(100,0); rotate(3.14116/2*(-1));//voir plus haut move(300,0); rotate(3.14116/2*(-1)); move(100,0); rotate(3.14116/2); move(300,0); } void Motor::receiveCounterStat (int l, int r) { } void Motor::receivePos (double x, double y, double a) { x_ = x; y_ = y; a_ = a; log_ ("Pos") << "x " << x << " y " << y << " a " << a / (2 * M_PI) * 360; } void Motor::receiveSpeedStat (int t, int a) { } void Motor::receivePosStat (int te, int ti, int ae, int ai) { } void Motor::receivePwmStat (int l, int r) { } void Motor::receiveTimerStat (const int *t, int tn) { } void Motor::receiveInPort (unsigned int port) { }