scara.py

# untested port from https://reprap.org/forum/read.php?185,618696

SERVOFAKTORLEFT = 650
SERVOFAKTORRIGHT = 650

SERVOLEFTNULL = 2250
SERVORIGHTNULL = 920

L1=35  #both lower arms
L2=55.1  #left upper arm to pen
L3=13.2  #pen to left upper arm joint
L4=45  #reight upper arm

# origin points of left and right servo 
O1X=24 #22
O1Y=-25
O2X=49 #47
O2Y=-25
import math

def return_angle(a, b, c):
	angle = (a * a + c * c - b * b) / (2.0 * a * c)


	return math.acos(max(min(angle, 1), -1))

def set_xy(target_x, target_y):
  # calculate triangle between pen, servoLeft and arm joint
  # cartesian dx/dy
  dx = target_x - O1X;
  dy = target_y - O1Y;

  # polar lemgth (c) and angle (a1)
  c = math.sqrt(dx * dx + dy * dy); # 
  a1 = math.atan2(dy, dx); #
  a2 = return_angle(L1, L2, c);

  servo_1 = math.floor(((a2 + a1 - math.pi) * SERVOFAKTORLEFT) + SERVOLEFTNULL)


  # Calcutlating the right servo angle

  # calculate joint arm point for triangle of the right servo arm (joint near the pen)
  a2 = return_angle(L2, L1, c);
  Hx = target_x + L3 * math.cos((a1 - a2 + 0.621) + math.pi); #36,5°
  Hy = target_y + L3 * math.sin((a1 - a2 + 0.621) + math.pi);

  # calculate triangle between pen joint, servoRight and arm joint
  dx = Hx - O2X;
  dy = Hy - O2Y;

  c = math.sqrt(dx * dx + dy * dy);
  a1 = math.atan2(dy, dx);
  a2 = return_angle(L1, L4, c);

  servo_2 = math.floor(((a1 - a2) * SERVOFAKTORRIGHT) + SERVORIGHTNULL)

  return (servo_1, servo_2)

for i in range(0, 100):
	print(set_xy(i, 0))