public WheelCommands GetWheelCommands(double r, double d)
{
// Equations are as follows:
// r = Wheel radius, meters
// d = Wheel seperation, meters
// Wr = Right wheel angular velocity, radians/sec
// Wl = Left wheel angular velocity, radians/sec
// V = Forward velocity, meters/sec
// W = Angular velocity, radians/sec
//
// V = (r/2)(Wr + Wl) = Forward velocity, meters/sec
// W = (r/d)(Wr - Wl) = Angular velocity, radians/sec
//
// Inverting matrix equation:
// Wr = (1/r)V + (d/r)W
// Wl = (1/r)V - (d/r)W
WheelCommands commands = new WheelCommands();
if ((r > 0.0) && (d > 0.0))
{
commands.OmegaR = (1 / r) * V + (d / r) * W;
commands.OmegaL = (1 / r) * V - (d / r) * W;
}
return(commands);
}
public override WheelCommands GetCommands(State2D_Difference error)
{
// Equations for body frame commands are as follows:
// V = Kp*rho*cos(alpha)
// W = Kp*sin(alpha)*cos(alpha) + Ka*alpha
double sinAlpha = Math.Sin(error.Alpha);
double cosAlpha = Math.Cos(error.Alpha);
BodyFrameCommands bodyCommands = new BodyFrameCommands(
Kp * error.Rho * cosAlpha,
Kp * sinAlpha * cosAlpha + Ka * error.Alpha);
WheelCommands wheelCommands = bodyCommands.GetWheelCommands(_r, _d);
return(wheelCommands);
}
