Kiwi Field Oriented Driving Programming

[GeeTwo]

Using Jared's code, and assuming Ether's clarifications are correct, j_y is positive "forward", and a counterclockwise-positive gyroscope:

Originally Posted by Jared, with j_j corrected

Again, j_x, j_y, and j_z all represent joystick values, and wheel 1, wheel 2, and wheel 3 are arranged like in Ether's Kiwi Omniwheel Inverse Kinematics paper you linked.

x = j_x * cos(theta) + j_y*sin(theta)
y = -j_x * sin(theta) + j_y * cos(theta)
z = j_z
wheel1 = x + z
wheel2 = -x/2 + 0.866y + z
wheel3 = -x/2 - 0.866y +z

Originally Posted by Ether

Jared:

Is j_y positive or negative for joystick pushed forward?

Is the code above for a gyro whose angle increases in the clockwise or counterclockwise direction?

I infer "y" and "x" refer to "forward" and "strafe right", respectively, where "forward" is as shown in this sketch, correct?

I don't see how sensor drift can produce the symptoms observed. Assuming that the various j_* and theta inputs vary continuously, the outputs should also vary continuously -- if a motor reverses direction , it wouldn't "flip", but would ramp down to zero and gradually reverse on the other side of the threshold.

Originally Posted by Nerdatope

I still can't quite get the robot to perform correctly. I've manipulated the x and y values the way you both describe, but it has not worked. I've tried everything from changing the angle from positive to negative, phase shifting the angle, swapping the sin and cos, etc.

Here are my symptoms:
What happens at best is the robot will immediately deviate from the direction its told. There appear to be angles at which the drive system will flip, where if the robot is just past the threshold the motors spin one way, but right after the motors spin the opposite way, creating a jittering motion. As time goes on, the joystick values become meaningless as the robot goes a completely irrelevant way in respect to the direction I'm wanting the robot to go.

I thank you for your time spent on this and will post if I figure anything else out.

I suggest printing out the inputs and intermediate outputs to identify the source(s) of the discontinuity that causes "jitter". The most likely things that occur to me are a discontinuity in theta (e.g. if you're reading the speed rather than angle or have a discontinuity at the 0 - 2pi wrap point), special cases in the code not reflected in the pseudocode above, and something deriving from possible overloading of the motor inputs - there are a number of conditions in which numbers with a magnitude greater than 1.0 will be sent to the motors. If it's the last, there are several ways to correct it, the simplest being to divide by the largest absolute value of the motor speeds:

biggest = max(abs(wheel1),abs(wheel2),abs(wheel3))
if (biggest > 1.0) {
   wheel1 = wheel1 / biggest
   wheel2 = wheel2 / biggest
   wheel3 = wheel3 / biggest
}

[Jared]

The strange behavior described could be because of an issue with degrees and radians or possibly because the code is looking at angular rate instead of the angle.

We might be able to help more if you post a video of what's happening.

[GeeTwo]

An even cleaner way to limit speeds:

biggest = max(1.0, abs(wheel1), abs(wheel2) ,abs(wheel3))
wheel1 = wheel1 / biggest
wheel2 = wheel2 / biggest
wheel3 = wheel3 / biggest

[/quote]
If you want to make the controls less sensitive at small deflections of the joysticks, you can change 1.0 in the first line for 1.5 or 2.0 or even as high as 2.414 (1 + sqrt(2)), which appears to be the highest number that can come out of the previous transformations.

And yes, using degrees and radians incorrectly could cause discontinuities, esp. at the transition through 0.

[teku14]

Doesn't this procedure make it so that even the slightest input will give full throttle? I mean sure it scales it appropriately now but What if you only wanted the wheels to run at say 0.1, 0.05, 0.05 ? Now this scales it to 1, 0.5, 0.5

[Jared]

Originally Posted by teku14

Doesn't this procedure make it so that even the slightest input will give full throttle? I mean sure it scales it appropriately now but What if you only wanted the wheels to run at say 0.1, 0.05, 0.05 ? Now this scales it to 1, 0.5, 0.5

It won't scale 0.1, .05, .05 at all. If you run GeeTwo's scaling function with these three numbers, biggest comes out equal to 1.0, and you divide all three wheel speeds by 1. The scaling will only happen if one wheel is over 1 (or under -1).

GeeTwo's code:

biggest = max(1.0, abs(wheel1), abs(wheel2) ,abs(wheel3))
wheel1 = wheel1 / biggest
wheel2 = wheel2 / biggest
wheel3 = wheel3 / biggest