Kiwi Field Oriented Driving Programming

[Nerdatope]

After reading through the posts on this site regarding holonomic drive systems and how to program them, I decided to build the three-wheeled system known as the "Kiwi" drive system. The posts I read and the drive system reverse kinematics can be seen via the link below:

http://www.chiefdelphi.com/media/papers/2390


So far I've had success getting my robot to move and rotate, but what I need help on is getting the system to be "field-centric", where no matter what the orientation of the robot is, a forward command will result in the robot moving in the same direction relative to the field. Does anyone know how to make the system field oriented with inputs from a gyroscope?

Thanks so much in advanced and I hope this first post of mine isn't too abnormal!

[Ether]

Originally Posted by Nerdatope

Does anyone know how to make the system field oriented with inputs from a gyroscope?

Page 2 of this document.

[Nerdatope]

Re:"Page 2 of this document."

Thanks for the reply, but I have read that. That psuedo-code is for a four wheel system with each wheel being 90 degrees apart. My system has three wheels that are 120 degrees apart. I'm sure there is some simple modification to what was presented, I just can't figure it out.

[Ether]

Originally Posted by Nerdatope

That psuedo-code is for a four wheel system with each wheel being 90 degrees apart.

The field-centric equations on that page are the same for any 3-degree-of-freedom drivetrain, be it a 4-wheel omni or a 3-wheel omni or even a swerve or a mecanum.

[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.

[billbo911]

Originally Posted by Nerdatope

....
Here are my symptoms:
What happens at best is the robot will immediately deviate from the direction its told..... 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.
.....

It sounds like the gyro, "Theta", is drifting. This is common with gyro's and is usually compensated for in the initialization process.

What does your initialization process consist of?
Are you moving the robot during initialization?
Have you created a process to "re-zero" the gyro?

[L1TH1UM]

Very helpful thread, thank you. However I have one question, in: "y = -j_x * sin(theta) + j_j * cos(theta)" what is "j_j" meant to represent?

[Jared]

Originally Posted by L1TH1UM

Very helpful thread, thank you. However I have one question, in: "y = -j_x * sin(theta) + j_j * cos(theta)" what is "j_j" meant to represent?

That should be j_y.

[L1TH1UM]

Originally Posted by Jared

That should be j_y.

Ah I see, thank you.

[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.

[Jared]

I also don't know which direction the gyro will consider to be a positive angle, so if the following doesn't work, try putting a negative sign in front of the gyro angle. I'm assuming that a counterclockwise rotation as viewed from above will be considered a positive angle because that's how the gyro on my desk in front of me works.

The easiest way to accomplish this would be to reuse the code you already have that calculates your wheel speeds when you give it x, y, and rotation. Currently, you have the joystick axes mapped directly to the x, y, and rotation inputs on the kiwi drive code. To achieve field centric control, you need to modify the x and y values. The rotation part doesn't need to be changed. j_x and j_y will represent the x and y values from the joystick, and theta will be the angle reported from the gyroscope, with an angle of zero representing the robot facing forward. x and y represent the values you pass to your code that calculates the wheel speeds that you already have.

x = j_x * cos(theta) + j_y*sin(theta)
y = -j_x * sin(theta) + j_j * cos(theta)


The whole thing put together should look like this. 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_j * cos(theta)
z = j_z
wheel1 = x + z
wheel2 = -x/2 + 0.866y + z
wheel3 = -x/2 - 0.866y +z

You can figure out where these equations come from if you draw triangles between your desired direction of travel, your field coordinate system, and your robot coordinate system.

[Ether]

Originally Posted by Jared

x = j_x * cos(theta) + j_y*sin(theta)
y = -j_x * sin(theta) + j_jy * cos(theta)

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?