Is it possible to do Inverse Kinematics using BS2?

koh84

HI, hope someone can help me here..
I am trying to use BS2 to calculate the Inverse Kinematics of SG5-UT Robotic ARM

The equation that i m calculating is as follow,
Px & Py & teta4 are changing variable.
L1,L2,L3 are constant.

A = Px - (L3)*(cosine(teta4))
B = Py - (L3) *(sine(teta4))

cosine(teta2) = [noparse][[/noparse](A*A)+(B*B)-(L1*L1)-(L2*L2) ]/(2 * L1* L2)

I have manage to calculate "cosine(teta2)", but the problem is, how can I calculate the
arc cosine in order the get the angle of teta2???

The following shows the code that i have written so far,

''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
teta2 VAR Word
teta3 VAR Word
Px VAR Word
Py VAR Word
degr VAR Word
brads VAR Byte

degr = 135
brads = degr * 128 / 180

Px = 5
Py = 5
A = (Px*100)+(L3*(COS(brads)*(-100)/127))
B = (-1)*((Py*100)-(L3*(SIN(brads)*100/127))) 'should be negative
A = A/100
B = B/100
DEBUG ? A
DEBUG ? B

teta2 = (-100*((A*A) + (B*B) - (L2*L2) - (L3*L3))/(2*L1*L2))
DEBUG ? teta2
''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

anyway, is there any alternative way to calculate the inverse kinematics
for the robotic arm? such as by using MATLAB and interface it with BS2, is it possible?

Chris Savage

Hello,

·· This subject has come up before.· You could check to see if one of the following links has any information for you.· Take care.

http://forums.parallax.com/showthread.php?p=567921

http://forums.parallax.com/showthread.php?p=594804

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Chris Savage
Parallax Tech Support

Kevin Wood

You can get an instant math upgrade for $15 - $20 by getting the uM floating point coprocessor:

www.parallax.com/detail.asp?product_id=604-00030
www.parallax.com/detail.asp?product_id=604-00050

koh84

Thank you for the help guys.
Anyway, there is something that i don't understand about the uM-FPV2 Chip, as seen on the schematics, it requires
a clock pin and a data pin which are connected to the io pin 15 and 14 respectively.

The default setting for these pins are:

FpuClock PIN 15

FpuData PIN 14

But, do I need to initialize anything inside my BS2 code in order to use this chip? Especially for the io pin 15 and 14?
Besides, it says that we can interface it to BS2 using either SPI and I2C interfaces. Well, how do I know which interface
should i choose? and what is the main different between them?

Hope you guys can help me with this... thanks..
Your help is much appreciated.

Kevin Wood

I haven't used the coprocessor, but have read about it in some Stamp articles.

There are instructions on the pages for the above links, and you can download an article from the Nuts & Volts downloads. The article is #106.

Tracy Allen

The Stamp has an arctangent function,
y ATN x

If you were able to calculate the x=cosine(theta1), and you need to find theta2 = arccosine(x), well, you can use a little geometry to find that
tangent(theta1) = sqr(1-x^2) / x. That is standard math, not Stamp math. In stamp math using brads for angle, and +/-128 to represent +/- 1, the unity factor under the square root is 128^2= 16384, instead of 1. So
theta2 = SQR (16384 - (x*x)) ATN x.

Now, whether or not that can be used for kinematics is another thing. The resolution and roundoff error limits the accuracy, especially if this involves a sequence of calculations or integration.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tracy Allen
www.emesystems.com