When you use the driver's reference frame you end up with a situation where the joystick limits the velocity components in certain situations. The easiest example is when the robot is rotated 45 degrees counter-clockwise and you push the stick straight up. They joystick is only letting you apply +127 as the magnitude of the velocity vector (square root of Vx^2 + Vy^2). At this orientation the robot would normally accept a velocity with magnitude up to 180 (Vx = 127 and Vy = 127). This means that the motors will max out at 70% of their full speed in certain orientations. You can either accept it or try to figure out a way to correct it.
Here is one solution I came up with. First you need to figure out the magnitude of the input vector and the angle formed by Vx and Vy (theta). Now you figure out what the maximum input vector magnitude is at angle theta and use it to calculate the percentage of the current input vector. This will give you a scaling factor, s.
theta = atand(joy_x, joy_y);
v_mag = sqrt(joy_x^2 + joy_y^2);
s = v_mag/MaxMagnitude(theta);
Now figure out what the input vector would be assuming the robot hadn't rotated. The angle will be beta = theta - phi. Now figure out what the maximum magnitude of the input vector is at angle beta, call it h. The value of h is now the maximum vector magnitude at the robot's home orientation.
beta = theta - phi;
if (beta < 0) beta += 360;
h = MaxMagnitude(beta) * s;
Your scaled input vector magnitude is now h * s. This is the hypotenuse of the triangle formed by Vx and Vy. Your new X and Y will be:
scaled_x = h * cos(theta)
scaled_y = h * sin(theta)
Use these scaled values as the inputs for Vx and Vy and the motor outputs should map for all robot orientations. The MaxMagnitude() function will be something like this VB function I used for testing (angle is in radians, not degrees):
Code:
Private Function vMax(angle As Double)
Dim a As Double
'find the equivalent angle in quadrant 1
If (angle <= (PI / 2)) Then
a = angle
ElseIf (angle <= PI) Then
a = PI - angle
ElseIf (angle <= (3 * PI / 2)) Then
a = angle - PI
Else
a = (2 * PI) - angle
End If
'find the max magnitude where x and y are in the
'range [0,127]. for angles less than 45 degrees
'the x component is 127 and for angles larger than
'45 degrees the y component is 127
If a <= PI / 4 Then
vMax = Round(127 / Cos(a), 0)
Else
vMax = Round(127 / Sin(a), 0)
End If
End Function
Attached is a VB simulator that shows the motor outputs based on the information I've posted in this thread.