SCREEN 12: REM 640 x 480, 16 colours

stepnum = 1
ShoulderX = 320
ShoulderY = 100
armlen1 = 190
armlen2 = 190
Armlen3 = 95
armang1 = 0
armang2 = 0
Armang3 = ATN(1) * 4 / -4
baseang = 0
jump = stepnum * (8 / (45 * 50)) * ATN(1)
basejump = stepnum * 8 * ATN(1) / 1565

main:
 xgo = 50: REM INPUT "X"; xgo
 ygo = 50: REM INPUT "Y"; ygo
 zgo = 10: REM INPUT "Z"; zgo
 GOSUB trig
 PRINT gamma, theta, phi
 PRINT -baseang + gamma, -armang1 + theta, -armang2 + phi
 PRINT -(baseang - gamma) / basejump, -(armang1 - theta) / jump, -(armang2 - phi) / jump
 INPUT p$
 FOR moving = 1 TO INT((gamma - baseang) / basejump)
 motor = 24
 IF SGN(INT(gamma - baseang)) = 1 THEN dir = 2 ELSE dir = 0
 GOSUB parport
 baseang = baseang + basejump
 NEXT moving
 GOSUB drawarm
 REM GOTO main
END

parport:
 REM communicate with arm on parallel port
 REM step arm by 1, then 0
 REM direction is 2 or 0
 REM motor select is 4,8,12,16,20,24 for motors 1-6

 ArmPort = 888
 info = dir + motor
 REM FOR t = 1 TO stepnum
 OUT ArmPort, info
 FOR q = 1 TO 5: NEXT q
 OUT ArmPort, info + 1
 FOR q = 1 TO 5: NEXT q
 OUT ArmPort, info
 FOR q = 1 TO 5: NEXT q
 REM NEXT t
 RETURN


drawarm:
 CLS
 CIRCLE (ShoulderX, 480 - ShoulderY), 10
 LINE (ShoulderX, 480 - ShoulderY)-STEP(armlen1 * COS(armang1), -armlen1 * SIN(armang1))
 CIRCLE STEP(0, 0), 10
 LINE -STEP(armlen2 * COS(armang2), -armlen2 * SIN(armang2))
 CIRCLE STEP(0, 0), 10
 LINE -STEP(Armlen3 * COS(Armang3), -Armlen3 * SIN(Armang3))
 PRINT baseang
 RETURN

trig:
 REM get to (xgo,ygo,zgo)
 a = armlen1
 b = armlen2
 rgo = SQR(xgo ^ 2 + ygo ^ 2)
 c = SQR(rgo ^ 2 + zgo ^ 2)
 gamma = ATN(xgo / ygo)
 alpha = ATN(zgo / rgo)
 plob = a ^ 2 + rgo ^ 2 + zgo ^ 2 - b ^ 2
 plib = 2 * a * SQR(rgo ^ 2 + zgo ^ 2)
 theta = ATN(SQR(plib ^ 2 - plob ^ 2) / plob) + alpha
 phi = ATN(-SQR(b ^ 2 - (c * SIN(theta - alpha)) ^ 2) / (c * SIN(theta - alpha))) + theta
 REM output theta=baseang, theta=armang1, phi=armang2
 RETURN