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