Re: [Playerstage-users] SET SPEED AND GOTO PART 3????? (THANKS IN ADVANCE)

[Brian G. <br...@ge...>]

On Oct 6, 2006, at 12:11 PM, Shawn Anderson wrote:

>
> 2. What is Simple P-control on the current angular error, exactly??

P control = proportional control:

http://en.wikipedia.org/wiki/Proportional_control

Below is the outline of controlling a robot in order to achieve a  
translational velocity 'vx_desired' and a rotational position  
'pa_desired'.  Hope it helps.

	brian.

// How far away from the desired angle (in radians) we have to be in  
order to make a correction (minimizes oscillations)
#define DEADZONE_THRESHOLD DTOR(5.0)
// Gain that will be multiplied by the error (in radians) to compute  
the new velocity (in radians / second)
#define PROPORTIONAL_GAIN 1.0

// computes the signed minimum difference between the two angles.
double
angle_diff(double a, double b)
{
   double d1, d2;
   a = NORMALIZE(a);
   b = NORMALIZE(b);
   d1 = a-b;
   d2 = 2*M_PI - fabs(d1);
   if(d1 > 0)
     d2 *= -1.0;
   if(fabs(d1) < fabs(d2))
     return(d1);
   else
     return(d2);
}

double vx = vx_desired;
double va;

double pa_current = position2d_proxy.GetYaw();
double pa_error = angle_diff(pa_desired, pa_current);
if(fabs(pa_error) < DEADZONE_THRESHOLD)
   va = 0.0;
else
   va = PROPORTIONAL_GAIN * pa_error;

position2d_proxy.SetSpeed(vx,va);