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);
}
public abstract WheelCommands GetCommands(State2D_Difference error);