DCE-Eurobot: robofsm/balet.cc Source File

00001 #include <math.h>
00002 #include "trgen.h"
00003 #include "balet.h"
00004 
00008 double balet(Pos &rp, const double aktPos[], double rychlosti[], const double k[])
00009 {
00010         double v, phi_e, xe, ye, omega, vl, vr, r, x, y, phi;
00011 
00012         r = 0.15;
00013 
00014         x = aktPos[0];
00015         y = aktPos[1];
00016         phi = aktPos[2];
00017 
00018         xe = cos(phi)*(rp.x-x)+sin(phi)*(rp.y-y);
00019         ye = -sin(phi)*(rp.x-x)+cos(phi)*(rp.y-y);
00020         phi_e = fmod(rp.phi - phi, 2.0*M_PI);
00021         if (phi_e > +M_PI) phi_e -= 2.0*M_PI;
00022         if (phi_e < -M_PI) phi_e += 2.0*M_PI;
00023         
00024         //ssPrintf("%5.2g %5.2g %5.0f\n", xe, ye, phi_e/M_PI*180);
00025 
00026         v = rp.v*cos(phi_e) + k[0]*xe;
00027         if (phi_e > 1e-10 && fabs(phi_e - 0.5*M_PI) > 1e-6)
00028             omega = rp.omega + k[1]*phi_e + k[2]*(sin(phi_e)/phi_e)*ye;
00029         else
00030             omega = rp.omega + k[1]*phi_e;
00031         //omega = rp.omega + k[1]*phi_e + k[2]*(cos(phi_e)*cos(phi_e))*ye;
00032 
00033         vr = v + r*omega;
00034         vl = v - r*omega;
00035 
00036         rychlosti[0] = vl;
00037         rychlosti[1] = vr;
00038 
00039         return sqrt((rp.x-x)*(rp.x-x)+(rp.y-y)*(rp.y-y));
00040 }