From 21af4d885eff12ebe6c6d0472ec4b3897bcbaeae Mon Sep 17 00:00:00 2001 From: haller Date: Sat, 16 Apr 2005 23:43:45 +0000 Subject: Modifications diverses Codages de test_asserv --- 2005/i/robert/src/asserv/asserv.cc | 96 +++++++++++++++++++++++++++++++++++--- 1 file changed, 89 insertions(+), 7 deletions(-) (limited to '2005/i/robert/src/asserv/asserv.cc') diff --git a/2005/i/robert/src/asserv/asserv.cc b/2005/i/robert/src/asserv/asserv.cc index ed24fe6..e3a0f59 100644 --- a/2005/i/robert/src/asserv/asserv.cc +++ b/2005/i/robert/src/asserv/asserv.cc @@ -29,10 +29,10 @@ /// Constructeur Asserv::Asserv (const Config & config, Receiver & receiver) - :proto_(*this), log_("asserv") + :proto_(*this), receiver_(receiver), log_("asserv") { // Rechargement des paramètres - loadConfig(); + loadConfig(config); //Ouverture du port série proto_.open(ttyName_); //Initialisation de l'AVR @@ -109,7 +109,6 @@ void Asserv::setSpeed(double xSpeed, double ySpeed) proto_.send('s',"bb", vx, vy); } -// XXX changer rCycl2Pwm pour prendre en compte le format void Asserv::setPwm(double xPwm, double yPwm) { // Conversion rCycl->Pwm @@ -119,6 +118,12 @@ void Asserv::setPwm(double xPwm, double yPwm) proto_.send('w',"ww" ,xp, yp); } +/// Informe l'AVR d'arréter de dire "j'ai fini" +void Asserv::finishAck(void) +{ + proto_.send('F', "b", 0); +} + /// Statistiques void Asserv::statCounter(double period) { @@ -234,6 +239,11 @@ void Asserv::setKd(double Kd) proto_.send('p',"bw", 'd', static_cast(Kd * 256)); } +void Asserv::setSpeedIntMax(int16_t maxInt) +{ + proto_.send('p', "bW", 'a', maxInt); +} + void Asserv::setAccel(double accel) { // Conversion mmps2ppperiod @@ -253,13 +263,85 @@ void Asserv::setMaxSpeed(double maxSpeed) /// implémentation du proto::Receiver void Asserv::receive(char command, const Proto::Frame &frame) { - // XXX Faire des truc ici + switch(command) + { + case 'C': //Compteurs des moteurs (uint16) + int lMotor; + int rMotor; + proto_.decode(frame, "ww", lMotor, rMotor); + receiver_.receiveCounter(lMotor, rMotor); + break; + case 'X': + int xPos; + proto_.decode(frame, "D", xPos); + // Convertion Pas->mm + xPos *= static_cast(mmPpas_); + receiver_.receivePosX(xPos); + break; + case 'Y': + int yPos; + proto_.decode(frame, "D", yPos); + // Convertion Pas->mm + yPos *= static_cast(mmPpas_); + receiver_.receivePosY(yPos); + break; + case 'A': + int aPos; + proto_.decode(frame, "D", aPos); + aPos = static_cast(aPos * (2 * M_PI)); + aPos /= (1 << 24); + receiver_.receivePosA(aPos); + break; + case 'S': + int oldLSpeed, LSpeed, oldRSpeed, RSpeed; + proto_.decode(frame, "WWWW", oldLSpeed, LSpeed, oldRSpeed, RSpeed); + receiver_.receiveMotor(oldLSpeed, LSpeed, oldRSpeed, RSpeed); + break; + case 'W': + int leftPwm, rightPwm; + proto_.decode(frame, "WW", leftPwm, rightPwm); + receiver_.receivePwm(leftPwm / pwmMax_, rightPwm / pwmMax_); + break; + case 'T': + { + int motorTimer5 = static_cast(frame.args[0]); + int motorTimer4 = static_cast(frame.args[1]); + int motorTimer3 = static_cast(frame.args[2]); + int motorTimer2 = static_cast(frame.args[3]); + int motorTimer1 = static_cast(frame.args[4]); + int motorTimer0 = static_cast(frame.args[5]); + receiver_.receiveTiming(motorTimer5 * sPperiod_, + motorTimer4 * sPperiod_, + motorTimer3 * sPperiod_, + motorTimer2 * sPperiod_, + motorTimer1 * sPperiod_, + motorTimer0 * sPperiod_); + break; + } + case 'P': + { + int statePIn; + proto_.decode(frame, "b", statePIn); + receiver_.receiveInPort(statePIn); + break; + } + case 'F': + receiver_.jaifini(); + break; + } } /// Charge les données de la classe config -void Asserv::loadConfig(void) -{ - // XXX Faire des truc ici +void Asserv::loadConfig(const Config & config) +{ + footing_ = config.get("asserv.footing"); + epsilon_ = config.get("asserv.epsilon"); + accel_ = config.get("asserv.accel"); + kp_ = config.get("asserv.kp"); + ki_ = config.get("asserv.ki"); + kd_ = config.get("asserv.kd"); + maxSpeed_ = config.get("asserv.maxSpeed"); + pwmMax_ = config.get("asserv.maxPwm"); } int Asserv::mm2pasD(double dist) -- cgit v1.2.3