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.cc157
1 files changed, 116 insertions, 41 deletions
diff --git a/2005/i/robert/src/asserv/asserv.cc b/2005/i/robert/src/asserv/asserv.cc
index c1be051..fc17d9e 100644
--- a/2005/i/robert/src/asserv/asserv.cc
+++ b/2005/i/robert/src/asserv/asserv.cc
@@ -47,17 +47,21 @@ void Asserv::reset(void)
// Envoie des données de conf sur l'AVR
setFooting(footing_);
setEpsilon(epsilon_);
- setAccel(accel_);
setKp(kp_);
setKi(ki_);
setKd(kd_);
+ setSpeedIntMax(speedIntMax_);
+ setAccel(accel_);
setMaxSpeed(maxSpeed_);
+ setUseTazFSM(useTazFSM_);
+ setInvPwm(leftInvPwm_, rightInvPwm_);
statCounter(pCounter_);
statPosition(pPosition_);
statMotor(pMotor_);
statPwm(pPwm_);
statTiming(pTiming_);
statInPort(pStatInPort_);
+ statSharp(pStatInPort_);
}
/// Essaie de purger la liste d'émission et indique si elle est vide.
@@ -76,7 +80,8 @@ bool Asserv::wait(int timeout)
void Asserv::linearMove(double distance)
{
// Conversion mm->PasW
- int distPas = mm2pasD(distance);
+ int distPas = mm2pas(distance);
+ log_ ("linearMove") << "avConv" << distance << "apConv" << distPas;
// On envoie la commande à l'AVR
proto_.send('l', "w", distPas);
}
@@ -85,6 +90,7 @@ void Asserv::angularMove(double angle)
{
// Conversion radian->256ème
int a = radTo256(angle);
+ log_ ("angularMove") << "avConv" << angle << "apConv" << a;
// Envopie vers avr
proto_.send('a',"b", a);
}
@@ -92,8 +98,10 @@ void Asserv::angularMove(double angle)
void Asserv::goToPosition(double xPos, double yPos)
{
// Convertion mm->PasD
- int x = mm2pasD(xPos);
- int y = mm2pasD(yPos);
+ int x = mm2pas(xPos, true);
+ int y = mm2pas(yPos, true);
+ log_ ("goToPosition") << "avConvX" << xPos << "apConvX" << x;
+ log_ ("goToPosition") << "avConvY" << yPos << "apConvY" << y;
// Envoie vers l'AVR
proto_.send('g', "dd", x, y);
}
@@ -102,8 +110,16 @@ void Asserv::fuckTheWall(double speed)
{
// Conversion mm/s->Pas/Period
int v = mms2ppp(speed);
+ log_ ("fuckTheWall") << "avConv" << speed << "apConv" << v;
// Envoie vers l'AVR
- proto_.send('f',"b", v);
+ fuckTheWall(v);
+}
+
+void Asserv::fuckTheWall(int speed)
+{
+ log_ ("fuckTheWall") << "avConv";
+ // Envoie vers l'AVR
+ proto_.send('f',"b", speed);
}
void Asserv::setSpeed(double xSpeed, double ySpeed)
@@ -111,8 +127,18 @@ void Asserv::setSpeed(double xSpeed, double ySpeed)
// Conversion mm/s->Pas/Period
int vx = mms2ppp(xSpeed);
int vy = mms2ppp(ySpeed);
+ log_ ("setSpeed") << "avConvX" << xSpeed << "apConvX" << vx;
+ log_ ("setSpeed") << "avConvY" << ySpeed << "apConvY" << vy;
// Envoie vers l'AVR
- proto_.send('s',"bb", vx, vy);
+ setSpeed(vx,vy);
+}
+
+void Asserv::setSpeed(int xSpeed, int ySpeed)
+{
+ log_ ("setSpeed") << "avConvX" << xSpeed ;
+ log_ ("setSpeed") << "avConvY" << ySpeed ;
+ // Envoie vers l'AVR
+ proto_.send('s',"bb", xSpeed, ySpeed);
}
void Asserv::setPwm(double xPwm, double yPwm)
@@ -120,6 +146,8 @@ void Asserv::setPwm(double xPwm, double yPwm)
// Conversion rCycl->Pwm
int xp = rCycl2Pwm(xPwm);
int yp = rCycl2Pwm(yPwm);
+ log_ ("setPwm") << "avConvX" << xPwm << "apConvX" << xp;
+ log_ ("setPwm") << "avConvY" << yPwm << "apConvY" << yp;
// Envoie sur l'AVR
proto_.send('w',"ww" ,xp, yp);
}
@@ -131,16 +159,17 @@ void Asserv::finishAck(void)
}
/// Statistiques
-void Asserv::statCounter(double period)
+void Asserv::statCounter(int period)
{
pCounter_ = period;
// Convertion s->Period
int p = s2period(period);
+ log_ ("statCounter") << "avConv" << period << "apConv" << p;
// Envoie sur l'AVR
proto_.send('C', "b", p);
}
-void Asserv::statPosition(double period)
+void Asserv::statPosition(int period)
{
pPosition_ = period;
// Convertion s->Period
@@ -149,7 +178,7 @@ void Asserv::statPosition(double period)
proto_.send('X', "b", p);
}
-void Asserv::statMotor(double period)
+void Asserv::statMotor(int period)
{
pMotor_ = period;
// Convertion s->Period
@@ -158,7 +187,7 @@ void Asserv::statMotor(double period)
proto_.send('S', "b", p);
}
-void Asserv::statPwm(double period)
+void Asserv::statPwm(int period)
{
pPwm_ = period;
// Convertion s->Period
@@ -167,7 +196,7 @@ void Asserv::statPwm(double period)
proto_.send('W', "b", p);
}
-void Asserv::statTiming(double period)
+void Asserv::statTiming(int period)
{
pTiming_ = period;
// Convertion s->Period
@@ -176,7 +205,7 @@ void Asserv::statTiming(double period)
proto_.send('T', "b", p);
}
-void Asserv::statInPort(double period)
+void Asserv::statInPort(int period)
{
pStatInPort_ = period;
// Convertion s->Period
@@ -185,11 +214,20 @@ void Asserv::statInPort(double period)
proto_.send('P', "b", p);
}
+void Asserv::statSharp(int period)
+{
+ pSharp_ = period;
+ // Conversion s->period
+ int p = s2period(period);
+ // Envoie sur l'AVR
+ proto_.send('H', "b", p);
+}
+
/// Change les paramètres
void Asserv::setXPos(double xPos)
{
// Conversion mm->pas
- int p = mm2pasD(xPos);
+ int p = mm2pas(xPos);
// Envoie à l'AVR
proto_.send('p',"bd", 'x', p);
}
@@ -197,7 +235,7 @@ void Asserv::setXPos(double xPos)
void Asserv::setYPos(double yPos)
{
// Conversion mm->pas
- int p = mm2pasD(yPos);
+ int p = mm2pas(yPos);
// Envoie à l'AVR
proto_.send('p',"bd", 'y', p);
}
@@ -205,7 +243,7 @@ void Asserv::setYPos(double yPos)
void Asserv::setAngle(double angle)
{
// Conversion mm->pas
- int a = mm2pasD(angle);
+ int a = radTo256(angle);
// Envoie à l'AVR
proto_.send('p',"bd", 'y', a);
}
@@ -245,28 +283,50 @@ void Asserv::setKd(double Kd)
proto_.send('p',"bw", 'd', static_cast<int>(Kd * 256));
}
-void Asserv::setSpeedIntMax(int16_t maxInt)
+void Asserv::setSpeedIntMax(int maxInt)
{
speedIntMax_ = maxInt;
proto_.send('p', "bW", 'a', maxInt);
}
-void Asserv::setAccel(double accel)
+void Asserv::setAccel(int accel)
{
- // Conversion mmps2ppperiod
- int a = mmps2ppperiod(accel);
+ accel_ = accel_;
+ log_ ("setAccel") << "avConv" << accel;
// envoie vers l'AVR
- proto_.send('p',"bb", 'a', a);
+ proto_.send('p',"bb", 'a', accel);
+}
+
+/// Renvoie l'accélération en unité international (mm/s²)
+double Asserv::getAccel(void)
+{
+ return mmPpas_ / (sPperiod_ * sPperiod_) / accel_;
}
-void Asserv::setMaxSpeed(double maxSpeed)
+void Asserv::setMaxSpeed(int maxSpeed)
{
+ maxSpeed_ = maxSpeed;
// Conversion mm->ppp
int p = mms2ppp(maxSpeed);
// Envoie à l'AVR
proto_.send('p',"bb", 'm', p);
}
+void Asserv::setUseTazFSM(bool use)
+{
+ useTazFSM_ = use;
+ // Envoie la commande à l'AVR
+ proto_.send('t', "b", use ? 1 : 0);
+}
+
+void Asserv::setInvPwm(bool leftInvPwm, bool rightInvPwm)
+{
+ leftInvPwm_ = leftInvPwm;
+ rightInvPwm_ = rightInvPwm;
+ // Envoie la commande à l'AVR
+ proto_.send('p', "bbb", 'W', leftInvPwm ? 1 : 0, rightInvPwm ? 1 : 0);
+}
+
/// implémentation du proto::Receiver
void Asserv::receive(char command, const Proto::Frame &frame)
{
@@ -302,7 +362,7 @@ void Asserv::receive(char command, const Proto::Frame &frame)
case 'S':
int oldLSpeed, LSpeed, oldRSpeed, RSpeed;
proto_.decode(frame, "WWWW", oldLSpeed, LSpeed, oldRSpeed, RSpeed);
- receiver_.receiveMotor(oldLSpeed, LSpeed, oldRSpeed, RSpeed);
+ receiver_.receiveSpeedStat(oldLSpeed, LSpeed, oldRSpeed, RSpeed);
break;
case 'W':
int leftPwm, rightPwm;
@@ -311,18 +371,13 @@ void Asserv::receive(char command, const Proto::Frame &frame)
break;
case 'T':
{
- int motorTimer5 = static_cast<int>(frame.args[0]);
int motorTimer4 = static_cast<int>(frame.args[1]);
int motorTimer3 = static_cast<int>(frame.args[2]);
int motorTimer2 = static_cast<int>(frame.args[3]);
int motorTimer1 = static_cast<int>(frame.args[4]);
int motorTimer0 = static_cast<int>(frame.args[5]);
- receiver_.receiveTiming(motorTimer5 * sPperiod_,
- motorTimer4 * sPperiod_,
- motorTimer3 * sPperiod_,
- motorTimer2 * sPperiod_,
- motorTimer1 * sPperiod_,
- motorTimer0 * sPperiod_);
+ receiver_.receiveTiming(motorTimer4, motorTimer3, motorTimer2,
+ motorTimer1, motorTimer0);
break;
}
case 'P':
@@ -332,8 +387,21 @@ void Asserv::receive(char command, const Proto::Frame &frame)
receiver_.receiveInPort(statePIn);
break;
}
+ case 'H':
+ {
+ int stateSharp0, stateSharp1, stateSharp2;
+ proto_.decode(frame, "www", stateSharp0, stateSharp1, stateSharp2);
+ receiver_.receiveSharp(stateSharp0, stateSharp1, stateSharp2);
+ break;
+ }
+ case 't':
+ {
+ int state, subState;
+ proto_.decode(frame, "bb", state, subState);
+ receiver_.receiveTazState(state, subState);
+ }
case 'F':
- receiver_.jaifini();
+ receiver_.done();
break;
}
}
@@ -344,34 +412,41 @@ void Asserv::loadConfig(const Config & config)
ttyName_ = config.get<std::string>("asserv.tty");
footing_ = config.get<int>("asserv.footing");
epsilon_ = config.get<double>("asserv.epsilon");
- accel_ = config.get<double>("asserv.accel");
+ accel_ = config.get<int>("asserv.accel");
kp_ = config.get<double>("asserv.kp");
ki_ = config.get<double>("asserv.ki");
kd_ = config.get<double>("asserv.kd");
speedIntMax_ = config.get<int>("asserv.speedIntMax");
- maxSpeed_ = config.get<double>("asserv.maxSpeed");
+ maxSpeed_ = config.get<int>("asserv.maxSpeed");
+ useTazFSM_ = config.get<bool>("asserv.useTazFSM");
mmPpas_ = config.get<double>("asserv.mmPpas");
sPperiod_ = config.get<double>("asserv.sPperiod");
- pwmMax = config.get<double>("asserv.pwmMax");
+ pwmMax_ = config.get<int>("asserv.pwmMax");
+ leftInvPwm_ = config.get<bool>("asserv.leftInvPwm");
+ rightInvPwm_ = config.get<bool>("asserv.rightInvPwm");
pCounter_ = 0;
pPosition_ = 0;
pMotor_ = 0;
pPwm_ = 0;
pTiming_ = 0;
pStatInPort_ = 0;
+ pSharp_ = 0;
}
-int Asserv::mm2pasD(double dist)
+int Asserv::mm2pas(double dist, bool format24eme)
{
- return static_cast<int>((dist/mmPpas_)*256);
+ if(format24eme)
+ return static_cast<int>((dist/mmPpas_)*256);
+ else
+ return static_cast<int>(dist/mmPpas_);
}
int Asserv::radTo256(double angle, bool format24eme)
{
if(format24eme)
- return static_cast<int>((angle / (2 * M_PI)) * (1 << 24));
+ return static_cast<int>((angle / (2 * M_PI)) * 256 * (1 << 24));
else
- return static_cast<int>(angle / (2 * M_PI));
+ return static_cast<int>((angle / (2 * M_PI))* 256);
}
int Asserv::mms2ppp(double vitesse)
@@ -389,7 +464,7 @@ int Asserv::s2period(double period)
return static_cast<int>(period/sPperiod_);
}
-int Asserv::mmps2ppperiod(double accel)
-{
- return static_cast<int>(1/(accel*(sPperiod_*sPperiod_/mmPpas_)));
-}
+//int Asserv::mmps2ppperiod(double accel)
+//{
+// return static_cast<int>(1/(accel*(sPperiod_*sPperiod_/mmPpas_)));
+//}