summaryrefslogtreecommitdiff
path: root/2005/i/robert/src/asserv/asserv.cc
diff options
context:
space:
mode:
Diffstat (limited to '2005/i/robert/src/asserv/asserv.cc')
-rw-r--r--2005/i/robert/src/asserv/asserv.cc67
1 files changed, 25 insertions, 42 deletions
diff --git a/2005/i/robert/src/asserv/asserv.cc b/2005/i/robert/src/asserv/asserv.cc
index 9a3a2c4..ed24fe6 100644
--- a/2005/i/robert/src/asserv/asserv.cc
+++ b/2005/i/robert/src/asserv/asserv.cc
@@ -25,6 +25,7 @@
#include "asserv.hh"
+#include<cmath>
/// Constructeur
Asserv::Asserv (const Config & config, Receiver & receiver)
@@ -38,11 +39,6 @@ Asserv::Asserv (const Config & config, Receiver & receiver)
reset();
}
-/// Destructeur XXX Voir si il sert à quelque chose
-Asserv::~Asserv(void)
-{
-}
-
/// Reset de l'AVR
void Asserv::reset(void)
{
@@ -56,12 +52,9 @@ void Asserv::reset(void)
setKi(ki_);
setKd(kd_);
setMaxSpeed(maxSpeed_);
-
- // XXX Envoie un stop
- setSpeed();
}
-/// XXX Essaie de purger la liste d'émission et indique si elle est vide.
+/// Essaie de purger la liste d'émission et indique si elle est vide.
bool Asserv::sync(void)
{
return proto_.sync();
@@ -77,7 +70,7 @@ bool Asserv::wait(int timeout)
void Asserv::linearMove(double distance)
{
// Conversion mm->PasW
- int distPas = mm2pasW(distance);
+ int distPas = mm2pasD(distance);
// On envoie la commande à l'AVR
proto_.send('l', "w", distPas);
}
@@ -116,14 +109,14 @@ void Asserv::setSpeed(double xSpeed, double ySpeed)
proto_.send('s',"bb", vx, vy);
}
-// XXX Faut mettre le format en config
+// XXX changer rCycl2Pwm pour prendre en compte le format
void Asserv::setPwm(double xPwm, double yPwm)
{
// Conversion rCycl->Pwm
int xp = rCycl2Pwm(xPwm);
int yp = rCycl2Pwm(yPwm);
// Envoie sur l'AVR
- proto_.send('w',"ww",xp, yp);
+ proto_.send('w',"ww" ,xp, yp);
}
/// Statistiques
@@ -131,7 +124,7 @@ void Asserv::statCounter(double period)
{
pCounter_ = period;
// Convertion s->Period
- int p = s2period(period); // XXX manque un cast, vérifier tout les cast
+ int p = s2period(period);
// Envoie sur l'AVR
proto_.send('C', "b", p);
}
@@ -184,29 +177,26 @@ void Asserv::statInPort(double period)
/// Change les paramètres
void Asserv::setXPos(double xPos)
{
- // XXX Faut conserver la valeur?
// Conversion mm->pas
int p = mm2pasD(xPos);
// Envoie à l'AVR
- proto_.send('p',"bd", 'x', p); // XXX C'est bon ça??
+ proto_.send('p',"bd", 'x', p);
}
void Asserv::setYPos(double yPos)
{
- // XXX Faut conserver la valeur?
// Conversion mm->pas
int p = mm2pasD(yPos);
// Envoie à l'AVR
- proto_.send('p',"bd", 'y', p); // XXX C'est bon ça??
+ proto_.send('p',"bd", 'y', p);
}
void Asserv::setAngle(double angle)
{
- // XXX Faut conserver la valeur?
// Conversion mm->pas
- int a = mm2pasW(angle); //XXX Vérifier si c'est W ou D
+ int a = mm2pasD(angle);
// Envoie à l'AVR
- proto_.send('p',"bd", 'y', a); // XXX C'est bon ça??
+ proto_.send('p',"bd", 'y', a);
}
void Asserv::setFooting(int16_t footing)
@@ -219,33 +209,29 @@ void Asserv::setFooting(int16_t footing)
void Asserv::setEpsilon(double epsilon)
{
epsilon_ = epsilon;
- // XXX Conversion ici
// Envoie à l'AVR
- proto_.send('p',"bd", 'e', (int)epsilon);
+ proto_.send('p',"bd", 'e', static_cast<int>(epsilon * 256));
}
void Asserv::setKp(double Kp)
{
kp_ = Kp;
// Envoie à l'AVR
- // XXX Conversion ici
- proto_.send('p',"bw", 'p', (int)Kp); // XXX Vérifier les commandes pour tout
+ proto_.send('p',"bw", 'p', static_cast<int>(Kp * 256));
}
void Asserv::setKi(double Ki)
{
ki_ = Ki;
- // XXX Conversion ici
// Envoie à l'AVR
- proto_.send('p',"bw", 'i',(int) Ki);
+ proto_.send('p',"bw", 'i', static_cast<int>(Ki * 256));
}
void Asserv::setKd(double Kd)
{
kd_ = Kd;
// Envoie à l'AVR
- // XXX Conversion ici
- proto_.send('p',"bw", 'd', (int) Kd); /// XXX Vérifier la commande
+ proto_.send('p',"bw", 'd', static_cast<int>(Kd * 256));
}
void Asserv::setAccel(double accel)
@@ -261,13 +247,13 @@ void Asserv::setMaxSpeed(double maxSpeed)
// Conversion mm->ppp
int p = mms2ppp(maxSpeed);
// Envoie à l'AVR
- proto_.send('p',"bb", 'm', p); // XXX C'est bon ça??
+ proto_.send('p',"bb", 'm', p);
}
/// implémentation du proto::Receiver
void Asserv::receive(char command, const Proto::Frame &frame)
{
- // XXX Truc à mettre
+ // XXX Faire des truc ici
}
/// Charge les données de la classe config
@@ -276,30 +262,27 @@ void Asserv::loadConfig(void)
// XXX Faire des truc ici
}
-/// Fonctions de conversion
-int Asserv::mm2pasW(double dist)
-{
- return static_cast<int>(dist/mmPpas_); // XXX Ca pue
-}
-
int Asserv::mm2pasD(double dist)
{
- return static_cast<int>(dist/mmPpas_); // XXX Du cul
+ return static_cast<int>((dist/mmPpas_)*256);
}
-int Asserv::radTo256(double angle)
+int Asserv::radTo256(double angle, bool format24eme)
{
- return static_cast<int>((angle/2*3.14/*pi*/)*256); // XXX En fait tout pue
+ if(format24eme)
+ return static_cast<int>((angle / (2 * M_PI)) * (1 << 24));
+ else
+ return static_cast<int>(angle / (2 * M_PI));
}
int Asserv::mms2ppp(double vitesse)
{
- return static_cast<int>(vitesse*(sPperiod_/mmPpas_)); // XXX Ouvrez les fenêtres
+ return static_cast<int>(vitesse*(sPperiod_/mmPpas_));
}
int Asserv::rCycl2Pwm(double rCycl)
{
- return static_cast<int>(rCycl*pwmMax_); // XXX Ouvrez la clim
+ return static_cast<int>(rCycl*pwmMax_);
}
int Asserv::s2period(double period)
@@ -309,5 +292,5 @@ int Asserv::s2period(double period)
int Asserv::mmps2ppperiod(double accel)
{
- return static_cast<int>(accel*(sPperiod_*sPperiod_/mmPpas_)); // XXX Ca y est on est tous mort
+ return static_cast<int>(1/(accel*(sPperiod_*sPperiod_/mmPpas_)));
}