src/motion/balet.cc

   1 #include <math.h>
   2 #include "trgen.h"
   3 #include "balet.h"
   4
   5 #ifdef MATLAB_MEX_FILE
   6 #include "simstruc.h"
   7 #endif
   8
   9
  10 /**
  11  * Controller for reference trajectory tracking
  12  *
  13  * @param rp Requested position (and speeds)
  14  * @param mp Measured position (without speed)
  15  * @param k Controller constants
  16  * @param speed Output of the controller (forward and angular speed)
  17  *
  18  * @return distance from reference position (angle is not counted)
  19  */
  20 double balet(const Pos &rp, const Pos &mp, const struct balet_params &k, Pos &speed)
  21 {
  22     double phi_e, xe, ye, scale;
  23
  24         // Calculate errors
  25         xe =  cos(mp.phi)*(rp.x-mp.x)+sin(mp.phi)*(rp.y-mp.y); // tangent error
  26         ye = -sin(mp.phi)*(rp.x-mp.x)+cos(mp.phi)*(rp.y-mp.y); // perpendicular error
  27         phi_e = fmod(rp.phi - mp.phi, 2.0*M_PI);               // angle error
  28         if (phi_e > +M_PI) phi_e -= 2.0*M_PI;
  29         if (phi_e < -M_PI) phi_e += 2.0*M_PI;
  30
  31         //ssPrintf("%5.2g %5.2g %5.0f\n", xe, ye, phi_e/M_PI*180);
  32
  33         speed.v = rp.v*cos(phi_e) + k.p_tangent*xe;
  34         if (fabs(phi_e) > 1e-10)
  35             scale = sin(phi_e)/phi_e;
  36         else
  37             scale = 1;
  38         // To avoid positive feedback when going backwards.
  39         if (rp.v < 0) scale =-scale;
  40         speed.omega = rp.omega + k.p_angle*phi_e + k.p_perpen*scale*ye;
  41
  42 //      vr = v + r*omega;
  43 //      vl = v - r*omega;
  44
  45 //      rychlosti[0] = vl;
  46 //      rychlosti[1] = vr;
  47
  48                 // FIXME: how do you interrupt motion while angular error?
  49         return sqrt((rp.x-mp.x)*(rp.x-mp.x)+(rp.y-mp.y)*(rp.y-mp.y));
  50 }