2-Segment Arm Control

[Orborde]

I expect a lot of people to be using an arm of some sort this year, and on our team, at least, controlling the thing looks to be interesting, to say the least. We're using a 2-segment arm this year, assuming we don't run into major problems involving leverage, and this idea popped into my head for controlling the thing.

Given some target coordinates (Xt, Yt), relative to the base of the arm, some simple measurements, and two motors (overpowered servos, really) that can be set to any angle, this algorithm should move the end of the arm to where it is needed quickly and easily.

The target coordinates (Xt, Yt) could be set by a joystick or two that moves the target point around at a constant rate.

Pseudocode:

Set the target coordinates (Xt, Yt)


(see diagram)
A = Length of first arm segment
B = Length of second arm segment
C = sqrt(Xt ^ 2 + Yt ^ 2)
D = sqrt((Xt - Xb) ^ 2 + (Yt - Yb) ^ 2)
E = sqrt(Xb ^ 2 + Yb ^ 2) <- See below

Using the Law of Cosines...

b = invcos((A ^ 2 + C ^ 2 - B ^ 2) / (2 * A * C))
c = invcos((A ^ 2 + B ^ 2 - C ^ 2) / (2 * A * B))
d = invcos((C ^ 2 + E ^ 2 - D ^ 2) / (2 * C * E))

Finally, set the motor angles with your algorithm of choice.

Motor 1 Angle = b + d
Motor 2 Angle = c

Notes:
E is defined as it is in order to ensure consistency between it and Xt,Yt. It may be easier to simply measure it; I'm not sure. This may be an optimization that can be done without any problem; I was queasy about it. On second thought, it should be fairly easy to obtain the (Xb,Yb) coordinates since, as laid out, they're right up against right-angle edges of the frame.

Probably the most irritating part of this from a programming standpoint is getting useful math functions. Anyone know where I could find the ones I need?

Questions? Comments? Grievous errors? Feel free to lob any and all of them at me.

[Anthony Kesich]

Here is a good CORDIC math library I've found. Team 296 made it, so thank them. The only thing I added was a power function. Its a simple algorithm, but it made my code cleaner. Read the comments, for they explain the return type. Everything is integer math (Longs, I believe), so the numbers represent obscure fractions of a degree. Read the comments and you'll do fine.

-Tony K