summaryrefslogtreecommitdiff
path: root/2005/i/robert/src/motor/motor.cc
diff options
context:
space:
mode:
Diffstat (limited to '2005/i/robert/src/motor/motor.cc')
-rw-r--r--2005/i/robert/src/motor/motor.cc56
1 files changed, 49 insertions, 7 deletions
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 <cmath>
+
/// Constructeur
Motor::Motor (const Config & config)
- :asserv_(config, *this), idle_(true)
+ :asserv_(config, *this), log_("Motor"),idle_(true), pinUpdated_(false)
{
- pStatPosition = config.get<int>("motor.pStatPosition");
- pStatMotor = config.get<int>("motor.pStatMotor");
+ // XXX Quant est-il des positions de départ du robot???
+ pStatPosition_ = config.get<int>("motor.pStatPosition");
+ pStatMotor_ = config.get<int>("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";
}