'Robotic Arm Simulation
'
'Programmed by Jeff Lewis
'Use the numeric keypad with Num Lock on to control the arm
'7 and 9 rotate the shoulder
'4 and 6 rotate the elbow
'1 and 3 open and close the hand




SCREEN 7

shoulderangle = 270
elbowangle = 90
handangle = 45
CONST pi = 3.14159265359#
DIM objectx(5)
DIM objecty(5)
grab = -1

FOR ctr = 0 TO 5
objectx(ctr) = 230
NEXT ctr

y = 50

FOR ctr = 0 TO 5
objecty(ctr) = y
y = y + 20
NEXT ctr

FOR ctr = 0 TO 5
grab(ctr) = 0
NEXT ctr

a:

angle = shoulderangle * (pi / 180)
elbowx = 160 + 40 * COS(angle)
elbowy = 100 - 40 * SIN(angle)

angle = (elbowangle + shoulderangle) * (pi / 180)
wristx = elbowx + 40 * COS(angle)
wristy = elbowy - 40 * SIN(angle)

angle = (45 + elbowangle + shoulderangle) * (pi / 180)
angle2 = (-45 + elbowangle + shoulderangle) * (pi / 180)
hand1x = wristx + 5 * COS(angle)
hand1y = wristy - 5 * SIN(angle)
hand2x = wristx + 5 * COS(angle2)
hand2y = wristy - 5 * SIN(angle2)

angle = (handangle + elbowangle + shoulderangle) * (pi / 180)
angle2 = (-handangle + elbowangle + shoulderangle) * (pi / 180)
finger1x = hand1x + 5 * COS(angle)
finger1y = hand1y - 5 * SIN(angle)
finger2x = hand2x + 5 * COS(angle2)
finger2y = hand2y - 5 * SIN(angle2)

IF closed = 1 THEN
FOR ctr = 0 TO 5
IF finger1x < objectx(ctr) + 5 AND finger1x > objectx(ctr) - 5 AND finger1y < objecty(ctr) + 5 AND finger1y > objecty(ctr) - 5 THEN
grab = ctr
END IF
NEXT ctr
END IF

IF grab > -1 THEN
objectx(grab) = finger1x
objecty(grab) = finger1y
END IF


CLS

LINE (160, 100)-(elbowx, elbowy)
LINE (elbowx, elbowy)-(wristx, wristy)
LINE (wristx, wristy)-(hand1x, hand1y)
LINE (wristx, wristy)-(hand2x, hand2y)
LINE (hand1x, hand1y)-(finger1x, finger1y)
LINE (hand2x, hand2y)-(finger2x, finger2y)

dotcolor = 4

'PSET (160, 100), dotcolor
PSET (elbowx, elbowy), dotcolor
PSET (wristx, wristy), dotcolor
PSET (hand1x, hand1y), dotcolor
PSET (hand2x, hand2y), dotcolor
'PSET (finger1x, finger1y), dotcolor
'PSET (finger2x, finger2y), dotcolor

FOR ctr = 0 TO 5
CIRCLE (objectx(ctr), objecty(ctr)), 5, ctr + 9
NEXT ctr



b:
getkey$ = INKEY$
IF getkey$ = "" THEN
GOTO b:
END IF

closed = 0

SELECT CASE getkey$
CASE "q"
GOTO quit
CASE "7"
shoulderangle = shoulderangle + 5
CASE "9"
shoulderangle = shoulderangle - 5
CASE "4"
elbowangle = elbowangle + 5
CASE "6"
elbowangle = elbowangle - 5
CASE "1"
handangle = 45
grab = -1
CASE "3"
handangle = -45
closed = 1
END SELECT




GOTO a

quit: