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);