Help with math for mixing function, trig?

bulkhead · 2006-10-22 05:28

I'm trying to control my robot with tank style drive (2 servo motors, like the BOE) with a single joystick. I need to convert the joystick's X and Y values (roughly 0-255) into values for the servos. The method I have come up with as follows (or skip down to see the math trouble I'm having):

IF joystick is on LEFT half
Right servo = squareRoot(X^2 + Y^2 ) //magnitude
Left servo = Right servo * ( cos(arcTan(X/Y))
IF joystick is on RIGHT half
do opposite

This is quite complicated for just mixing the two axis values into two servo values. If anyone can suggest a simpler way that works or works nearly the same (I'm not looking for 128 speed resolution or anything, maybe 10 or so) I'll use that instead. If not, I'll just stick with what I have

My question is, which class do I use for the trig calculations, cordic, IntegerMath, or Trig.java? As I said the resolution isn't that important, +/- 5 degrees is ok. What I need is an execution time of less than 100 ms (if that isn't too much to ask for, I'd like to keep my main loop < 200 ms), but most importantly, memory efficiency (I need my robot to run for at least 10 minutes before getting "out of memory error").

Also, how do I take the square root of something? It lets me do 25^2 but not 25 ^ .5. Is there anyway to efficiently approximate it? I was thinking of using a derivative linearization approximation, but you'd still need to iterate through until you reached a root near that one, and that would take quite some time and memory.

My biggest concern is that all these calculations will use so much memory since they are being done several times a second that my robot will be out of memory and "lock up" after a few minutes of use, so if there is a more efficient way that sacrifices some precision, that's fine with me. Thanks.

Peter Verkaik · 2006-10-22 08:15

Firstly,
Using your calculations:
Right servo = sqrt(X^2 + Y^2 )
cos(arcTan(X/Y) = Y/sqrt(X^2 + Y^2)
Left servo = Right servo * ( cos(arcTan(X/Y))·= Y

What exactly do you want?
If you could specify a few·joystick positions and
robot movements that would give more insight.

regards peter
·

bulkhead · 2006-10-22 17:18

Hmm, I guess left servo is just Y and right servo is the magnitude of (X,Y). Ok, let me see if I can come up with a few examples.

Joystick full UP: right servo full speed, left servo full speed
Joystick full LEFT: right servo full speed, left servo stopped
Joystick full LEFT UP: right servo half speed, left servo a fraction of full speed
Joystick full DOWN: right servo full reverse, left servo full reverse
Joystick full LEFT DOWN: left servo half reverse, right servo full reverse

Looking at this there is a contradiction when the joystick approaches full LEFT: approaching from upwards the right servo is going forwards, approaching from below it is going in reverse.

I am still not sure exactly how this is best done. I just want to be able to easily control and maneuver my robot with a single joystick.

Peter Verkaik · 2006-10-22 17:31

I drew up a diagram to visualize it.
To resolve the contradiction I swapped quadrants 2 and 3,
so when the joystick is left from center, then you get a left rotation
when the joystick is right from center you get a right rotation.
The red arrows are the 6 basic movements.

The question is what to assign to left and right when at any of
the remaining (black) positions.

regards peter

Post Edited (Peter Verkaik) : 10/22/2006 5:50:02 PM GMT

bulkhead · 2006-10-22 17:49

Yeah that's exactly what I'm looking for. I guess I could just assign LEFT to "spin left in place" and RIGHT to "spin right in place" and just have a dead point where it transitions between LEFT and LEFT +partial UP/DOWN.

I'll try to work it out and see how it goes. It looks like I don't need trig, but how do I do square root?

Peter Verkaik · 2006-10-22 17:54

I updated the diagram.
When X=0 then LEFT=RIGHT or LEFT-RIGHT=0, the Y is then the speed forward or reverse.
However, now there is·a contradiction going from quadrant2 to quadrant3
(RIGHT=+MAX changes to RIGHT=-MAX, but only when·x=0) but that is
easy to handle.

regards peter

Peter Verkaik · 2006-10-22 19:03

Here is an updated diagram that should resolve any contradiction.
I think you need to setup calculations based on quadrant.

regards peter

Beau Schwabe · 2006-10-22 20:36

Here is a visualization program I put together for a 2-wheel robot that would work for a tank as well. The inputs are meant to be from a RC-Receiver module getting X and Y joystick values
from the RC-Transmitter ranging from 1000uS to 2000uS. The output is in the form of PWM signals (0% to 100%) that should drive Left and Right motors for a 2-wheel robot or a tank.

The only 'real' calculations are determining distance (DX) which needs to figure out square root. Everything else is basic add,subtract,multiply,divide.

Here is an interesting page that shows how to work out square root that is analogous to how you would do division longhand.
www.nist.gov/dads/HTML/squareRoot.html

TANK.QB    - QuickBasic4.5 Program ( ASCII file - Use Mouse instead of Joystick )
TANK.EX_   - QuickBasic4.5 Compiled EXE program ( Change extension from EX_ to EXE <-- Run in Windows95 compatibility mode )
SCREEN.JPG - Couple of Program Screen shots.

Edit·- Escape Key EXIT's the program.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe

IC Layout Engineer
Parallax, Inc.

Post Edited (Beau Schwabe (Parallax)) : 10/22/2006 10:24:17 PM GMT

Peter Verkaik · 2006-10-22 20:56

Here is my final diagram showing your 'dead' zones as spin zones.
I showed the signs for left and right inside the areas.
The left and right directions when moving backwards are
for when you look into the backward direction.

regards peter

bulkhead · 2006-10-22 21:22

Thanks, Peter, for the diagram. I have it programmed right now with the dead zones for spinning in place and it seems to drive as expected. There is a weird spot then transitioning from rotating in place to driving forwards, when one servo will switch directions instantaneously. I'm going to try to smooth out this mixing method, if it still doen't work perhaps I'll try something else.

Beau, what software do you use to open those files? I couldn't open them with what I have.

I found an already existing square root function in the UnsignedIntMath.java class and it worked well for me. My problem now is I'd like to take the cube root of the speed values I currently have to make the control more linear, since the servo speed is the cube of its signal. I looked at that link that shows the algorithm for calculating square and cube, but its pretty complicated (for cube roots). Is there an already existing class that can calculate cube roots?

Jon Keinath · 2006-10-22 21:37

the TANK.QB will open with notepad, and rename TANK.EX_ to TANK.EXE

-Jon

Peter Verkaik · 2006-10-23 05:43

Here is a small routine for calculating left and right
(abs value determines speed, sign determines direction)
Regarding instant direction change, this could be the result
of moving the joystick to fast in opposite direction.
The solution is to detect·this direction change
(compare new left and right to previous left and right)
and if there is a sign change, insert a stop before
using the new direction.
The routine applies to my last diagram.

regards peter

Help with math for mixing function, trig?

Comments