From 79cee4f5f7fc5179c1476b8e9cf031059544cb56 Mon Sep 17 00:00:00 2001 From: haller Date: Sat, 30 Apr 2005 22:55:24 +0000 Subject: Prise en charge du jack codage de goTo Ajout de log Version qui compile, à tester --- 2005/i/robert/src/motor/motor.cc | 56 ++++++++++++++++++++++++++++++----- 2005/i/robert/src/motor/motor.hh | 11 +++++-- 2005/i/robert/src/motor/test_motor.cc | 3 ++ 3 files changed, 61 insertions(+), 9 deletions(-) (limited to '2005/i/robert/src') diff --git a/2005/i/robert/src/motor/motor.cc b/2005/i/robert/src/motor/motor.cc index 0bc7525..b743d1d 100644 --- a/2005/i/robert/src/motor/motor.cc +++ b/2005/i/robert/src/motor/motor.cc @@ -25,12 +25,15 @@ #include "motor.hh" +#include + /// Constructeur Motor::Motor (const Config & config) - :asserv_(config, *this), idle_(true) + :asserv_(config, *this), log_("Motor"),idle_(true), pinUpdated_(false) { - pStatPosition = config.get("motor.pStatPosition"); - pStatMotor = config.get("motor.pStatMotor"); + // XXX Quant est-il des positions de départ du robot??? + pStatPosition_ = config.get("motor.pStatPosition"); + pStatMotor_ = config.get("motor.pStatMotor"); } /// Initialise les moteurs @@ -41,17 +44,18 @@ void Motor::init (void) // On stop le moteur stop(); // on règle le rafraichissement des positions - asserv_.statPosition(pStatPosition); - asserv_.statMotor(pStatMotor); + asserv_.statPosition(pStatPosition_); + asserv_.statMotor(pStatMotor_); // on remet les position à 0 XXX à 0? dns ai en dur posX_ = posY_ = posA_ = 0; + log_ ("initialisation") << "Etat" << "Terminée"; } // Arrête les moteurs void Motor::stop(void) { asserv_.setSpeed(); - while(!idle()){} ///XXX + while(!idle()){} ///XXX ca pue } double Motor::getX(void) @@ -71,12 +75,28 @@ double Motor::getA(void) void Motor::goTo(double x, double y, double a) { - // XXX Méthode carré ou pas? + + //Détermination de la distance à parcourir + double dist = sqrt((x - posX_) * (x- posX_) + (y - posY_) * (y - posY_)); // XXX A vérifier + //Détermination de l'angle qui va bien + double angle = acos ((x - posX_) / dist); + + //On tourne + rotation(angle); // XXX est-ce que ca attend quelque chose avant de passer à la commande suivant ou bien... + log_ ("goTo") << "Etat" << "Rotation" << "angle" << angle; + // XXX while (!idle) { sync();} + //On avance + linearMove(dist); + log_("goTo") << "Etat" << "linearMove" << "distance" << dist; + // rotation vers l'angle a + rotation(a); + log_("goTo") << "Etat" << "Dernière rotation" << "angle" << a; } void Motor::recalage(void) { // XXX je ne vois pas là + log_ ("recalage") << "Etat" << "A CODER!!!!"; } bool Motor::idle(void) @@ -87,12 +107,14 @@ bool Motor::idle(void) 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; } @@ -106,6 +128,23 @@ 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(); +} + void Motor::receiveCounter (double lMotor, double rMotor) { } @@ -146,6 +185,8 @@ void Motor::receiveTiming (int motorTimer4, void Motor::receiveInPort (int port) { + pinState_ = port; + pinUpdated_ = true; } void Motor::receiveSharp (int sharp1, int sharp2, int sharp3) @@ -160,4 +201,5 @@ void Motor::done (void) { idle_ = true; asserv_.finishAck(); + log_ ("done") << "Action précédente" << "Terminée"; } diff --git a/2005/i/robert/src/motor/motor.hh b/2005/i/robert/src/motor/motor.hh index 60eb504..1ab092a 100644 --- a/2005/i/robert/src/motor/motor.hh +++ b/2005/i/robert/src/motor/motor.hh @@ -34,6 +34,8 @@ class Motor : public Asserv::Receiver private: /// Communication avec l'asservissement Asserv asserv_; + /// Système de log + Log log_; /// position double posX_; double posY_; @@ -42,8 +44,11 @@ class Motor : public Asserv::Receiver /// Etat des commandes bool idle_; /// Paramètre de conf - int pStatPosition; - int pStatMotor; + int pStatPosition_; + int pStatMotor_; + /// Etat du port d'entrée + int pinState_; + bool pinUpdated_; public: /// Constructeur @@ -72,6 +77,8 @@ class Motor : public Asserv::Receiver bool sync(void); /// On attend... void wait(int timeout); + /// Retoure l'état du jack (false entrée et true sortie) + bool jackState(void); /// déclaration des fonctions de receiver void receiveCounter (double lMotor, double rMotor); void receivePosX (double xPos); diff --git a/2005/i/robert/src/motor/test_motor.cc b/2005/i/robert/src/motor/test_motor.cc index 477da41..9ccd4b8 100644 --- a/2005/i/robert/src/motor/test_motor.cc +++ b/2005/i/robert/src/motor/test_motor.cc @@ -99,6 +99,9 @@ main (int argc, char **argv) throw std::runtime_error("syntax error"); motor.rotation(strtod(argv[i], 0)); break; + case 'J': + std::cout << "Etat du Jack: " << (motor.jackState() ? "Retiré" : "Dedans"); + break; } break; } -- cgit v1.2.3