summaryrefslogtreecommitdiff
path: root/2005
diff options
context:
space:
mode:
authorhaller2005-04-30 22:55:24 +0000
committerhaller2005-04-30 22:55:24 +0000
commit79cee4f5f7fc5179c1476b8e9cf031059544cb56 (patch)
tree1b3d5603b6ee92a6a2bef76d3f76f85349920fe9 /2005
parent6ee37563decaae479821488cb5ab13838d1a4cec (diff)
Prise en charge du jack
codage de goTo Ajout de log Version qui compile, à tester
Diffstat (limited to '2005')
-rw-r--r--2005/i/robert/src/motor/motor.cc56
-rw-r--r--2005/i/robert/src/motor/motor.hh11
-rw-r--r--2005/i/robert/src/motor/test_motor.cc3
3 files changed, 61 insertions, 9 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";
}
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;
}