RC SERVO ROBOT ARM: Weekend project

This is our weekend project: a simple RC servo robot arm. 

Host controller is Mac using Python scripting, while servo controller is a spare Fubarino SD.

I tried with MSP430 Launchpad, but Serial + 3 servos was over the capability of this little Mac-hated board. Fubarino is a cherry plenty of power, and the ported SoftwareServo library from ChipKit is stable and effective.
Fubarino accepts directly joint angle in ms. Python script uses a simple Inverse Kinematics (Updated versione: math credits to COŞKUN YETİM):

def ik(self, position):
#ARM LENGHTS
l1 = self.arm1
l2 = self.arm2

#END EFFECTOR POSITION IN RESPECT OF ORIGIN NODE
x = position.x - self.home_position.x
y = position.y - self.home_position.y

#TO LIMIT END POSITION WITHIN ARM REACHABLE CIRCLE
dist = min(l1+l2, sqrt(x*x + y*y))
dub = sqrt(x*x + y*y) / dist
x /= dub
y /= dub

#SIMPLE, NO?
thetar = acos(x/dist)
theta1 = acos((l1*l1 + x*x + y*y - l2*l2) / (2*l1*dist)) + thetar
theta2 = pi - acos((l1*l1 + l2*l2 - (x*x + y*y))/(2*l1*l2))

self.set(180-degrees(theta1), 180-degrees(theta2))

Graphic is provided by a simple Svg to path script. The library used (svg.path) can provide point coordinates for Line, Arc, CubicBezier and QuadraticBezier without pain. All graphic may be created within Inkscape.


Overall code may be downloaded at this link.