From ecbb51012f9f230914f51aaa3b9cdfba8f9ef89c Mon Sep 17 00:00:00 2001 From: haller Date: Thu, 21 Apr 2005 22:50:59 +0000 Subject: Correction de bug Encore des tout pitit truc à vérifier --- 2005/i/robert/src/asserv/asserv.cc | 44 ++++++++++++++++---------------------- 1 file changed, 19 insertions(+), 25 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 c90eaa5..fea281a 100644 --- a/2005/i/robert/src/asserv/asserv.cc +++ b/2005/i/robert/src/asserv/asserv.cc @@ -142,15 +142,12 @@ void Asserv::setSpeed(int xSpeed, int ySpeed) proto_.send('s',"bb", xSpeed, ySpeed); } -void Asserv::setPwm(double xPwm, double yPwm) +void Asserv::setPwm(int xPwm, int yPwm) { - // Conversion rCycl->Pwm - int xp = rCycl2Pwm(xPwm); - int yp = rCycl2Pwm(yPwm); - log_ ("setPwm") << "avConvX" << xPwm << "apConvX" << xp; - log_ ("setPwm") << "avConvY" << yPwm << "apConvY" << yp; + log_ ("setPwm") << "avConvX" << xPwm; + log_ ("setPwm") << "avConvY" << yPwm; // Envoie sur l'AVR - proto_.send('w',"ww" ,xp, yp); + proto_.send('w',"ww" ,xPwm, yPwm); } /// Informe l'AVR d'arréter de dire "j'ai fini" @@ -230,7 +227,7 @@ void Asserv::statSharp(int period) void Asserv::setXPos(double xPos) { // Conversion mm->pas - int p = mm2pas(xPos); + int p = mm2pas(xPos, true); // Envoie à l'AVR proto_.send('p',"bd", 'x', p); } @@ -238,17 +235,15 @@ void Asserv::setXPos(double xPos) void Asserv::setYPos(double yPos) { // Conversion mm->pas - int p = mm2pas(yPos); + int p = mm2pas(yPos, true); // Envoie à l'AVR proto_.send('p',"bd", 'y', p); } void Asserv::setAngle(double angle) { - // Conversion mm->pas - int a = radTo256(angle, true); // Envoie à l'AVR - proto_.send('p',"bd", 'a', a); + proto_.send('p',"bd", 'a', static_cast(angle / (2 * M_PI) * (1 << 24))); } void Asserv::setFooting(int16_t footing) @@ -303,7 +298,7 @@ void Asserv::setAccel(int accel) /// Renvoie l'accélération en unité international (mm/s²) double Asserv::getAccel(void) { - return mmPpas_ / (sPperiod_ * sPperiod_) / accel_; + return mmPpas_ / (sPperiod_ * sPperiod_) / accel_; // XXX Vérifier ca } void Asserv::setMaxSpeed(int maxSpeed) @@ -345,23 +340,22 @@ void Asserv::receive(char command, const Proto::Frame &frame) int xPos; proto_.decode(frame, "D", xPos); // Convertion Pas->mm - xPos *= static_cast(mmPpas_); - receiver_.receivePosX(xPos); + receiver_.receivePosX(xPos * mmPpas_ / 256); break; case 'Y': int yPos; proto_.decode(frame, "D", yPos); // Convertion Pas->mm - yPos *= static_cast(mmPpas_); - receiver_.receivePosY(yPos); + receiver_.receivePosY(yPos * mmPpas_ / 256); break; case 'A': - int aPos; - proto_.decode(frame, "D", aPos); - aPos = static_cast(aPos * (2 * M_PI)); - aPos /= (1 << 24); - receiver_.receivePosA(aPos); - break; + { + int aPos; + proto_.decode(frame, "D", aPos); + double aPosD = (aPos * (2 * M_PI) / (1 << 24)); + receiver_.receivePosA(aPosD); + break; + } case 'S': int oldLSpeed, LSpeed, oldRSpeed, RSpeed; proto_.decode(frame, "WWWW", oldLSpeed, LSpeed, oldRSpeed, RSpeed); @@ -370,7 +364,7 @@ void Asserv::receive(char command, const Proto::Frame &frame) case 'W': int leftPwm, rightPwm; proto_.decode(frame, "WW", leftPwm, rightPwm); - receiver_.receivePwm(leftPwm / pwmMax_, rightPwm / pwmMax_); + receiver_.receivePwm(leftPwm, rightPwm); break; case 'T': { @@ -380,7 +374,7 @@ void Asserv::receive(char command, const Proto::Frame &frame) int motorTimer1 = static_cast(frame.args[4]); int motorTimer0 = static_cast(frame.args[5]); receiver_.receiveTiming(motorTimer4, motorTimer3, motorTimer2, - motorTimer1, motorTimer0); + motorTimer1, motorTimer0); break; } case 'P': -- cgit v1.2.3