Robotic Arm Simulation - QBasic Source Code

A simple little program to simulate a robotic arm that can pick up and drop circles. The challenge for this one was the multiple joints, each depending on the angle and position of the joint it connects to. Well, I wouldn't consider that much of a challenge, anymore, but I wrote this when I was still pretty new to geometry.

Source Code:

'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: